FreeWRL/FreeX3D  3.0.0
Matrix4.java
1 package org.web3d.x3d.sai;
2 
3 public class Matrix4 {
4  public float[][] matrix;
5  public static int SIZE = 4;
6  public Matrix4() {
7  int i;
8  int j;
9  matrix = new float[SIZE][SIZE];
10  for (i = 0; i < SIZE; i++) {
11  for (j=0; j<SIZE; j++) {
12  matrix[i][j] = 0;
13  }
14  }
15  }
16 
17  public Matrix4(float[][] init) {
18  int i;
19  int j;
20 
21  matrix = new float[SIZE][SIZE];
22 
23  for (i = 0; i < SIZE; i++) {
24  for (j = 0; j < SIZE; j++) {
25  matrix[i][j] = init[i][j];
26  }
27  }
28  }
29 
30  public Matrix4(float[] init) {
31  int i;
32  int j;
33  int count;
34 
35  if (init.length < SIZE*SIZE) {
36  throw new ArrayIndexOutOfBoundsException("Initialization array passed to Matrix3 of insufficient length");
37  }
38 
39  matrix = new float[SIZE][SIZE];
40  count = 0;
41 
42  for (i = 0; i < SIZE; i++) {
43  for (j = 0; j < SIZE; j++) {
44  matrix[i][j] = init[count];
45  count++;
46  }
47  }
48  }
49 
50  public void setIdentity() {
51  int i, j;
52 
53  for (i = 0; i < SIZE; i++) {
54  for (j = 0; j < SIZE; j++) {
55  if (i==j) {
56  matrix[i][j] = 1.0F;
57  } else {
58  matrix[i][j] = 0.0F;
59  }
60  }
61  }
62  }
63 
64  public void set(int row, int column, float value) {
65  if ((row > SIZE) || (row < 0) || (column > SIZE) || (column < 0)) {
66  throw new ArrayIndexOutOfBoundsException("Matrix 3 set passed invalid row or column value");
67  }
68 
69  matrix[row][column] = value;
70  }
71 
72  public float get(int row, int column){
73  if ((row > SIZE) || (row < 0) || (column > SIZE) || (column < 0)) {
74  throw new ArrayIndexOutOfBoundsException("Matrix 3 set passed invalid row or column value");
75  }
76 
77  return matrix[row][column];
78  }
79 
80  public void setTransform(SFVec3f translation, SFRotation rotation, SFVec3f scale, SFRotation scaleOrientation, SFVec3f centre) {
81  float[][] rot;
82  float[][] finalscale;
83  float[][] srot;
84  float[][] nsrot;
85  float[][] strans;
86  float[] axisr;
87  float[][] trans;
88  float[][] nstrans;
89  float[][] sc;
90  float[] value;
91  int i,j;
92  float length;
93  float x,y,z;
94  float c, s, t;
95 
96  this.setIdentity();
97 
98  // Change the rotation into a matrix
99  rot = new float[SIZE][SIZE];
100  srot = new float[SIZE][SIZE];
101  nsrot = new float[SIZE][SIZE];
102  trans = new float[SIZE][SIZE];
103  strans = new float[SIZE][SIZE];
104  nstrans = new float[SIZE][SIZE];
105  sc = new float[SIZE][SIZE];
106  finalscale = new float[SIZE][SIZE];
107 
108  value = new float[SIZE];
109  axisr = new float[SIZE];
110 
111  for (i = 0; i < SIZE; i++) {
112  for (j=0; j < SIZE; j++) {
113  if (i == j) {
114  rot[i][j] = 1.0F;
115  srot[i][j] = 1.0F;
116  nsrot[i][j] = 1.0F;
117  trans[i][j] = 1.0F;
118  strans[i][j] = 1.0F;
119  nstrans[i][j] = 1.0F;
120  sc[i][j] = 1.0F;
121  finalscale[i][j] = 1.0F;
122  } else {
123  trans[i][j] = 0.0F;
124  strans[i][j] = 0.0F;
125  nstrans[i][j] = 0.0F;
126  sc[i][j] = 0.0F;
127  rot[i][j] = 0.0F;
128  srot[i][j] = 0.0F;
129  nsrot[i][j] = 0.0F;
130  finalscale[i][j] = 0.0F;
131  }
132  }
133  }
134 
135  if (translation != null) {
136  try {
137  translation.getValue(value);
138  } catch (Exception e) {
139  System.out.println(e);
140  }
141 
142  trans[3][0] = value[0];
143  trans[3][1] = value[1];
144  trans[3][2] = value[2];
145  }
146 
147  if (scale != null) {
148  try {
149  scale.getValue(value);
150  } catch (Exception e) {
151  System.out.println(e);
152  }
153 
154  sc[0][0] = value[0];
155  sc[1][1] = value[1];
156  sc[2][2] = value[2];
157 
158  if (centre != null) {
159  try {
160  centre.getValue(value);
161  } catch (Exception e) {
162  System.out.println(e);
163  }
164 
165  strans[3][0] = value[0];
166  strans[3][1] = value[1];
167  strans[3][2] = value[2];
168 
169  }
170 
171  if (scaleOrientation!=null) {
172  try {
173  scaleOrientation.getValue(axisr);
174  } catch (Exception e) {
175  System.out.println(e);
176  }
177 
178  length = (float) (Math.sqrt(axisr[0]*axisr[0] + axisr[1]*axisr[1] + axisr[2]*axisr[2]));
179 
180  x = axisr[0]/length;
181  y = axisr[1]/length;
182  z = axisr[2]/length;
183 
184 
185  c = (float)(Math.cos(axisr[3]));
186  s = (float)(Math.sin(axisr[3]));
187  t = 1-c;
188 
189  srot[0][0] = t*x*x + c;
190  srot[0][1] = t*x*y - z*s;
191  srot[0][2] = t*x*z + y*s;
192  srot[1][0] = t*x*y + z*s;
193  srot[1][1] = t*y*y + c;
194  srot[1][2] = t*y*z - x*s;
195  srot[2][0] = t*x*z - y*s;
196  srot[2][1] = t*y*z + x*s;
197  srot[2][2] = t*z*z + c;
198  }
199 
200  for (i = 0; i < SIZE; i++) {
201  for (j=0; j< SIZE; j++) {
202  nsrot[i][j] *= -1.0F;
203  nstrans[i][j] *= -1.0F;
204  }
205  }
206 
207  }
208 
209 
210  if (rotation != null) {
211 
212  try {
213  rotation.getValue(axisr);
214  } catch (Exception e) {
215  System.out.println(e);
216  }
217 
218  length = (float)(Math.sqrt(axisr[0]*axisr[0] + axisr[1]*axisr[1] + axisr[2]*axisr[2]));
219 
220  x = axisr[0]/length;
221  y = axisr[1]/length;
222  z = axisr[2]/length;
223 
224 
225  c = (float)(Math.cos(axisr[3]));
226  s = (float)(Math.sin(axisr[3]));
227  t = 1-c;
228 
229  rot[0][0] = t*x*x + c;
230  rot[0][1] = t*x*y - z*s;
231  rot[0][2] = t*x*z + y*s;
232  rot[1][0] = t*x*y + z*s;
233  rot[1][1] = t*y*y + c;
234  rot[1][2] = t*y*z - x*s;
235  rot[2][0] = t*x*z - y*s;
236  rot[2][1] = t*y*z + x*s;
237  rot[2][2] = t*z*z + c;
238  }
239 
240  matrix = multiply(matrix, trans);
241  matrix = multiply(matrix, strans);
242  matrix = multiply(matrix, rot);
243  matrix = multiply(matrix, srot);
244  matrix = multiply(matrix, sc);
245  matrix = multiply(matrix, nsrot);
246  matrix = multiply(matrix, nstrans);
247  }
248 
249  public void getTransform(SFVec3f translation, SFRotation rotation, SFVec3f scale) {
250  float[] t;
251  float[] s;
252  float[] r;
253 
254  r = new float[SIZE];
255  s = new float[SIZE];
256  t = new float[SIZE];
257 
258  t[0] = matrix[0][3];
259  t[1] = matrix[1][3];
260  t[2] = matrix[2][3];
261 
262  translation.setValue(t);
263 
264  // Note that this only works if the scale transform was applied first - since that is how transforms are applied in VRML, and this is a much
265  // more efficient way of doing things we make that assumption
266 
267  s[0] = (float) (Math.sqrt(matrix[0][0]*matrix[0][0] + matrix[1][0]*matrix[1][0] + matrix[2][0]*matrix[2][0] + matrix[3][0]*matrix[3][0]));
268  s[1] = (float) (Math.sqrt(matrix[0][1]*matrix[0][1] + matrix[1][1]*matrix[1][1] + matrix[2][1]*matrix[2][1] + matrix[3][1]*matrix[3][1]));
269  s[2] = (float) (Math.sqrt(matrix[0][2]*matrix[0][2] + matrix[1][2]*matrix[1][2] + matrix[2][2]*matrix[2][2] + matrix[3][2]*matrix[3][2]));
270 
271  scale.setValue(s);
272 
273  r[3] = (float) (Math.acos((matrix[0][0] + matrix[1][1] + matrix[2][2] - 1)/2.0F));
274  r[0] = (float)((matrix[2][1]-matrix[1][2])/(Math.sqrt((matrix[2][1]-matrix[1][2])*(matrix[2][1]-matrix[1][2]) + (matrix[0][2]-matrix[2][0])*(matrix[0][2]*matrix[2][0]) + (matrix[1][0]-matrix[0][1])*(matrix[1][0]-matrix[0][1]))));
275  r[1] = (float)((matrix[0][2]-matrix[2][0])/(Math.sqrt((matrix[2][1]-matrix[1][2])*(matrix[2][1]-matrix[1][2]) + (matrix[0][2]-matrix[2][0])*(matrix[0][2]*matrix[2][0]) + (matrix[1][0]-matrix[0][1])*(matrix[1][0]-matrix[0][1]))));
276  r[2] = (float)((matrix[1][0]-matrix[0][1])/(Math.sqrt((matrix[2][1]-matrix[1][2])*(matrix[2][1]-matrix[1][2]) + (matrix[0][2]-matrix[2][0])*(matrix[0][2]*matrix[2][0]) + (matrix[1][0]-matrix[0][1])*(matrix[1][0]-matrix[0][1]))));
277 
278  rotation.setValue(r);
279  }
280 
281  public Matrix4 inverse() {
282  float[] transp;
283  float[] dst;
284  float[] tmp;
285  float[] mymatrix;
286  int i, j, count;
287  float A;
288 
289  transp = new float[16];
290  dst = new float[16];
291  tmp = new float[12];
292  mymatrix = new float[16];
293 
294  count = 0;
295 
296  A = matrix[0][0]*matrix[1][1]*matrix[2][2]*matrix[3][3] + matrix[0][0]*matrix[1][2]*matrix[2][3]*matrix[3][1] + matrix[0][0]*matrix[1][3]*matrix[2][1]*matrix[3][2];
297  A += matrix[0][1]*matrix[1][0]*matrix[2][3]*matrix[3][2] + matrix[0][1]*matrix[1][2]*matrix[2][0]*matrix[3][3] + matrix[0][1]*matrix[1][3]*matrix[2][2]*matrix[3][0];
298  A += matrix[0][2]*matrix[1][0]*matrix[2][1]*matrix[3][3] + matrix[0][2]*matrix[1][1]*matrix[2][3]*matrix[3][0] + matrix[0][2]*matrix[1][3]*matrix[2][0]*matrix[3][1];
299  A += matrix[0][3]*matrix[1][0]*matrix[2][2]*matrix[3][1] + matrix[0][3]*matrix[1][1]*matrix[2][0]*matrix[3][2] + matrix[0][3]*matrix[1][2]*matrix[2][1]*matrix[3][0];
300  A = A - matrix[0][0]*matrix[1][1]*matrix[2][3]*matrix[3][2] - matrix[0][0]*matrix[1][2]*matrix[2][1]*matrix[3][3] - matrix[0][0]*matrix[1][3]*matrix[2][2]*matrix[3][1];
301  A = A - matrix[0][1]*matrix[1][0]*matrix[2][2]*matrix[3][3] - matrix[0][1]*matrix[1][2]*matrix[2][3]*matrix[3][0] - matrix[0][1]*matrix[1][3]*matrix[2][0]*matrix[3][2];
302  A = A - matrix[0][2]*matrix[1][0]*matrix[2][3]*matrix[3][1] - matrix[0][2]*matrix[1][1]*matrix[2][0]*matrix[3][3] - matrix[0][2]*matrix[1][3]*matrix[2][1]*matrix[3][0];
303  A = A - matrix[0][3]*matrix[1][0]*matrix[2][1]*matrix[3][2] - matrix[0][3]*matrix[1][1]*matrix[2][2]*matrix[3][0] - matrix[0][3]*matrix[1][2]*matrix[2][0]*matrix[3][1];
304 
305  if (A == 0) {
306  return null;
307  } else {
308  A = 1.0F/A;
309  }
310 
311  dst[0] = matrix[1][1]*matrix[2][2]*matrix[3][3] + matrix[1][2]*matrix[2][3]*matrix[3][1] + matrix[1][3]*matrix[2][1]*matrix[3][2];
312  dst[0] = dst[0] - matrix[1][1]*matrix[2][3]*matrix[3][2] - matrix[1][2]*matrix[2][1]*matrix[3][3] - matrix[1][3]*matrix[2][2]*matrix[3][1];
313  dst[1] = matrix[0][1]*matrix[2][3]*matrix[3][2] + matrix[0][2]*matrix[2][1]*matrix[3][3] + matrix[0][3]*matrix[2][2]*matrix[3][1];
314  dst[1] = dst[1] - matrix[0][1]*matrix[2][2]*matrix[3][3] - matrix[0][2]*matrix[2][3]*matrix[3][1] - matrix[0][3]*matrix[2][1]*matrix[3][2];
315  dst[2] = matrix[0][1]*matrix[1][2]*matrix[3][3] + matrix[0][2]*matrix[1][3]*matrix[3][1] + matrix[0][3]*matrix[1][1]*matrix[3][2];
316  dst[2] = dst[2] - matrix[0][1]*matrix[1][3]*matrix[3][2] - matrix[0][2]*matrix[1][1]*matrix[3][3] - matrix[0][3]*matrix[1][2]*matrix[3][1];
317  dst[3] = matrix[0][1]*matrix[1][3]*matrix[2][2] + matrix[0][2]*matrix[1][1]*matrix[2][3] + matrix[0][3]*matrix[1][2]*matrix[2][1];
318  dst[3] = dst[3] - matrix[0][1]*matrix[1][2]*matrix[2][3] - matrix[0][2]*matrix[1][3]*matrix[2][1] - matrix[0][3]*matrix[1][1]*matrix[2][2];
319  dst[4] = matrix[1][0]*matrix[2][3]*matrix[3][2] + matrix[1][2]*matrix[2][0]*matrix[3][3] + matrix[1][3]*matrix[2][2]*matrix[3][0];
320  dst[4] = dst[4] - matrix[1][0]*matrix[2][2]*matrix[3][3] - matrix[1][2]*matrix[2][3]*matrix[3][0] - matrix[1][3]*matrix[2][0]*matrix[3][2];
321  dst[5] = matrix[0][0]*matrix[2][2]*matrix[3][3] + matrix[0][2]*matrix[2][3]*matrix[3][0] + matrix[0][3]*matrix[2][0]*matrix[3][2];
322  dst[5] = dst[5] - matrix[0][0]*matrix[2][3]*matrix[3][2] - matrix[0][2]*matrix[2][0]*matrix[3][3] - matrix[0][3]*matrix[2][2]*matrix[3][0];
323  dst[6] = matrix[0][0]*matrix[1][3]*matrix[3][2] + matrix[0][2]*matrix[1][0]*matrix[3][3] + matrix[0][3]*matrix[1][2]*matrix[3][0];
324  dst[6] = dst[6] - matrix[0][0]*matrix[1][2]*matrix[3][3] - matrix[0][2]*matrix[1][3]*matrix[3][0] - matrix[0][3]*matrix[1][0]*matrix[3][2];
325  dst[7] = matrix[0][0]*matrix[1][2]*matrix[2][3] + matrix[0][2]*matrix[1][3]*matrix[2][0] + matrix[0][3]*matrix[1][0]*matrix[2][2];
326  dst[7] = dst[7] - matrix[0][0]*matrix[1][3]*matrix[2][2] - matrix[0][2]*matrix[1][0]*matrix[2][3] - matrix[0][3]*matrix[1][2]*matrix[2][0];
327  dst[8] = matrix[1][0]*matrix[2][1]*matrix[3][3] + matrix[1][1]*matrix[2][3]*matrix[3][0] + matrix[1][3]*matrix[2][0]*matrix[3][1];
328  dst[8] = dst[8] - matrix[1][0]*matrix[2][3]*matrix[3][1] - matrix[1][1]*matrix[2][0]*matrix[3][3] - matrix[1][3]*matrix[2][1]*matrix[3][0];
329  dst[9] = matrix[0][0]*matrix[2][3]*matrix[3][1] + matrix[0][1]*matrix[2][0]*matrix[3][3] + matrix[0][3]*matrix[2][1]*matrix[3][0];
330  dst[9] = dst[9] - matrix[0][0]*matrix[2][1]*matrix[3][3] - matrix[0][1]*matrix[2][3]*matrix[3][0] - matrix[0][3]*matrix[2][0]*matrix[3][1];
331  dst[10] = matrix[0][0]*matrix[1][1]*matrix[3][3] + matrix[0][1]*matrix[1][3]*matrix[3][0] + matrix[0][3]*matrix[1][0]*matrix[3][1];
332  dst[10] = dst[10] - matrix[0][0]*matrix[1][3]*matrix[3][1] - matrix[0][1]*matrix[1][0]*matrix[3][3] - matrix[0][3]*matrix[1][1]*matrix[3][0];
333  dst[11] = matrix[0][0]*matrix[1][3]*matrix[2][1] + matrix[0][1]*matrix[1][0]*matrix[2][3] + matrix[0][3]*matrix[1][1]*matrix[2][0];
334  dst[11] = dst[11] - matrix[0][0]*matrix[1][1]*matrix[2][3] - matrix[0][1]*matrix[1][3]*matrix[2][0] - matrix[0][3]*matrix[1][0]*matrix[2][1];
335  dst[12] = matrix[1][0]*matrix[2][2]*matrix[3][1] + matrix[1][1]*matrix[2][0]*matrix[3][2] + matrix[1][2]*matrix[2][1]*matrix[3][0];
336  dst[12] = dst[12] - matrix[1][0]*matrix[2][1]*matrix[3][2] - matrix[1][1]*matrix[2][2]*matrix[3][0] - matrix[1][2]*matrix[2][0]*matrix[3][1];
337  dst[13] = matrix[0][0]*matrix[2][1]*matrix[3][2] + matrix[0][1]*matrix[2][2]*matrix[3][0] + matrix[0][2]*matrix[2][0]*matrix[3][1];
338  dst[13] = dst[13] - matrix[0][0]*matrix[2][2]*matrix[3][1] - matrix[0][1]*matrix[2][0]*matrix[3][2] - matrix[0][2]*matrix[2][1]*matrix[3][0];
339  dst[14] = matrix[0][0]*matrix[1][2]*matrix[3][1] + matrix[0][1]*matrix[1][0]*matrix[3][2] + matrix[0][2]*matrix[1][1]*matrix[3][0];
340  dst[14] = dst[14] - matrix[0][0]*matrix[1][1]*matrix[3][2] - matrix[0][1]*matrix[1][2]*matrix[3][0] - matrix[0][2]*matrix[1][0]*matrix[3][1];
341  dst[15] = matrix[0][0]*matrix[1][1]*matrix[2][2] + matrix[0][1]*matrix[1][2]*matrix[2][0] + matrix[0][2]*matrix[1][0]*matrix[2][1];
342  dst[15] = dst[15] - matrix[0][0]*matrix[1][2]*matrix[2][1] - matrix[0][1]*matrix[1][0]*matrix[2][2] - matrix[0][2]*matrix[1][1]*matrix[2][0];
343 
344 
345  for (j = 0; j < 16; j++)
346  dst[j] *= A;
347 
348  return new Matrix4(dst);
349  }
350 
351  public Matrix4 transpose() {
352  float[] transp;
353  int i, j;
354 
355  transp = new float[16];
356 
357  for (i=0; i < SIZE; i++) {
358  for (j=0; j < SIZE; j++) {
359  transp[j*SIZE + i] = matrix[i][j];
360  }
361  }
362 
363  return new Matrix4(transp);
364  }
365 
366  public Matrix4 multiplyLeft(Matrix4 mat) {
367  int i, j, k;
368  float[] ans;
369  float sum;
370  float[][] multp;
371 
372  multp = new float[SIZE][SIZE];
373 
374  for ( i = 0; i < SIZE; i++) {
375  for (j = 0; j < SIZE; j++) {
376  multp[i][j] = mat.get(i, j);
377  }
378  }
379 
380  ans = new float[SIZE*SIZE];
381 
382  for (i = 0; i < SIZE; i++) {
383  for (j=0; j < SIZE; j++) {
384  sum = 0.0F;
385  for (k = 0; k < SIZE; k++) {
386  sum = sum + multp[i][k]*matrix[k][j];
387  }
388  ans[(i*SIZE)+j] = sum;
389  }
390  }
391 
392  return new Matrix4(ans);
393  }
394 
395  public float[][] multiply(float[][] multp, float[][] mat) {
396  int i, j, k;
397  float sum;
398  float[][] ans;
399 
400  ans = new float[SIZE][SIZE];
401 
402  for (i = 0; i < SIZE; i++) {
403  for (j=0; j < SIZE; j++) {
404  sum = 0.0F;
405  for (k = 0; k < SIZE; k++) {
406  sum = sum + multp[i][k]*mat[k][j];
407  }
408  ans[i][j] = sum;
409  }
410  }
411 
412  return ans;
413  }
414 
415 
416  public Matrix4 multiplyRight(Matrix4 mat) {
417  int i, j, k;
418  float[] ans;
419  float sum;
420  float[][] multp;
421 
422  multp = new float[SIZE][SIZE];
423 
424  for ( i = 0; i < SIZE; i++) {
425  for (j = 0; j < SIZE; j++) {
426  multp[i][j] = mat.get(i, j);
427  }
428  }
429 
430  ans = new float[SIZE*SIZE];
431 
432  for (i = 0; i < SIZE; i++) {
433  for (j=0; j < SIZE; j++) {
434  sum = 0.0F;
435  for (k = 0; k < SIZE; k++) {
436  sum = sum + matrix[i][k]*multp[k][j];
437  }
438  ans[i*SIZE+j] = sum;
439  }
440  }
441 
442  return new Matrix4(ans);
443  }
444  public float[] multiplyRowVector(float[] vec) {
445  int i, j, k;
446  float[] ans;
447  float sum;
448 
449  ans = new float[SIZE];
450 
451  for (i = 0; i < SIZE; i++) {
452  sum = 0.0F;
453  for (k = 0; k < SIZE; k++) {
454  sum = sum + vec[k] * matrix[i][k];
455  }
456  ans[i] = sum;
457  }
458 
459  return ans;
460  }
461 
462  public float[] multiplyColVector(float[] vec) {
463  int i, j, k;
464  float[] ans;
465  float sum;
466 
467  ans = new float[SIZE*SIZE];
468 
469  for (i = 0; i < SIZE; i++) {
470  sum = 0.0F;
471  for (k = 0; k < SIZE; k++) {
472  sum = sum + matrix[k][i]*vec[k];
473  }
474  ans[i] = sum;
475  }
476 
477  return ans;
478  }
479 }