001: /**********************************************************
002: Copyright (C) 2001 Daniel Selman
003:
004: First distributed with the book "Java 3D Programming"
005: by Daniel Selman and published by Manning Publications.
006: http://manning.com/selman
007:
008: This program is free software; you can redistribute it and/or
009: modify it under the terms of the GNU General Public License
010: as published by the Free Software Foundation, version 2.
011:
012: This program is distributed in the hope that it will be useful,
013: but WITHOUT ANY WARRANTY; without even the implied warranty of
014: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
015: GNU General Public License for more details.
016:
017: The license can be found on the WWW at:
018: http://www.fsf.org/copyleft/gpl.html
019:
020: Or by writing to:
021: Free Software Foundation, Inc.,
022: 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
023:
024: Authors can be contacted at:
025: Daniel Selman: daniel@selman.org
026:
027: If you make changes you think others would like, please
028: contact one of the authors or someone at the
029: www.j3d.org web site.
030: **************************************************************/package com.db.utils.euler;
031:
032: import javax.media.j3d.*;
033: import javax.vecmath.*;
034:
035: public class Euler {
036: public static int EulOrdXYZs() {
037: return EulOrd(X, EulParEven, EulRepNo, EulFrmS);
038: }
039:
040: public static int EulOrdXYXs() {
041: return EulOrd(X, EulParEven, EulRepYes, EulFrmS);
042: }
043:
044: public static int EulOrdXZYs() {
045: return EulOrd(X, EulParOdd, EulRepNo, EulFrmS);
046: }
047:
048: public static int EulOrdXZXs() {
049: return EulOrd(X, EulParOdd, EulRepYes, EulFrmS);
050: }
051:
052: public static int EulOrdYZXs() {
053: return EulOrd(Y, EulParEven, EulRepNo, EulFrmS);
054: }
055:
056: public static int EulOrdYZYs() {
057: return EulOrd(Y, EulParEven, EulRepYes, EulFrmS);
058: }
059:
060: public static int EulOrdYXZs() {
061: return EulOrd(Y, EulParOdd, EulRepNo, EulFrmS);
062: }
063:
064: public static int EulOrdYXYs() {
065: return EulOrd(Y, EulParOdd, EulRepYes, EulFrmS);
066: }
067:
068: public static int EulOrdZXYs() {
069: return EulOrd(Z, EulParEven, EulRepNo, EulFrmS);
070: }
071:
072: public static int EulOrdZXZs() {
073: return EulOrd(Z, EulParEven, EulRepYes, EulFrmS);
074: }
075:
076: public static int EulOrdZYXs() {
077: return EulOrd(Z, EulParOdd, EulRepNo, EulFrmS);
078: }
079:
080: public static int EulOrdZYZs() {
081: return EulOrd(Z, EulParOdd, EulRepYes, EulFrmS);
082: }
083:
084: /* Rotating axes */
085: public static int EulOrdZYXr() {
086: return EulOrd(X, EulParEven, EulRepNo, EulFrmR);
087: }
088:
089: public static int EulOrdXYXr() {
090: return EulOrd(X, EulParEven, EulRepYes, EulFrmR);
091: }
092:
093: public static int EulOrdYZXr() {
094: return EulOrd(X, EulParOdd, EulRepNo, EulFrmR);
095: }
096:
097: public static int EulOrdXZXr() {
098: return EulOrd(X, EulParOdd, EulRepYes, EulFrmR);
099: }
100:
101: public static int EulOrdXZYr() {
102: return EulOrd(Y, EulParEven, EulRepNo, EulFrmR);
103: }
104:
105: public static int EulOrdYZYr() {
106: return EulOrd(Y, EulParEven, EulRepYes, EulFrmR);
107: }
108:
109: public static int EulOrdZXYr() {
110: return EulOrd(Y, EulParOdd, EulRepNo, EulFrmR);
111: }
112:
113: public static int EulOrdYXYr() {
114: return EulOrd(Y, EulParOdd, EulRepYes, EulFrmR);
115: }
116:
117: public static int EulOrdYXZr() {
118: return EulOrd(Z, EulParEven, EulRepNo, EulFrmR);
119: }
120:
121: public static int EulOrdZXZr() {
122: return EulOrd(Z, EulParEven, EulRepYes, EulFrmR);
123: }
124:
125: public static int EulOrdXYZr() {
126: return EulOrd(Z, EulParOdd, EulRepNo, EulFrmR);
127: }
128:
129: public static int EulOrdZYZr() {
130: return EulOrd(Z, EulParOdd, EulRepYes, EulFrmR);
131: }
132:
133: public static int EulFrm(int ord) {
134: // DCS, was unsigned
135: return ((ord) & 1);
136: }
137:
138: public static int EulRep(int ord) {
139: // DCS, was unsigned
140: return (((ord) >> 1) & 1);
141: }
142:
143: public static int EulPar(int ord) {
144: return (((ord) >> 2) & 1);
145: }
146:
147: public static int EulAxI(int ord) {
148: // DCS, was unsigned
149: return ((int) (EulSafe((((ord) >> 3) & 3))));
150: }
151:
152: public static int EulAxJ(int ord) {
153: int i = 0;
154:
155: if (EulPar(ord) == EulParOdd)
156: i = 1;
157:
158: return ((int) (EulNext(EulAxI(ord) + i)));
159: }
160:
161: public static int EulAxK(int ord) {
162: int i = 0;
163:
164: if (EulPar(ord) != EulParOdd)
165: i = 1;
166:
167: return ((int) (EulNext(EulAxI(ord) + i)));
168: }
169:
170: public static int EulAxH(int ord) {
171: if (EulRep(ord) == EulRepNo)
172: return EulAxK(ord);
173:
174: return EulAxI(ord);
175: }
176:
177: public static int EulOrd(int i, int p, int r, int f) {
178: return (((((((i) << 1) + (p)) << 1) + (r)) << 1) + (f));
179: }
180:
181: // enum
182: static final int X = 0;
183: static final int Y = 1;
184: static final int Z = 2;
185: static final int W = 3;
186:
187: static final int EulRepNo = 0;
188: static final int EulRepYes = 1;
189:
190: static final int EulParEven = 0;
191: static final int EulParOdd = 1;
192:
193: static final int EulFrmS = 0;
194: static final int EulFrmR = 1;
195:
196: static final float FLT_EPSILON = 1.192092896e-07F;
197:
198: static EulGetOrdInfo EulGetOrd(int ord) {
199: EulGetOrdInfo info = new EulGetOrdInfo();
200:
201: // note, used to be unsigned!
202: int o = ord;
203: info.f = o & 1;
204: o >>= 1;
205: info.s = o & 1;
206: o >>= 1;
207: info.n = o & 1;
208: o >>= 1;
209: info.i = EulSafe(o & 3);
210: info.j = EulNext(info.i + info.n);
211: info.k = EulNext(info.i + 1 - info.n);
212:
213: if (info.s != 0)
214: info.h = info.k;
215: else
216: info.h = info.i;
217:
218: return info;
219: }
220:
221: static int EulSafe(int val) {
222: int[] valArray = { 0, 1, 2, 0 };
223: return valArray[val];
224: }
225:
226: static int EulNext(int val) {
227: int[] valArray = { 1, 2, 0, 1 };
228: return valArray[val];
229: }
230:
231: // float HMatrix[4][4];
232:
233: /* Convert matrix to Euler angles (in radians). */
234: public static EulerAngles Eul_FromMatrix(float[][] M, int order) {
235: EulerAngles ea = new EulerAngles();
236:
237: EulGetOrdInfo info = EulGetOrd(order);
238:
239: int i = info.i;
240: int j = info.j;
241: int k = info.k;
242: int h = info.h;
243: int n = info.n;
244: int s = info.s;
245: int f = info.f;
246:
247: if (s == EulRepYes) {
248: double sy = Math
249: .sqrt(M[i][j] * M[i][j] + M[i][k] * M[i][k]);
250: if (sy > 16 * FLT_EPSILON) {
251: ea.x = (float) Math.atan2(M[i][j], M[i][k]);
252: ea.y = (float) Math.atan2(sy, M[i][i]);
253: ea.z = (float) Math.atan2(M[j][i], -M[k][i]);
254: } else {
255: ea.x = (float) Math.atan2(-M[j][k], M[j][j]);
256: ea.y = (float) Math.atan2(sy, M[i][i]);
257: ea.z = 0;
258: }
259: } else {
260: double cy = Math
261: .sqrt(M[i][i] * M[i][i] + M[j][i] * M[j][i]);
262: if (cy > 16 * FLT_EPSILON) {
263: ea.x = (float) Math.atan2(M[k][j], M[k][k]);
264: ea.y = (float) Math.atan2(-M[k][i], cy);
265: ea.z = (float) Math.atan2(M[j][i], M[i][i]);
266: } else {
267: ea.x = (float) Math.atan2(-M[j][k], M[j][j]);
268: ea.y = (float) Math.atan2(-M[k][i], cy);
269: ea.z = 0;
270: }
271: }
272: if (n == EulParOdd) {
273: ea.x = -ea.x;
274: ea.y = -ea.y;
275: ea.z = -ea.z;
276: }
277: if (f == EulFrmR) {
278: float t = ea.x;
279: ea.x = ea.z;
280: ea.z = t;
281: }
282: ea.w = order;
283: return (ea);
284: }
285:
286: /* Convert quaternion to Euler angles (in radians). */
287: public static EulerAngles Eul_FromQuat(Quat q, int order) {
288: float[][] M = new float[4][4];
289: double Nq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
290: double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
291: double xs = q.x * s, ys = q.y * s, zs = q.z * s;
292: double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
293: double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
294: double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
295: M[X][X] = (float) (1.0 - (yy + zz));
296: M[X][Y] = (float) (xy - wz);
297: M[X][Z] = (float) (xz + wy);
298: M[Y][X] = (float) (xy + wz);
299: M[Y][Y] = (float) (1.0 - (xx + zz));
300: M[Y][Z] = (float) (yz - wx);
301: M[Z][X] = (float) (xz - wy);
302: M[Z][Y] = (float) (yz + wx);
303: M[Z][Z] = (float) (1.0 - (xx + yy));
304: M[W][X] = M[W][Y] = M[W][Z] = M[X][W] = M[Y][W] = M[Z][W] = 0.0f;
305: M[W][W] = 1.0f;
306: return (Eul_FromMatrix(M, order));
307: }
308:
309: public static Point3d getEulerRotation(Transform3D t3d) {
310: Point3d Rotation = new Point3d();
311:
312: Matrix3d m1 = new Matrix3d();
313: t3d.get(m1);
314:
315: // extract the rotation angles from the upper 3x3 rotation
316: // component of the 4x4 transformation matrix
317: Rotation.y = -java.lang.Math.asin(m1.getElement(2, 0));
318: double c = java.lang.Math.cos(Rotation.y);
319: double tRx, tRy, tRz;
320:
321: if (java.lang.Math.abs(Rotation.y) > 0.00001) {
322: tRx = m1.getElement(2, 2) / c;
323: tRy = -m1.getElement(2, 1) / c;
324:
325: Rotation.x = java.lang.Math.atan2(tRy, tRx);
326:
327: tRx = m1.getElement(0, 0) / c;
328: tRy = -m1.getElement(1, 0) / c;
329:
330: Rotation.z = java.lang.Math.atan2(tRy, tRx);
331: } else {
332: Rotation.x = 0.0;
333:
334: tRx = m1.getElement(1, 1);
335: tRy = m1.getElement(0, 1);
336:
337: Rotation.z = java.lang.Math.atan2(tRy, tRx);
338: }
339:
340: Rotation.x = -Rotation.x;
341: Rotation.z = -Rotation.z;
342:
343: // now try to ensure that the values are positive by adding 2PI if necessary...
344: if (Rotation.x < 0.0)
345: Rotation.x += 2 * java.lang.Math.PI;
346:
347: if (Rotation.y < 0.0)
348: Rotation.y += 2 * java.lang.Math.PI;
349:
350: if (Rotation.z < 0.0)
351: Rotation.z += 2 * java.lang.Math.PI;
352:
353: return Rotation;
354: }
355: }
|