1
2 package hep.wired.util;
3
4 import org.jdom.DataConversionException;
5
6 import java.awt.geom.NoninvertibleTransformException;
7 import java.text.DecimalFormat;
8
9 import org.freehep.xml.io.XMLIO;
10 import org.freehep.xml.io.XMLIOManager;
11
12 /***
13 * A 3x4 matrix for projections of 3D space to 3D space.
14 *
15 * @author Mark Donszelmann
16 * @version $Id: Matrix3D.java 2083 2005-07-20 17:16:52Z duns $
17 */
18
19 public class Matrix3D implements XYZindices, UVWindices, Cloneable, XMLIO {
20
21 private static final DecimalFormat fmt = new DecimalFormat("0000.000");
22
23 private double m00, m01, m02, m03;
24 private double m10, m11, m12, m13;
25 private double m20, m21, m22, m23;
26 private boolean orthoNormalized;
27
28 /***
29 * Creates an identity matrix.
30 */
31 public Matrix3D() {
32 this(1, 0, 0,
33 0, 1, 0,
34 0, 0, 1,
35 0, 0, 0);
36 }
37
38 /***
39 * Creates a matrix with given parameters.
40 */
41 public Matrix3D(double m00, double m10, double m20,
42 double m01, double m11, double m21,
43 double m02, double m12, double m22,
44 double m03, double m13, double m23) {
45 set(m00, m10, m20,
46 m01, m11, m21,
47 m02, m12, m22,
48 m03, m13, m23);
49 }
50
51 public Object clone() {
52 return new Matrix3D(m00, m10, m20,
53 m01, m11, m21,
54 m02, m12, m22,
55 m03, m13, m23);
56 }
57
58 public int hashCode() {
59 long bits;
60 bits = Double.doubleToLongBits(m00);
61 bits = bits * 31 + Double.doubleToLongBits(m10);
62 bits = bits * 31 + Double.doubleToLongBits(m20);
63 bits = bits * 31 + Double.doubleToLongBits(m01);
64 bits = bits * 31 + Double.doubleToLongBits(m11);
65 bits = bits * 31 + Double.doubleToLongBits(m21);
66 bits = bits * 31 + Double.doubleToLongBits(m02);
67 bits = bits * 31 + Double.doubleToLongBits(m12);
68 bits = bits * 31 + Double.doubleToLongBits(m22);
69 bits = bits * 31 + Double.doubleToLongBits(m03);
70 bits = bits * 31 + Double.doubleToLongBits(m13);
71 bits = bits * 31 + Double.doubleToLongBits(m23);
72 return (((int) bits) ^ ((int) (bits >> 32)));
73 }
74
75 public boolean equals(Object obj) {
76 if (!(obj instanceof Matrix3D)) return super.equals(obj);
77
78 Matrix3D m = (Matrix3D)obj;
79
80 return (m.m00 == m00) && (m.m10 == m10) && (m.m20 == m20)
81 && (m.m01 == m01) && (m.m11 == m11) && (m.m21 == m21)
82 && (m.m02 == m02) && (m.m12 == m12) && (m.m22 == m22)
83 && (m.m03 == m03) && (m.m13 == m13) && (m.m23 == m23);
84 }
85
86 /***
87 * Returns the sx value (m00).
88 */
89 public double getScaleX() {
90 return m00;
91 }
92
93 /***
94 * Returns the sy value (m11).
95 */
96 public double getScaleY() {
97 return m11;
98 }
99
100 /***
101 * Returns the sz value (m22).
102 */
103 public double getScaleZ() {
104 return m22;
105 }
106
107 /***
108 * Returns the tx value (m03).
109 */
110 public double getTranslateX() {
111 return m03;
112 }
113
114 /***
115 * Returns the ty value (m13).
116 */
117 public double getTranslateY() {
118 return m13;
119 }
120
121 /***
122 * Returns the tz value (m23).
123 */
124 public double getTranslateZ() {
125 return m23;
126 }
127
128 /***
129 * Returns a rotate matrix to rotate theta radians over unit vector (nx, ny, nz).
130 * <pre>
131 * [cos(theta)+nx*nx*(1-cos(theta)) nx*ny*(1-cos(theta))-nz*sin(theta) nx*nz*(1-cos(theta))+ny*sin(theta) 0 ]
132 * [ny*nx*(1-cos(theta)+nz*sin(theta) cos(theta)+ny*ny*(1-cos(theta)) ny*nz*(1-cos(theta))-nz*sin(theta) 0 ]
133 * [nz*nx*(1-cos(theta)-ny*sin(theta) nz*ny*(1-cos(theta))+nx*sin(theta) cos(theta)+nz*nz*(1-cos(theta)) 0 ]
134 * [0 0 0 1 ]
135 * </pre>
136 * see "M.Pique, Matrix Techniques, Graphics Gems I [GLASSNER, 1990], pp. 465-469"
137 */
138 public static Matrix3D getRotateInstance(double theta, double nx, double ny, double nz) {
139 double cos = Math.cos(theta);
140 double sin = Math.sin(theta);
141 double one_cos = 1 - cos;
142 return new Matrix3D(
143 cos + nx*nx*one_cos, ny*nx*one_cos + nz*sin, nz*nx*one_cos - ny*sin,
144
145 nx*ny*one_cos - nz*sin, cos + ny*ny*one_cos, nz*ny*one_cos + nx*sin,
146
147 nx*nz*one_cos + ny*sin, ny*nz*one_cos - nx*sin, cos + nz*nz*one_cos,
148
149 0, 0, 0
150 );
151 }
152
153 /***
154 * Returns a rotate matrix to rotate omega over the Z-axis, theta over the X-axis, followed phi over the Y-axis.
155 */
156 public static Matrix3D getRotateInstance(double phi, double theta, double omega) {
157 Matrix3D m = new Matrix3D();
158 if (omega != 0) m.rotate(omega, 0, 0, 1);
159 if (theta != 0) m.rotate(theta, 1, 0, 0);
160 if (phi != 0) m.rotate(phi, 0, 1, 0);
161 return m;
162 }
163
164 /***
165 * Rotate phi radians over x-axis, where y to z is a positive rotation.
166 * Concatenates with matrix:
167 * <pre>
168 * [1 0 0 0]
169 * [0 cos(phi) -sin(phi) 0]
170 * [0 sin(phi) cos(phi) 0]
171 * [0 0 0 1]
172 * </pre>
173 * See "Foley, van Dam, Computer Graphics, 1990, pp.213-222"
174 */
175 public void rotateX(double phi) {
176 double cos = Math.cos(phi);
177 double sin = Math.sin(phi);
178 set(m00, m10, m20,
179 m01*cos + m02*sin, m11*cos + m12*sin, m21*cos + m22*sin,
180 -m01*sin + m02*cos, -m11*sin + m12*cos, -m21*sin + m22*cos,
181 m03, m13, m23);
182 }
183
184 /***
185 * Rotate theta radians over y-axis, where z to x is a positive rotation.
186 * Concatenates with matrix:
187 * <pre>
188 * [cos(theta) 0 sin(theta) 0]
189 * [0 1 0 0]
190 * [-sin(theta) 0 cos(theta) 0]
191 * [0 0 0 1]
192 * </pre>
193 * See "Foley, van Dam, Computer Graphics, 1990, pp.213-222"
194 */
195 public void rotateY(double theta) {
196 double cos = Math.cos(theta);
197 double sin = Math.sin(theta);
198 set(m00*cos - m02*sin, m10*cos - m12*sin, m20*cos - m22*sin,
199 m01, m11, m21,
200 m00*sin + m02*cos, m10*sin + m12*cos, m20*sin - m22*cos,
201 m03, m13, m23);
202 }
203
204 /***
205 * Rotate omega radians over z-axis, where x to y is a positive rotation.
206 * Concatenates with matrix:
207 * <pre>
208 * [cos(omega) -sin(omega) 0 0]
209 * [sin(omega) cos(omega) 0 0]
210 * [0 0 1 0]
211 * [0 0 0 1]
212 * </pre>
213 * See "Foley, van Dam, Computer Graphics, 1990, pp.213-222"
214 */
215 public void rotateZ(double omega) {
216 double cos = Math.cos(omega);
217 double sin = Math.sin(omega);
218 set(m00*cos + m01*sin, m10*cos + m11*sin, m20*cos + m21*sin,
219 -m00*sin + m01*cos, -m10*sin + m11*cos, -m20*sin + m21*cos,
220 m02, m12, m22,
221 m03, m13, m23);
222 }
223
224 /***
225 * Rotates theta radians over unit vector (nx, ny, nz). Preconcatenates with the matrix:
226 * <pre>
227 * [cos(theta)+nx*nx*(1-cos(theta)) nx*ny*(1-cos(theta))-nz*sin(theta) nx*nz*(1-cos(theta))+ny*sin(theta) 0 ]
228 * [ny*nx*(1-cos(theta)+nz*sin(theta) cos(theta)+ny*ny*(1-cos(theta)) ny*nz*(1-cos(theta))-nz*sin(theta) 0 ]
229 * [nz*nx*(1-cos(theta)-ny*sin(theta) nz*ny*(1-cos(theta))+nx*sin(theta) cos(theta)+nz*nz*(1-cos(theta)) 0 ]
230 * [0 0 0 1 ]
231 * </pre>
232 * see "M.Pique, Matrix Techniques, Graphics Gems I [GLASSNER, 1990], pp. 465-469"
233 */
234 public void rotate(double theta, double nx, double ny, double nz) {
235 preConcatenate(Matrix3D.getRotateInstance(theta, nx, ny, nz));
236 }
237
238 /***
239 * Scales by sx, sy and sz. Preconcatenates with the matrix:
240 * <pre>
241 * [ sx 0 0 0 ]
242 * [ 0 sy 0 0 ]
243 * [ 0 0 sz 0 ]
244 * [ 0 0 0 1 ]
245 * </pre>
246 */
247 public void scale(double sx, double sy, double sz) {
248 m00 *= sx; m01 *= sx; m02 *= sx; m03 *= sx;
249 m10 *= sy; m11 *= sy; m12 *= sy; m13 *= sy;
250 m20 *= sz; m21 *= sz; m22 *= sz; m23 *= sz;
251 orthoNormalized = false;
252 }
253
254 /***
255 * Shears by shx and shy. Preconcatenates with the matrix:
256 * <pre>
257 * [ 1 shx 0 0 ]
258 * [ shy 1 0 0 ]
259 * [ 0 0 1 0 ]
260 * [ 0 0 0 1 ]
261 * </pre>
262 */
263 public void shear(double shx, double shy) {
264 set(m00 + shx*m10,
265 m10 + shy*m00,
266 m20,
267
268 m01 + shx*m11,
269 m11 + shy*m01,
270 m21,
271
272 m02 + shx*m12,
273 m12 + shy*m02,
274 m22,
275
276 m03 + shx*m13,
277 m13 + shy*m03,
278 m23
279 );
280 }
281
282 /***
283 * Translates by tx, ty and tz. Preconcatenates with the matrix:
284 * <pre>
285 * [ 1 0 0 tx ]
286 * [ 0 1 0 ty ]
287 * [ 0 0 1 tz ]
288 * [ 0 0 0 1 ]
289 * </pre>
290 */
291 public void translate(double tx, double ty, double tz) {
292 m03 += tx;
293 m13 += ty;
294 m23 += tz;
295 }
296
297 /***
298 * Model-Translates by tx, ty and tz. Concatenates with the matrix:
299 * <pre>
300 * [ 1 0 0 tx ]
301 * [ 0 1 0 ty ]
302 * [ 0 0 1 tz ]
303 * [ 0 0 0 1 ]
304 * </pre>
305 */
306 public void modelTranslate(double tx, double ty, double tz) {
307
308
309
310 m03 += m00*tx+m01*ty+m02*tz;
311 m13 += m10*tx+m11*ty+m12*tz;
312 m23 += m20*tx+m21*ty+m22*tz;
313 }
314
315
316 private void set(double m00, double m10, double m20,
317 double m01, double m11, double m21,
318 double m02, double m12, double m22,
319 double m03, double m13, double m23) {
320 this.m00 = m00;
321 this.m10 = m10;
322 this.m20 = m20;
323
324 this.m01 = m01;
325 this.m11 = m11;
326 this.m21 = m21;
327
328 this.m02 = m02;
329 this.m12 = m12;
330 this.m22 = m22;
331
332 this.m03 = m03;
333 this.m13 = m13;
334 this.m23 = m23;
335
336 this.orthoNormalized = false;
337 }
338
339 /***
340 * Returns the angle and unit vector of the rotation matrix of this matrix.
341 */
342 public double getRotation(double[] n) {
343 Matrix3D m = orthoNormalized ? this : createOrthonormalized();
344 double cosTheta = (m.m00 + m.m11 + m.m22 - 1) / 2;
345 double theta = Math.acos(cosTheta);
346 double twoSinTheta = 2*Math.sin(theta);
347 if (twoSinTheta == 0) {
348 n[0] = n[1] = n[2] = 0;
349 } else {
350 n[0] = (m.m21 - m.m12) / twoSinTheta;
351 n[1] = (m.m02 - m.m20) / twoSinTheta;
352 n[2] = (m.m01 - m.m10) / twoSinTheta;
353 }
354 return theta;
355 }
356
357 /***
358 * Returns the determinant.
359 */
360 public double getDeterminant() {
361 return
362 (m00*m11 - m01*m10)*m22
363 -(m00*m12 - m02*m10)*m21
364 +(m01*m12 - m02*m11)*m20;
365 }
366
367 /***
368 * Create and return the orthonormalized rotation matrix by using the Gram-Schmidt algorithm.
369 */
370 public Matrix3D createOrthonormalized() {
371 double v1length, v2length, v3length;
372 double p1, p2;
373 Matrix3D r = new Matrix3D();
374 r.m00 = m00; r.m10 = m10; r.m20 = m20;
375
376 v1length = r.m00*r.m00 + r.m10*r.m10 + r.m20*r.m20;
377 p1 = (m01*r.m00 + m11*r.m10 + m21*r.m20)/v1length;
378 r.m01 = m01 - p1*m00;
379 r.m11 = m11 - p1*m10;
380 r.m21 = m21 - p1*m20;
381
382 v2length = r.m01*r.m01 + r.m11*r.m11 + r.m21*r.m21;
383 p1 = (m02*r.m00 + m12*r.m10 + m22*r.m20)/v1length;
384 p2 = (m02*r.m01 + m12*r.m11 + m22*r.m21)/v2length;
385 r.m02 = m02 - p1*m00 - p2*m01;
386 r.m12 = m12 - p1*m10 - p2*m11;
387 r.m22 = m22 - p1*m20 - p2*m21;
388
389 v3length = r.m02*r.m02 + r.m12*r.m12 + r.m22*r.m22;
390
391 double v1n = Math.sqrt(v1length);
392 double v2n = Math.sqrt(v2length);
393 double v3n = Math.sqrt(v3length);
394 r.m00 /= v1n; r.m10 /= v1n; r.m20 /= v1n;
395 r.m01 /= v2n; r.m11 /= v2n; r.m21 /= v2n;
396 r.m02 /= v3n; r.m12 /= v3n; r.m22 /= v3n;
397
398 return r;
399 }
400
401 /***
402 * Create and returnes a normalized matrix of this matrix.
403 */
404 public Matrix3D createNormalized() {
405 Matrix3D r = (Matrix3D)clone();
406
407 double v1n = Math.sqrt(r.m00*r.m00 + r.m10*r.m10 + r.m20*r.m20);
408 double v2n = Math.sqrt(r.m01*r.m01 + r.m11*r.m11 + r.m21*r.m21);
409 double v3n = Math.sqrt(r.m02*r.m02 + r.m12*r.m12 + r.m22*r.m22);
410 r.m00 /= v1n; r.m10 /= v1n; r.m20 /= v1n;
411 r.m01 /= v2n; r.m11 /= v2n; r.m21 /= v2n;
412 r.m02 /= v3n; r.m12 /= v3n; r.m22 /= v3n;
413
414 return r;
415 }
416
417 /***
418 * Creates and returns the inverse of this matrix.
419 */
420 public Matrix3D createInverse() throws NoninvertibleTransformException {
421 double d = getDeterminant();
422 if (Math.abs(d) <= Double.MIN_VALUE) throw new NoninvertibleTransformException("Matrix3D cannot be inverted. Determinant: "+d+"\n"+toString());
423
424 return new Matrix3D(
425 (m11*m22 - m12*m21)/d,
426 (m12*m20 - m10*m22)/d,
427 (m10*m21 - m11*m20)/d,
428
429 (m21*m02 - m22*m01)/d,
430 (m22*m00 - m20*m02)/d,
431 (m20*m01 - m21*m00)/d,
432
433 (m01*m12 - m02*m11)/d,
434 (m02*m10 - m00*m12)/d,
435 (m00*m11 - m01*m10)/d,
436
437 (m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22))/d,
438 (m02*(m13*m20 - m10*m23) + m03*(m10*m22 - m12*m20) + m00*(m12*m23 - m13*m22))/d,
439 (m03*(m11*m20 - m10*m21) + m00*(m13*m21 - m11*m23) + m01*(m10*m23 - m13*m20))/d
440 );
441 }
442
443 /***
444 * Concatenates this matrix with m, such that M' = M * m.
445 */
446 public void concatenate(Matrix3D m) {
447 set(
448 m00*m.m00 + m01*m.m10 + m02*m.m20,
449 m10*m.m00 + m11*m.m10 + m12*m.m20,
450 m20*m.m00 + m21*m.m10 + m22*m.m20,
451
452 m00*m.m01 + m01*m.m11 + m02*m.m21,
453 m10*m.m01 + m11*m.m11 + m12*m.m21,
454 m20*m.m01 + m21*m.m11 + m22*m.m21,
455
456 m00*m.m02 + m01*m.m12 + m02*m.m22,
457 m10*m.m02 + m11*m.m12 + m12*m.m22,
458 m20*m.m02 + m21*m.m12 + m22*m.m22,
459
460 m00*m.m03 + m01*m.m13 + m02*m.m23 + m03,
461 m10*m.m03 + m11*m.m13 + m12*m.m23 + m13,
462 m20*m.m03 + m21*m.m13 + m22*m.m23 + m23
463 );
464 }
465
466 /***
467 * Pre-concatenates this matrix with m, such that M' = m * M.
468 */
469 public void preConcatenate(Matrix3D m) {
470 set(
471 m.m00*m00 + m.m01*m10 + m.m02*m20,
472 m.m10*m00 + m.m11*m10 + m.m12*m20,
473 m.m20*m00 + m.m21*m10 + m.m22*m20,
474
475 m.m00*m01 + m.m01*m11 + m.m02*m21,
476 m.m10*m01 + m.m11*m11 + m.m12*m21,
477 m.m20*m01 + m.m21*m11 + m.m22*m21,
478
479 m.m00*m02 + m.m01*m12 + m.m02*m22,
480 m.m10*m02 + m.m11*m12 + m.m12*m22,
481 m.m20*m02 + m.m21*m12 + m.m22*m22,
482
483 m.m00*m03 + m.m01*m13 + m.m02*m23 + m.m03,
484 m.m10*m03 + m.m11*m13 + m.m12*m23 + m.m13,
485 m.m20*m03 + m.m21*m13 + m.m22*m23 + m.m23
486 );
487 }
488
489 /***
490 * Transforms xyz by this matrix, using translation if delta is false.
491 * The input array is returned.
492 */
493 public double[] transform(double[] xyz, boolean delta) {
494 int d = delta ? 0 : 1;
495 double x = xyz[X];
496 double y = xyz[Y];
497 double z = xyz[Z];
498
499 xyz[U] = m00*x + m01*y + m02*z + m03*d;
500 xyz[V] = m10*x + m11*y + m12*z + m13*d;
501 xyz[W] = m20*x + m21*y + m22*z + m23*d;
502
503 return xyz;
504 }
505
506 /***
507 * Transforms xyz by this matrix for n points, using translation if delta is false.
508 * The input array is returned.
509 */
510 public double[][] transform(double[][] xyz, int n, boolean delta) {
511 int d = delta ? 0 : 1;
512 for (int i=0; i<n; i++) {
513 double x = xyz[X][i];
514 double y = xyz[Y][i];
515 double z = xyz[Z][i];
516 xyz[U][i] = m00*x + m01*y + m02*z + m03*d;
517 xyz[V][i] = m10*x + m11*y + m12*z + m13*d;
518 xyz[W][i] = m20*x + m21*y + m22*z + m23*d;
519 }
520 return xyz;
521 }
522
523 public String toString() {
524 StringBuffer s = new StringBuffer();
525 s.append("Matrix3D: ");
526 s.append(fmt.format(m00));
527 s.append(", ");
528 s.append(fmt.format(m01));
529 s.append(", ");
530 s.append(fmt.format(m02));
531 s.append(", ");
532 s.append(fmt.format(m03));
533 s.append("\n");
534
535 s.append(" ");
536 s.append(fmt.format(m10));
537 s.append(", ");
538 s.append(fmt.format(m11));
539 s.append(", ");
540 s.append(fmt.format(m12));
541 s.append(", ");
542 s.append(fmt.format(m13));
543 s.append("\n");
544
545 s.append(" ");
546 s.append(fmt.format(m20));
547 s.append(", ");
548 s.append(fmt.format(m21));
549 s.append(", ");
550 s.append(fmt.format(m22));
551 s.append(", ");
552 s.append(fmt.format(m23));
553 s.append("\n");
554
555 return s.toString();
556 }
557
558
559
560
561
562 public void save(XMLIOManager xmlioManager,
563 org.jdom.Element nodeEl) {
564 nodeEl.setAttribute("m00", Double.toString(m00));
565 nodeEl.setAttribute("m10", Double.toString(m10));
566 nodeEl.setAttribute("m20", Double.toString(m20));
567
568 nodeEl.setAttribute("m01", Double.toString(m01));
569 nodeEl.setAttribute("m11", Double.toString(m11));
570 nodeEl.setAttribute("m21", Double.toString(m21));
571
572 nodeEl.setAttribute("m02", Double.toString(m02));
573 nodeEl.setAttribute("m12", Double.toString(m12));
574 nodeEl.setAttribute("m22", Double.toString(m22));
575
576 nodeEl.setAttribute("m03", Double.toString(m03));
577 nodeEl.setAttribute("m13", Double.toString(m13));
578 nodeEl.setAttribute("m23", Double.toString(m23));
579 }
580
581 public void restore(XMLIOManager xmlioManager,
582 org.jdom.Element nodeEl) {
583 try {
584 set(nodeEl.getAttribute("m00").getDoubleValue(),
585 nodeEl.getAttribute("m10").getDoubleValue(),
586 nodeEl.getAttribute("m20").getDoubleValue(),
587
588 nodeEl.getAttribute("m01").getDoubleValue(),
589 nodeEl.getAttribute("m11").getDoubleValue(),
590 nodeEl.getAttribute("m21").getDoubleValue(),
591
592 nodeEl.getAttribute("m02").getDoubleValue(),
593 nodeEl.getAttribute("m12").getDoubleValue(),
594 nodeEl.getAttribute("m22").getDoubleValue(),
595
596 nodeEl.getAttribute("m03").getDoubleValue(),
597 nodeEl.getAttribute("m13").getDoubleValue(),
598 nodeEl.getAttribute("m23").getDoubleValue()
599 );
600 } catch (DataConversionException dce) {
601 throw new IllegalArgumentException(getClass()+": "+dce.getMessage());
602 }
603 }
604 }