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