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