1: package com.jme.math;
2:
3: import java.io.Externalizable;
4: import java.io.IOException;
5: import java.io.ObjectInput;
6: import java.io.ObjectOutput;
7: import java.util.logging.Logger;
8:
9: import com.jme.system.JmeException;
10: import com.jme.util.export.InputCapsule;
11: import com.jme.util.export.JMEExporter;
12: import com.jme.util.export.JMEImporter;
13: import com.jme.util.export.OutputCapsule;
14: import com.jme.util.export.Savable;
15:
16: /**
17: *Quaterniondefines a single example of a more general class of
18: * hypercomplex numbers. Quaternions extends a rotation in three dimensions to a
19: * rotation in four dimensions. This avoids "gimbal lock" and allows for smooth
20: * continuous rotation.
21: * 四元数
22: *
23: *Quaternionis defined by four floating point numbers: {x y z w}.
24: *
25: * @author Mark Powell
26: * @author Joshua Slack
27: */
28: public class Quaternion implements Externalizable, Savable, Cloneable {
29: private static final Logger logger = Logger.getLogger(Quaternion.class
30: .getName());
31:
32: private static final long serialVersionUID = 1L;
33:
34: public float x, y, z, w;
35:
36: /**
37: * Constructor instantiates a newQuaternionobject
38: * initializing all values to zero, except w which is initialized to 1.
39: *
40: */
41: public Quaternion() {
42: x = 0;
43: y = 0;
44: z = 0;
45: w = 1;
46: }
47:
48: /**
49: * Constructor instantiates a newQuaternionobject from the
50: * given list of parameters.
51: *
52: * @param x
53: * the x value of the quaternion.
54: * @param y
55: * the y value of the quaternion.
56: * @param z
57: * the z value of the quaternion.
58: * @param w
59: * the w value of the quaternion.
60: */
61: public Quaternion(float x, float y, float z, float w) {
62: this.x = x;
63: this.y = y;
64: this.z = z;
65: this.w = w;
66: }
67:
68: /**
69: * sets the data in aQuaternionobject from the given list of
70: * parameters.
71: *
72: * @param x
73: * the x value of the quaternion.
74: * @param y
75: * the y value of the quaternion.
76: * @param z
77: * the z value of the quaternion.
78: * @param w
79: * the w value of the quaternion.
80: */
81: public void set(float x, float y, float z, float w) {
82: this.x = x;
83: this.y = y;
84: this.z = z;
85: this.w = w;
86: }
87:
88: /**
89: * Sets the data in thisQuaternionobject to be equal to the
90: * passedQuaternionobject. The values are copied producing a
91: * new object.
92: *
93: * @param q
94: * The Quaternion to copy values from.
95: * @return this for chaining
96: */
97: public Quaternion set(Quaternion q) {
98: this.x = q.x;
99: this.y = q.y;
100: this.z = q.z;
101: this.w = q.w;
102: return this;
103: }
104:
105: /**
106: * Constructor instantiates a newQuaternionobject from a
107: * collection of rotation angles.
108: * 从欧拉角转换成四元数
109: *
110: * @param angles
111: * the angles of rotation (x, y, z) that will define the
112: *Quaternion.
113: */
114: public Quaternion(float[] angles) {
115: fromAngles(angles);
116: }
117:
118: /**
119: * Constructor instantiates a newQuaternionobject from an
120: * interpolation between two other quaternions.
121: * 球面线性插值构造四元数
122: *
123: * @param q1
124: * the first quaternion.
125: * @param q2
126: * the second quaternion.
127: * @param interp
128: * the amount to interpolate between the two quaternions.
129: */
130: public Quaternion(Quaternion q1, Quaternion q2, float interp) {
131: slerp(q1, q2, interp);
132: }
133:
134: /**
135: * Constructor instantiates a newQuaternionobject from an
136: * existing quaternion, creating a copy.
137: *
138: * @param q
139: * the quaternion to copy.
140: */
141: public Quaternion(Quaternion q) {
142: this.x = q.x;
143: this.y = q.y;
144: this.z = q.z;
145: this.w = q.w;
146: }
147:
148: /**
149: * Sets this Quaternion to {0, 0, 0, 1}. Same as calling set(0,0,0,1).
150: */
151: public void loadIdentity() {
152: x = y = z = 0;
153: w = 1;
154: }
155:
156: /**
157: * @return true if this Quaternion is {0,0,0,1}
158: */
159: public boolean isIdentity() {
160: if (x == 0 && y == 0 && z == 0 && w == 1)
161: return true;
162: else
163: return false;
164: }
165:
166: /**
167: *fromAnglesbuilds a quaternion from the Euler rotation
168: * angles (y,r,p).
169: * 从欧拉角转换成四元数
170: *
171: * @param angles
172: * the Euler angles of rotation (in radians).
173: */
174: public void fromAngles(float[] angles) {
175: if (angles.length != 3)
176: throw new IllegalArgumentException(
177: "Angles array must have three elements");
178:
179: fromAngles(angles[0], angles[1], angles[2]);
180: }
181:
182: /**
183: *fromAnglesbuilds a Quaternion from the Euler rotation
184: * angles (y,r,p). Note that we are applying in order: roll, pitch, yaw but
185: * we've ordered them in x, y, and z for convenience. See:
186: * http://www.euclideanspace
187: * .com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
188: * 从欧拉角转换成四元数
189: *
190: * @param yaw
191: * the Euler yaw of rotation (in radians). (aka Bank, often rot
192: * around x)
193: * @param roll
194: * the Euler roll of rotation (in radians). (aka Heading, often
195: * rot around y)
196: * @param pitch
197: * the Euler pitch of rotation (in radians). (aka Attitude, often
198: * rot around z)
199: */
200: public Quaternion fromAngles(float yaw, float roll, float pitch) {
201: float angle;
202: float sinRoll, sinPitch, sinYaw, cosRoll, cosPitch, cosYaw;
203: angle = pitch * 0.5f;
204: sinPitch = FastMath.sin(angle);
205: cosPitch = FastMath.cos(angle);
206: angle = roll * 0.5f;
207: sinRoll = FastMath.sin(angle);
208: cosRoll = FastMath.cos(angle);
209: angle = yaw * 0.5f;
210: sinYaw = FastMath.sin(angle);
211: cosYaw = FastMath.cos(angle);
212:
213: // variables used to reduce multiplication calls.
214: float cosRollXcosPitch = cosRoll * cosPitch;
215: float sinRollXsinPitch = sinRoll * sinPitch;
216: float cosRollXsinPitch = cosRoll * sinPitch;
217: float sinRollXcosPitch = sinRoll * cosPitch;
218:
219: w = (cosRollXcosPitch * cosYaw - sinRollXsinPitch * sinYaw);
220: x = (cosRollXcosPitch * sinYaw + sinRollXsinPitch * cosYaw);
221: y = (sinRollXcosPitch * cosYaw + cosRollXsinPitch * sinYaw);
222: z = (cosRollXsinPitch * cosYaw - sinRollXcosPitch * sinYaw);
223:
224: normalize();
225: return this;
226: }
227:
228: /**
229: *toAnglesreturns this quaternion converted to Euler rotation
230: * angles (yaw,roll,pitch).
231: * See http://www.euclideanspace.com/maths/geometry/rotations/conversions/
232: * quaternionToEuler/index.htm
233: * 从四元数转换成欧拉角
234: *
235: * @param angles
236: * the float[] in which the angles should be stored, or null if
237: * you want a new float[] to be created
238: * @return the float[] in which the angles are stored.
239: */
240: public float[] toAngles(float[] angles) {
241: if (angles == null)
242: angles = new float[3];
243: else if (angles.length != 3)
244: throw new IllegalArgumentException(
245: "Angles array must have three elements");
246:
247: float sqw = w * w;
248: float sqx = x * x;
249: float sqy = y * y;
250: float sqz = z * z;
251: float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise
252: // is correction factor
253: float test = x * y + z * w;
254: if (test > 0.499 * unit) { // singularity at north pole
255: angles[1] = 2 * FastMath.atan2(x, w);
256: angles[2] = FastMath.HALF_PI;
257: angles[0] = 0;
258: } else if (test < -0.499 * unit) { // singularity at south pole
259: angles[1] = -2 * FastMath.atan2(x, w);
260: angles[2] = -FastMath.HALF_PI;
261: angles[0] = 0;
262: } else {
263: angles[1] = FastMath.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz
264: + sqw); // roll or heading
265: angles[2] = FastMath.asin(2 * test / unit); // pitch or attitude
266: angles[0] = FastMath.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz
267: + sqw); // yaw or bank
268: }
269: return angles;
270: }
271:
272: /**
273: *
274: *fromRotationMatrixgenerates a quaternion from a supplied
275: * matrix. This matrix is assumed to be a rotational matrix.
276: * 从矩阵转换成四元数，矩阵必须是旋转矩阵
277: *
278: * @param matrix
279: * the matrix that defines the rotation.
280: */
281: public Quaternion fromRotationMatrix(Matrix3f matrix) {
282: return fromRotationMatrix(matrix.m00, matrix.m01, matrix.m02,
283: matrix.m10, matrix.m11, matrix.m12, matrix.m20, matrix.m21,
284: matrix.m22);
285: }
286:
287: /**
288: * 从矩阵转换成四元数，矩阵必须是旋转矩阵
289: */
290: public Quaternion fromRotationMatrix(float m00, float m01, float m02,
291: float m10, float m11, float m12, float m20, float m21, float m22) {
292: // Use the Graphics Gems code, from
293: // ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
294: // *NOT* the "Matrix and Quaternions FAQ", which has errors!
295:
296: // the trace is the sum of the diagonal elements; see
297: // http://mathworld.wolfram.com/MatrixTrace.html
298: float t = m00 + m11 + m22;
299:
300: // we protect the division by s by ensuring that s>=1
301: if (t >= 0) { // |w| >= .5
302: float s = FastMath.sqrt(t + 1); // |s|>=1 ...
303: w = 0.5f * s;
304: s = 0.5f / s; // so this division isn't bad
305: x = (m21 - m12) * s;
306: y = (m02 - m20) * s;
307: z = (m10 - m01) * s;
308: } else if ((m00 > m11) && (m00 > m22)) {
309: float s = FastMath.sqrt(1.0f + m00 - m11 - m22); // |s|>=1
310: x = s * 0.5f; // |x| >= .5
311: s = 0.5f / s;
312: y = (m10 + m01) * s;
313: z = (m02 + m20) * s;
314: w = (m21 - m12) * s;
315: } else if (m11 > m22) {
316: float s = FastMath.sqrt(1.0f + m11 - m00 - m22); // |s|>=1
317: y = s * 0.5f; // |y| >= .5
318: s = 0.5f / s;
319: x = (m10 + m01) * s;
320: z = (m21 + m12) * s;
321: w = (m02 - m20) * s;
322: } else {
323: float s = FastMath.sqrt(1.0f + m22 - m00 - m11); // |s|>=1
324: z = s * 0.5f; // |z| >= .5
325: s = 0.5f / s;
326: x = (m02 + m20) * s;
327: y = (m21 + m12) * s;
328: w = (m10 - m01) * s;
329: }
330:
331: return this;
332: }
333:
334: /**
335: *toRotationMatrixconverts this quaternion to a rotational
336: * matrix. Note: the result is created from a normalized version of this
337: * quat.
338: * 由四元数构造完整的旋转矩阵
339: *
340: * @return the rotation matrix representation of this quaternion.
341: */
342: public Matrix3f toRotationMatrix() {
343: Matrix3f matrix = new Matrix3f();
344: return toRotationMatrix(matrix);
345: }
346:
347: /**
348: *toRotationMatrixconverts this quaternion to a rotational
349: * matrix. The result is stored in result.
350: * 由四元数构造完整的旋转矩阵
351: *
352: * @param result
353: * The Matrix3f to store the result in.
354: * @return the rotation matrix representation of this quaternion.
355: */
356: public Matrix3f toRotationMatrix(Matrix3f result) {
357:
358: float norm = norm();
359: // we explicitly test norm against one here, saving a division
360: // at the cost of a test and branch. Is it worth it?
361: float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0;
362:
363: // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
364: // will be used 2-4 times each.
365: float xs = x * s;
366: float ys = y * s;
367: float zs = z * s;
368: float xx = x * xs;
369: float xy = x * ys;
370: float xz = x * zs;
371: float xw = w * xs;
372: float yy = y * ys;
373: float yz = y * zs;
374: float yw = w * ys;
375: float zz = z * zs;
376: float zw = w * zs;
377:
378: // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
379: result.m00 = 1 - (yy + zz);
380: result.m01 = (xy - zw);
381: result.m02 = (xz + yw);
382: result.m10 = (xy + zw);
383: result.m11 = 1 - (xx + zz);
384: result.m12 = (yz - xw);
385: result.m20 = (xz - yw);
386: result.m21 = (yz + xw);
387: result.m22 = 1 - (xx + yy);
388:
389: return result;
390: }
391:
392: /**
393: *toRotationMatrixconverts this quaternion to a rotational
394: * matrix. The result is stored in result. 4th row and 4th column values are
395: * untouched. Note: the result is created from a normalized version of this
396: * quat.
397: * 由四元数构造完整的旋转矩阵
398: *
399: * @param result
400: * The Matrix4f to store the result in.
401: * @return the rotation matrix representation of this quaternion.
402: */
403: public Matrix4f toRotationMatrix(Matrix4f result) {
404:
405: float norm = norm();
406: // we explicitly test norm against one here, saving a division
407: // at the cost of a test and branch. Is it worth it?
408: float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0;
409:
410: // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
411: // will be used 2-4 times each.
412: float xs = x * s;
413: float ys = y * s;
414: float zs = z * s;
415: float xx = x * xs;
416: float xy = x * ys;
417: float xz = x * zs;
418: float xw = w * xs;
419: float yy = y * ys;
420: float yz = y * zs;
421: float yw = w * ys;
422: float zz = z * zs;
423: float zw = w * zs;
424:
425: // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
426: result.m00 = 1 - (yy + zz);
427: result.m01 = (xy - zw);
428: result.m02 = (xz + yw);
429: result.m10 = (xy + zw);
430: result.m11 = 1 - (xx + zz);
431: result.m12 = (yz - xw);
432: result.m20 = (xz - yw);
433: result.m21 = (yz + xw);
434: result.m22 = 1 - (xx + yy);
435:
436: return result;
437: }
438:
439: /**
440: *getRotationColumnreturns one of three columns specified by
441: * the parameter. This column is returned as aVector3fobject.
442: * 四元数转换到矩阵，获取指定的列构造的向量
443: *
444: * @param i
445: * the column to retrieve. Must be between 0 and 2.
446: * @return the column specified by the index.
447: */
448: public Vector3f getRotationColumn(int i) {
449: return getRotationColumn(i, null);
450: }
451:
452: /**
453: *getRotationColumnreturns one of three columns specified by
454: * the parameter. This column is returned as aVector3fobject.
455: * The value is retrieved as if this quaternion was first normalized.
456: * 四元数转换到矩阵，获取指定的列构造的向量
457: *
458: * @param i
459: * the column to retrieve. Must be between 0 and 2.
460: * @param store
461: * the vector object to store the result in. if null, a new one
462: * is created.
463: * @return the column specified by the index.
464: */
465: public Vector3f getRotationColumn(int i, Vector3f store) {
466: if (store == null)
467: store = new Vector3f();
468:
469: float norm = norm();
470: if (norm != 1.0f) {
471: norm = FastMath.invSqrt(norm);
472: }
473:
474: float xx = x * x * norm;
475: float xy = x * y * norm;
476: float xz = x * z * norm;
477: float xw = x * w * norm;
478: float yy = y * y * norm;
479: float yz = y * z * norm;
480: float yw = y * w * norm;
481: float zz = z * z * norm;
482: float zw = z * w * norm;
483:
484: switch (i) {
485: case 0:
486: store.x = 1 - 2 * (yy + zz);
487: store.y = 2 * (xy + zw);
488: store.z = 2 * (xz - yw);
489: break;
490: case 1:
491: store.x = 2 * (xy - zw);
492: store.y = 1 - 2 * (xx + zz);
493: store.z = 2 * (yz + xw);
494: break;
495: case 2:
496: store.x = 2 * (xz + yw);
497: store.y = 2 * (yz - xw);
498: store.z = 1 - 2 * (xx + yy);
499: break;
500: default:
501: logger.warning("Invalid column index.");
502: throw new JmeException("Invalid column index. " + i);
503: }
504:
505: return store;
506: }
507:
508: /**
509: *fromAngleAxissets this quaternion to the values specified
510: * by an angle and an axis of rotation. This method creates an object, so
511: * use fromAngleNormalAxis if your axis is already normalized.
512: * 绕轴axis（非单位向量）旋转angle（弧度）角的四元数
513: *
514: * @param angle
515: * the angle to rotate (in radians).
516: * @param axis
517: * the axis of rotation.
518: * @return this quaternion
519: */
520: public Quaternion fromAngleAxis(float angle, Vector3f axis) {
521: Vector3f normAxis = axis.normalize();
522: fromAngleNormalAxis(angle, normAxis);
523: return this;
524: }
525:
526: /**
527: *fromAngleNormalAxissets this quaternion to the values
528: * specified by an angle and a normalized axis of rotation.
529: * 绕轴axis（单位向量）旋转angle（弧度）角的四元数
530: *
531: * @param angle
532: * the angle to rotate (in radians).
533: * @param axis
534: * the axis of rotation (already normalized).
535: */
536: public Quaternion fromAngleNormalAxis(float angle, Vector3f axis) {
537: if (axis.x == 0 && axis.y == 0 && axis.z == 0) {
538: loadIdentity();
539: } else {
540: float halfAngle = 0.5f * angle;
541: float sin = FastMath.sin(halfAngle);
542: w = FastMath.cos(halfAngle);
543: x = sin * axis.x;
544: y = sin * axis.y;
545: z = sin * axis.z;
546: }
547: return this;
548: }
549:
550: /**
551: *toAngleAxissets a given angle and axis to that represented
552: * by the current quaternion. The values are stored as following: The axis
553: * is provided as a parameter and built by the method, the angle is returned
554: * as a float.
555: * 获取由该四元数代表的旋转的旋转轴和角
556: *
557: * @param axisStore
558: * the object we'll store the computed axis in.
559: * @return the angle of rotation in radians.
560: */
561: public float toAngleAxis(Vector3f axisStore) {
562: float sqrLength = x * x + y * y + z * z;
563: float angle;
564: if (sqrLength == 0.0f) {
565: angle = 0.0f;
566: if (axisStore != null) {
567: axisStore.x = 1.0f;
568: axisStore.y = 0.0f;
569: axisStore.z = 0.0f;
570: }
571: } else {
572: angle = (2.0f * FastMath.acos(w));
573: if (axisStore != null) {
574: float invLength = (1.0f / FastMath.sqrt(sqrLength));
575: axisStore.x = x * invLength;
576: axisStore.y = y * invLength;
577: axisStore.z = z * invLength;
578: }
579: }
580:
581: return angle;
582: }
583:
584: /**
585: *slerpsets this quaternion's value as an interpolation
586: * between two other quaternions.
587: * slerp:球面线性插值（Spherical Linear interpolation）,返回q1到q2之间的插值方位
588: *
589: * @param q1
590: * the first quaternion.
591: * @param q2
592: * the second quaternion.
593: * @param t
594: * the amount to interpolate between the two quaternions.
595: * 插值参数，在0到1之间变化
596: */
597: public Quaternion slerp(Quaternion q1, Quaternion q2, float t) {
598: // Create a local quaternion to store the interpolated quaternion
599: if (q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w) {
600: this.set(q1);
601: return this;
602: }
603:
604: float result = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z)
605: + (q1.w * q2.w);
606:
607: if (result < 0.0f) {
608: // Negate the second quaternion and the result of the dot product
609: q2.x = -q2.x;
610: q2.y = -q2.y;
611: q2.z = -q2.z;
612: q2.w = -q2.w;
613: result = -result;
614: }
615:
616: // Set the first and second scale for the interpolation
617: float scale0 = 1 - t;
618: float scale1 = t;
619:
620: // Check if the angle between the 2 quaternions was big enough to
621: // warrant such calculations
622: if ((1 - result) > 0.1f) {// Get the angle between the 2 quaternions,
623: // and then store the sin() of that angle
624: float theta = FastMath.acos(result);
625: float invSinTheta = 1f / FastMath.sin(theta);
626:
627: // Calculate the scale for q1 and q2, according to the angle and
628: // it's sine value
629: scale0 = FastMath.sin((1 - t) * theta) * invSinTheta;
630: scale1 = FastMath.sin((t * theta)) * invSinTheta;
631: }
632:
633: // Calculate the x, y, z and w values for the quaternion by using a
634: // special
635: // form of linear interpolation for quaternions.
636: this.x = (scale0 * q1.x) + (scale1 * q2.x);
637: this.y = (scale0 * q1.y) + (scale1 * q2.y);
638: this.z = (scale0 * q1.z) + (scale1 * q2.z);
639: this.w = (scale0 * q1.w) + (scale1 * q2.w);
640:
641: // Return the interpolated quaternion
642: return this;
643: }
644:
645: /**
646: * Sets the values of this quaternion to the slerp from itself to q2 by
647: * changeAmnt
648: * slerp:球面线性插值（Spherical Linear interpolation）
649: *
650: * @param q2
651: * Final interpolation value
652: * @param changeAmnt
653: * The amount diffrence
654: */
655: public void slerp(Quaternion q2, float changeAmnt) {
656: if (this.x == q2.x && this.y == q2.y && this.z == q2.z
657: && this.w == q2.w) {
658: return;
659: }
660:
661: float result = (this.x * q2.x) + (this.y * q2.y) + (this.z * q2.z)
662: + (this.w * q2.w);
663:
664: if (result < 0.0f) {
665: // Negate the second quaternion and the result of the dot product
666: q2.x = -q2.x;
667: q2.y = -q2.y;
668: q2.z = -q2.z;
669: q2.w = -q2.w;
670: result = -result;
671: }
672:
673: // Set the first and second scale for the interpolation
674: float scale0 = 1 - changeAmnt;
675: float scale1 = changeAmnt;
676:
677: // Check if the angle between the 2 quaternions was big enough to
678: // warrant such calculations
679: if ((1 - result) > 0.1f) {
680: // Get the angle between the 2 quaternions, and then store the sin()
681: // of that angle
682: float theta = FastMath.acos(result);
683: float invSinTheta = 1f / FastMath.sin(theta);
684:
685: // Calculate the scale for q1 and q2, according to the angle and
686: // it's sine value
687: scale0 = FastMath.sin((1 - changeAmnt) * theta) * invSinTheta;
688: scale1 = FastMath.sin((changeAmnt * theta)) * invSinTheta;
689: }
690:
691: // Calculate the x, y, z and w values for the quaternion by using a
692: // special
693: // form of linear interpolation for quaternions.
694: this.x = (scale0 * this.x) + (scale1 * q2.x);
695: this.y = (scale0 * this.y) + (scale1 * q2.y);
696: this.z = (scale0 * this.z) + (scale1 * q2.z);
697: this.w = (scale0 * this.w) + (scale1 * q2.w);
698: }
699:
700: /**
701: *addadds the values of this quaternion to those of the
702: * parameter quaternion. The result is returned as a new quaternion.
703: *
704: * @param q
705: * the quaternion to add to this.
706: * @return the new quaternion.
707: */
708: public Quaternion add(Quaternion q) {
709: return new Quaternion(x + q.x, y + q.y, z + q.z, w + q.w);
710: }
711:
712: /**
713: *addadds the values of this quaternion to those of the
714: * parameter quaternion. The result is stored in this Quaternion.
715: *
716: * @param q
717: * the quaternion to add to this.
718: * @return This Quaternion after addition.
719: */
720: public Quaternion addLocal(Quaternion q) {
721: this.x += q.x;
722: this.y += q.y;
723: this.z += q.z;
724: this.w += q.w;
725: return this;
726: }
727:
728: /**
729: *subtractsubtracts the values of the parameter quaternion
730: * from those of this quaternion. The result is returned as a new
731: * quaternion.
732: *
733: * @param q
734: * the quaternion to subtract from this.
735: * @return the new quaternion.
736: */
737: public Quaternion subtract(Quaternion q) {
738: return new Quaternion(x - q.x, y - q.y, z - q.z, w - q.w);
739: }
740:
741: /**
742: *subtractsubtracts the values of the parameter quaternion
743: * from those of this quaternion. The result is stored in this Quaternion.
744: *
745: * @param q
746: * the quaternion to subtract from this.
747: * @return This Quaternion after subtraction.
748: */
749: public Quaternion subtractLocal(Quaternion q) {
750: this.x -= q.x;
751: this.y -= q.y;
752: this.z -= q.z;
753: this.w -= q.w;
754: return this;
755: }
756:
757: /**
758: *multmultiplies this quaternion by a parameter quaternion.
759: * The result is returned as a new quaternion. It should be noted that
760: * quaternion multiplication is not cummulative so q * p != p * q.
761: *
762: * @param q
763: * the quaternion to multiply this quaternion by.
764: * @return the new quaternion.
765: */
766: public Quaternion mult(Quaternion q) {
767: return mult(q, null);
768: }
769:
770: /**
771: *multmultiplies this quaternion by a parameter quaternion
772: * (q). 'this' is not modified. It should be noted that quaternion
773: * multiplication is not cummulative so q * p != p * q.
774: *
775: * It IS safe for q and res to be the same object.
776: *
777: * @param q
778: * the quaternion to multiply this quaternion by.
779: * @param res
780: * the quaternion to store the result in (may be null). If
781: * non-null, the input values of 'res' will be ignored and
782: * replaced.
783: * @return If specified res is null, then a new Quaternion; otherwise
784: * returns the populated 'res'.
785: */
786: public Quaternion mult(Quaternion q, Quaternion res) {
787: if (res == null)
788: res = new Quaternion();
789: float qw = q.w, qx = q.x, qy = q.y, qz = q.z;
790: res.x = x * qw + y * qz - z * qy + w * qx;
791: res.y = -x * qz + y * qw + z * qx + w * qy;
792: res.z = x * qy - y * qx + z * qw + w * qz;
793: res.w = -x * qx - y * qy - z * qz + w * qw;
794: return res;
795: }
796:
797: /**
798: *applymultiplies this quaternion by a parameter matrix
799: * internally.
800: *
801: * @param matrix
802: * the matrix to apply to this quaternion.
803: */
804: public void apply(Matrix3f matrix) {
805: float oldX = x, oldY = y, oldZ = z, oldW = w;
806: fromRotationMatrix(matrix);
807: float tempX = x, tempY = y, tempZ = z, tempW = w;
808:
809: x = oldX * tempW + oldY * tempZ - oldZ * tempY + oldW * tempX;
810: y = -oldX * tempZ + oldY * tempW + oldZ * tempX + oldW * tempY;
811: z = oldX * tempY - oldY * tempX + oldZ * tempW + oldW * tempZ;
812: w = -oldX * tempX - oldY * tempY - oldZ * tempZ + oldW * tempW;
813: }
814:
815: /**
816: *
817: *fromAxescreates aQuaternionthat represents
818: * the coordinate system defined by three axes. These axes are assumed to be
819: * orthogonal and no error checking is applied. Thus, the user must insure
820: * that the three axes being provided indeed represents a proper right
821: * handed coordinate system.
822: * 从矩阵的各列转换到四元组
823: *
824: * @param axis
825: * the array containing the three vectors representing the
826: * coordinate system.
827: */
828: public Quaternion fromAxes(Vector3f[] axis) {
829: if (axis.length != 3)
830: throw new IllegalArgumentException(
831: "Axis array must have three elements");
832: return fromAxes(axis[0], axis[1], axis[2]);
833: }
834:
835: /**
836: *
837: *fromAxescreates aQuaternionthat represents
838: * the coordinate system defined by three axes. These axes are assumed to be
839: * orthogonal and no error checking is applied. Thus, the user must insure
840: * that the three axes being provided indeed represents a proper right
841: * handed coordinate system.
842: * 从矩阵的各列转换到四元组
843: *
844: * @param xAxis
845: * vector representing the x-axis of the coordinate system.
846: * @param yAxis
847: * vector representing the y-axis of the coordinate system.
848: * @param zAxis
849: * vector representing the z-axis of the coordinate system.
850: */
851: public Quaternion fromAxes(Vector3f xAxis, Vector3f yAxis, Vector3f zAxis) {
852: return fromRotationMatrix(xAxis.x, yAxis.x, zAxis.x, xAxis.y, yAxis.y,
853: zAxis.y, xAxis.z, yAxis.z, zAxis.z);
854: }
855:
856: /**
857: *
858: *toAxestakes in an array of three vectors. Each vector
859: * corresponds to an axis of the coordinate system defined by the quaternion
860: * rotation.
861: * 从四元组转换到旋转矩阵，获取矩阵的各列
862: *
863: * @param axis
864: * the array of vectors to be filled.
865: */
866: public void toAxes(Vector3f axis[]) {
867: Matrix3f tempMat = toRotationMatrix();
868: axis[0] = tempMat.getColumn(0, axis[0]);
869: axis[1] = tempMat.getColumn(1, axis[1]);
870: axis[2] = tempMat.getColumn(2, axis[2]);
871: }
872:
873: /**
874: *multmultiplies this quaternion by a parameter vector. The
875: * result is returned as a new vector. 'this' is not modified.
876: * 四元数与向量相乘
877: *
878: * @param v
879: * the vector to multiply this quaternion by.
880: * @return the new vector.
881: */
882: public Vector3f mult(Vector3f v) {
883: return mult(v, null);
884: }
885:
886: /**
887: *multmultiplies this quaternion by a parameter vector. The
888: * result is stored in the supplied vector This method is very poorly named,
889: * since the specified vector is modified and, contrary to the other *Local
890: * methods in this and other jME classes, 'this' remains unchanged.
891: * 四元数与向量相乘
892: *
893: * @param v
894: * the vector which this Quaternion multiplies.
895: * @return v
896: */
897: public Vector3f multLocal(Vector3f v) {
898: float tempX, tempY;
899: tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x
900: + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x;
901: tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z
902: * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x
903: * v.y;
904: v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x
905: - y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z;
906: v.x = tempX;
907: v.y = tempY;
908: return v;
909: }
910:
911: /**
912: * Multiplies this Quaternion by the supplied quaternion. The result is
913: * stored in this Quaternion, which is also returned for chaining. Similar
914: * to this *= q.
915: * 四元数与四元数相乘
916: *
917: * @param q
918: * The Quaternion to multiply this one by.
919: * @return This Quaternion, after multiplication.
920: */
921: public Quaternion multLocal(Quaternion q) {
922: float x1 = x * q.w + y * q.z - z * q.y + w * q.x;
923: float y1 = -x * q.z + y * q.w + z * q.x + w * q.y;
924: float z1 = x * q.y - y * q.x + z * q.w + w * q.z;
925: w = -x * q.x - y * q.y - z * q.z + w * q.w;
926: x = x1;
927: y = y1;
928: z = z1;
929: return this;
930: }
931:
932: /**
933: * Multiplies this Quaternion by the supplied quaternion. The result is
934: * stored in this Quaternion, which is also returned for chaining. Similar
935: * to this *= q.
936: * 四元数与四元数相乘
937: *
938: * @param qx
939: * - quat x value
940: * @param qy
941: * - quat y value
942: * @param qz
943: * - quat z value
944: * @param qw
945: * - quat w value
946: *
947: * @return This Quaternion, after multiplication.
948: */
949: public Quaternion multLocal(float qx, float qy, float qz, float qw) {
950: float x1 = x * qw + y * qz - z * qy + w * qx;
951: float y1 = -x * qz + y * qw + z * qx + w * qy;
952: float z1 = x * qy - y * qx + z * qw + w * qz;
953: w = -x * qx - y * qy - z * qz + w * qw;
954: x = x1;
955: y = y1;
956: z = z1;
957: return this;
958: }
959:
960: /**
961: *multmultiplies this quaternion by a parameter vector. The
962: * result is returned as a new vector. 'this' is not modified.
963: * 四元数与向量相乘
964: *
965: * @param v
966: * the vector to multiply this quaternion by.
967: * @param store
968: * the vector to store the result in. It IS safe for v and store
969: * to be the same object.
970: * @return the result vector.
971: */
972: public Vector3f mult(Vector3f v, Vector3f store) {
973: if (store == null)
974: store = new Vector3f();
975: if (v.x == 0 && v.y == 0 && v.z == 0) {
976: store.set(0, 0, 0);
977: } else {
978: float vx = v.x, vy = v.y, vz = v.z;
979: store.x = w * w * vx + 2 * y * w * vz - 2 * z * w * vy + x * x * vx
980: + 2 * y * x * vy + 2 * z * x * vz - z * z * vx - y * y * vx;
981: store.y = 2 * x * y * vx + y * y * vy + 2 * z * y * vz + 2 * w * z
982: * vx - z * z * vy + w * w * vy - 2 * x * w * vz - x * x
983: * vy;
984: store.z = 2 * x * z * vx + 2 * y * z * vy + z * z * vz - 2 * w * y
985: * vx - y * y * vz + 2 * w * x * vy - x * x * vz + w * w
986: * vz;
987: }
988: return store;
989: }
990:
991: /**
992: *multmultiplies this quaternion by a parameter scalar. The
993: * result is returned as a new quaternion.
994: * 标量乘
995: *
996: * @param scalar
997: * the quaternion to multiply this quaternion by.
998: * @return the new quaternion.
999: */
1000: public Quaternion mult(float scalar) {
1001: return new Quaternion(scalar * x, scalar * y, scalar * z, scalar * w);
1002: }
1003:
1004: /**
1005: *multmultiplies this quaternion by a parameter scalar. The
1006: * result is stored locally.
1007: * 标量乘
1008: *
1009: * @param scalar
1010: * the quaternion to multiply this quaternion by.
1011: * @return this.
1012: */
1013: public Quaternion multLocal(float scalar) {
1014: w *= scalar;
1015: x *= scalar;
1016: y *= scalar;
1017: z *= scalar;
1018: return this;
1019: }
1020:
1021: /**
1022: *dotcalculates and returns the dot product of this
1023: * quaternion with that of the parameter quaternion.
1024: * 点乘
1025: *
1026: * @param q
1027: * the quaternion to calculate the dot product of.
1028: * @return the dot product of this and the parameter quaternion.
1029: */
1030: public float dot(Quaternion q) {
1031: return w * q.w + x * q.x + y * q.y + z * q.z;
1032: }
1033:
1034: /**
1035: *normreturns the norm of this quaternion. This is the dot
1036: * product of this quaternion with itself.
1037: * 模的平方（点乘自身）
1038: *
1039: * @return the norm of the quaternion.
1040: */
1041: public float norm() {
1042: return w * w + x * x + y * y + z * z;
1043: }
1044:
1045: /**
1046: *normalizenormalizes the currentQuaternion
1047: * 规格化
1048: */
1049: public void normalize() {
1050: float n = FastMath.invSqrt(norm());
1051: x *= n;
1052: y *= n;
1053: z *= n;
1054: w *= n;
1055: }
1056:
1057: /**
1058: *inversereturns the inverse of this quaternion as a new
1059: * quaternion. If this quaternion does not have an inverse (if its normal is
1060: * 0 or less), then null is returned.
1061: * 四元数的逆
1062: *
1063: * @return the inverse of this quaternion or null if the inverse does not
1064: * exist.
1065: */
1066: public Quaternion inverse() {
1067: float norm = norm();
1068: if (norm > 0.0) {
1069: float invNorm = 1.0f / norm;
1070: return new Quaternion(-x * invNorm, -y * invNorm, -z * invNorm, w
1071: * invNorm);
1072: }
1073: // return an invalid result to flag the error
1074: return null;
1075: }
1076:
1077: /**
1078: *inversecalculates the inverse of this quaternion and
1079: * returns this quaternion after it is calculated. If this quaternion does
1080: * not have an inverse (if it's norma is 0 or less), nothing happens
1081: * 四元数的逆
1082: *
1083: * @return the inverse of this quaternion
1084: */
1085: public Quaternion inverseLocal() {
1086: float norm = norm();
1087: if (norm > 0.0) {
1088: float invNorm = 1.0f / norm;
1089: x *= -invNorm;
1090: y *= -invNorm;
1091: z *= -invNorm;
1092: w *= invNorm;
1093: }
1094: return this;
1095: }
1096:
1097: /**
1098: *negateinverts the values of the quaternion.
1099: * 负四元数
1100: *
1101: */
1102: public void negate() {
1103: x *= -1;
1104: y *= -1;
1105: z *= -1;
1106: w *= -1;
1107: }
1108:
1109: /**
1110: *
1111: *toStringcreates the string representation of this
1112: *Quaternion. The values of the quaternion are displace (x, y,
1113: * z, w), in the following manner:
1114: * com.jme.math.Quaternion: [x=1" y=2 z=3 w=1]
1115: *
1116: * @return the string representation of this object.
1117: * @see java.lang.Object#toString()
1118: */
1119: public String toString() {
1120: return Quaternion.class.getName() + ": [x=" + x + " y=" + y + " z=" + z
1121: + " w=" + w + "]";
1122: }
1123:
1124: /**
1125: *equalsdetermines if two quaternions are logically equal,
1126: * that is, if the values of (x, y, z, w) are the same for both quaternions.
1127: *
1128: * @param o
1129: * the object to compare for equality
1130: * @return true if they are equal, false otherwise.
1131: */
1132: public boolean equals(Object o) {
1133: if (!(o instanceof Quaternion)) {
1134: return false;
1135: }
1136:
1137: if (this == o) {
1138: return true;
1139: }
1140:
1141: Quaternion comp = (Quaternion) o;
1142: if (Float.compare(x, comp.x) != 0)
1143: return false;
1144: if (Float.compare(y, comp.y) != 0)
1145: return false;
1146: if (Float.compare(z, comp.z) != 0)
1147: return false;
1148: if (Float.compare(w, comp.w) != 0)
1149: return false;
1150: return true;
1151: }
1152:
1153: /**
1154: *
1155: *hashCodereturns the hash code value as an integer and is
1156: * supported for the benefit of hashing based collection classes such as
1157: * Hashtable, HashMap, HashSet etc.
1158: *
1159: * @return the hashcode for this instance of Quaternion.
1160: * @see java.lang.Object#hashCode()
1161: */
1162: public int hashCode() {
1163: int hash = 37;
1164: hash = 37 * hash + Float.floatToIntBits(x);
1165: hash = 37 * hash + Float.floatToIntBits(y);
1166: hash = 37 * hash + Float.floatToIntBits(z);
1167: hash = 37 * hash + Float.floatToIntBits(w);
1168: return hash;
1169:
1170: }
1171:
1172: /**
1173: *readExternalbuilds a quaternion from an
1174: *ObjectInputobject.
1175: * NOTE: Used with serialization. Not to be called manually.
1176: *
1177: * @param in
1178: * the ObjectInput value to read from.
1179: * @throws IOException
1180: * if the ObjectInput value has problems reading a float.
1181: * @see java.io.Externalizable
1182: */
1183: public void readExternal(ObjectInput in) throws IOException {
1184: x = in.readFloat();
1185: y = in.readFloat();
1186: z = in.readFloat();
1187: w = in.readFloat();
1188: }
1189:
1190: /**
1191: *writeExternalwrites this quaternion out to a
1192: *ObjectOutputobject. NOTE: Used with serialization. Not to
1193: * be called manually.
1194: *
1195: * @param out
1196: * the object to write to.
1197: * @throws IOException
1198: * if writing to the ObjectOutput fails.
1199: * @see java.io.Externalizable
1200: */
1201: public void writeExternal(ObjectOutput out) throws IOException {
1202: out.writeFloat(x);
1203: out.writeFloat(y);
1204: out.writeFloat(z);
1205: out.writeFloat(w);
1206: }
1207:
1208: private static final Vector3f tmpYaxis = new Vector3f();
1209: private static final Vector3f tmpZaxis = new Vector3f();
1210: private static final Vector3f tmpXaxis = new Vector3f();
1211:
1212: /**
1213: *lookAtis a convienence method for auto-setting the
1214: * quaternion based on a direction and an up vector. It computes the
1215: * rotation to transform the z-axis to point into 'direction' and the y-axis
1216: * to 'up'.
1217: *
1218: * @param direction
1219: * where to look at in terms of local coordinates
1220: * @param up
1221: * a vector indicating the local up direction. (typically {0, 1,
1222: * 0} in jME.)
1223: */
1224: public void lookAt(Vector3f direction, Vector3f up) {
1225: tmpZaxis.set(direction).normalizeLocal();
1226: tmpXaxis.set(up).crossLocal(direction).normalizeLocal();
1227: tmpYaxis.set(direction).crossLocal(tmpXaxis).normalizeLocal();
1228: fromAxes(tmpXaxis, tmpYaxis, tmpZaxis);
1229: }
1230:
1231: public void write(JMEExporter e) throws IOException {
1232: OutputCapsule cap = e.getCapsule(this);
1233: cap.write(x, "x", 0);
1234: cap.write(y, "y", 0);
1235: cap.write(z, "z", 0);
1236: cap.write(w, "w", 1);
1237: }
1238:
1239: public void read(JMEImporter e) throws IOException {
1240: InputCapsule cap = e.getCapsule(this);
1241: x = cap.readFloat("x", 0);
1242: y = cap.readFloat("y", 0);
1243: z = cap.readFloat("z", 0);
1244: w = cap.readFloat("w", 1);
1245: }
1246:
1247: public Class extends Quaternion> getClassTag() {
1248: return this.getClass();
1249: }
1250:
1251: /**
1252: * @return A new quaternion that describes a rotation that would point you
1253: * in the exact opposite direction of this Quaternion.
1254: * 反转
1255: */
1256: public Quaternion opposite() {
1257: return opposite(null);
1258: }
1259:
1260: /**
1261: * FIXME: This seems to have singularity type issues with angle == 0,
1262: * possibly others such as PI.
1263: * 反转
1264: *
1265: * @param store
1266: * A Quaternion to store our result in. If null, a new one is
1267: * created.
1268: * @return The store quaternion (or a new Quaterion, if store is null) that
1269: * describes a rotation that would point you in the exact opposite
1270: * direction of this Quaternion.
1271: */
1272: public Quaternion opposite(Quaternion store) {
1273: if (store == null)
1274: store = new Quaternion();
1275:
1276: Vector3f axis = new Vector3f();
1277: float angle = toAngleAxis(axis);
1278:
1279: store.fromAngleAxis(FastMath.PI + angle, axis);
1280: return store;
1281: }
1282:
1283: /**
1284: * @return This Quaternion, altered to describe a rotation that would point
1285: * you in the exact opposite direction of where it is pointing
1286: * currently.
1287: */
1288: public Quaternion oppositeLocal() {
1289: return opposite(this);
1290: }
1291:
1292: @Override
1293: public Quaternion clone() {
1294: try {
1295: return (Quaternion) super.clone();
1296: } catch (CloneNotSupportedException e) {
1297: throw new AssertionError(); // can not happen
1298: }
1299: }
1300:
1301: public float getX() {
1302: return x;
1303: }
1304:
1305: public void setX(float x) {
1306: this.x = x;
1307: }
1308:
1309: public float getY() {
1310: return y;
1311: }
1312:
1313: public void setY(float y) {
1314: this.y = y;
1315: }
1316:
1317: public float getZ() {
1318: return z;
1319: }
1320:
1321: public void setZ(float z) {
1322: this.z = z;
1323: }
1324:
1325: public float getW() {
1326: return w;
1327: }
1328:
1329: public void setW(float w) {
1330: this.w = w;
1331: }
1332: }
![clip_image048[9]](https://p-blog.csdn.net/images/p_blog_csdn_net/xiefeifeihu/596698/o_clip_image048%5B9%5D_633933854444531250.jpg "clip_image048[9]")
1: public Matrix3f toRotationMatrix(Matrix3f result) {
2:
3: float norm = norm();
4: // we explicitly test norm against one here, saving a division
5: // at the cost of a test and branch. Is it worth it?
6: float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0;
7:
8: // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
9: // will be used 2-4 times each.
10: float xs = x * s;
11: float ys = y * s;
12: float zs = z * s;
13: float xx = x * xs;
14: float xy = x * ys;
15: float xz = x * zs;
16: float xw = w * xs;
17: float yy = y * ys;
18: float yz = y * zs;
19: float yw = w * ys;
20: float zz = z * zs;
21: float zw = w * zs;
22:
23: // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
24: result.m00 = 1 - (yy + zz);
25: result.m01 = (xy - zw);
26: result.m02 = (xz + yw);
27: result.m10 = (xy + zw);
28: result.m11 = 1 - (xx + zz);
29: result.m12 = (yz - xw);
30: result.m20 = (xz - yw);
31: result.m21 = (yz + xw);
32: result.m22 = 1 - (xx + yy);
33:
34: return result;
35: }
1: /**
2: *getRotationColumnreturns one of three columns specified by
3: * the parameter. This column is returned as aVector3fobject.
4: * The value is retrieved as if this quaternion was first normalized.
5: * 四元数转换到矩阵，获取指定的列构造的向量
6: *
7: * @param i
8: * the column to retrieve. Must be between 0 and 2.
9: * @param store
10: * the vector object to store the result in. if null, a new one
11: * is created.
12: * @return the column specified by the index.
13: */
14: public Vector3f getRotationColumn(int i, Vector3f store) {
15: if (store == null)
16: store = new Vector3f();
17:
18: float norm = norm();
19: if (norm != 1.0f) {
20: norm = FastMath.invSqrt(norm);
21: }
22:
23: float xx = x * x * norm;
24: float xy = x * y * norm;
25: float xz = x * z * norm;
26: float xw = x * w * norm;
27: float yy = y * y * norm;
28: float yz = y * z * norm;
29: float yw = y * w * norm;
30: float zz = z * z * norm;
31: float zw = z * w * norm;
32:
33: switch (i) {
34: case 0:
35: store.x = 1 - 2 * (yy + zz);
36: store.y = 2 * (xy + zw);
37: store.z = 2 * (xz - yw);
38: break;
39: case 1:
40: store.x = 2 * (xy - zw);
41: store.y = 1 - 2 * (xx + zz);
42: store.z = 2 * (yz + xw);
43: break;
44: case 2:
45: store.x = 2 * (xz + yw);
46: store.y = 2 * (yz - xw);
47: store.z = 1 - 2 * (xx + yy);
48: break;
49: default:
50: logger.warning("Invalid column index.");
51: throw new JmeException("Invalid column index. " + i);
52: }
53:
54: return store;
55: }
四元数的逆：
![clip_image041[9]](https://p-blog.csdn.net/images/p_blog_csdn_net/xiefeifeihu/596698/o_clip_image041%5B9%5D_633933854366093750.jpg "clip_image041[9]")
![clip_image042[9]](https://p-blog.csdn.net/images/p_blog_csdn_net/xiefeifeihu/596698/o_clip_image042%5B9%5D_633933854374218750.jpg "clip_image042[9]")
1: public Quaternion inverse() {
2: float norm = norm();
3: if (norm > 0.0) {
4: float invNorm = 1.0f / norm;
5: return new Quaternion(-x * invNorm, -y * invNorm, -z * invNorm, w
6: * invNorm);
7: }
8: // return an invalid result to flag the error
9: return null;
10: }
![clip_image043[9]](https://p-blog.csdn.net/images/p_blog_csdn_net/xiefeifeihu/596698/o_clip_image043%5B9%5D_633933854386718750.jpg "clip_image043[9]")
1: public Quaternion multLocal(Quaternion q) {
2: float x1 = x * q.w + y * q.z - z * q.y + w * q.x;
3: float y1 = -x * q.z + y * q.w + z * q.x + w * q.y;
4: float z1 = x * q.y - y * q.x + z * q.w + w * q.z;
5: w = -x * q.x - y * q.y - z * q.z + w * q.w;
6: x = x1;
7: y = y1;
8: z = z1;
9: return this;
10: }
![clip_image046[9]](https://p-blog.csdn.net/images/p_blog_csdn_net/xiefeifeihu/596698/o_clip_image046%5B9%5D_633933854423281250.jpg "clip_image046[9]")
1: public Quaternion slerp(Quaternion q1, Quaternion q2, float t) {
2: // Create a local quaternion to store the interpolated quaternion
3: if (q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w) {
4: this.set(q1);
5: return this;
6: }
7:
8: float result = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z)
9: + (q1.w * q2.w);
10:
11: if (result < 0.0f) {
12: // Negate the second quaternion and the result of the dot product
13: q2.x = -q2.x;
14: q2.y = -q2.y;
15: q2.z = -q2.z;
16: q2.w = -q2.w;
17: result = -result;
18: }
19:
20: // Set the first and second scale for the interpolation
21: float scale0 = 1 - t;
22: float scale1 = t;
23:
24: // Check if the angle between the 2 quaternions was big enough to
25: // warrant such calculations
26: if ((1 - result) > 0.1f) {// Get the angle between the 2 quaternions,
27: // and then store the sin() of that angle
28: float theta = FastMath.acos(result);
29: float invSinTheta = 1f / FastMath.sin(theta);
30:
31: // Calculate the scale for q1 and q2, according to the angle and
32: // it's sine value
33: scale0 = FastMath.sin((1 - t) * theta) * invSinTheta;
34: scale1 = FastMath.sin((t * theta)) * invSinTheta;
35: }
36:
37: // Calculate the x, y, z and w values for the quaternion by using a
38: // special
39: // form of linear interpolation for quaternions.
40: this.x = (scale0 * q1.x) + (scale1 * q2.x);
41: this.y = (scale0 * q1.y) + (scale1 * q2.y);
42: this.z = (scale0 * q1.z) + (scale1 * q2.z);
43: this.w = (scale0 * q1.w) + (scale1 * q2.w);
44:
45: // Return the interpolated quaternion
46: return this;
47: }
22万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
