FreeWRL / FreeX3D 4.3.0
Matrix4.java
1package org.web3d.x3d.sai;
2
3public 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}