1 /**
  2  Copyright (c) 2010-2012 cocos2d-x.org
  3  Copyright (c) 2008, Luke Benstead.
  4  All rights reserved.
  5 
  6  Redistribution and use in source and binary forms, with or without modification,
  7  are permitted provided that the following conditions are met:
  8 
  9  Redistributions of source code must retain the above copyright notice,
 10  this list of conditions and the following disclaimer.
 11  Redistributions in binary form must reproduce the above copyright notice,
 12  this list of conditions and the following disclaimer in the documentation
 13  and/or other materials provided with the distribution.
 14 
 15  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 16  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 17  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 18  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 19  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 20  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 21  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 22  ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 23  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 24  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 25  */
 26 
 27 /**
 28  * The Quaternion class
 29  * @param {Number} x
 30  * @param {Number} y
 31  * @param {Number} z
 32  * @param {Number} w
 33  * @constructor
 34  */
 35 cc.kmQuaternion = function (x, y, z, w) {
 36     this.x = x || 0;
 37     this.y = y || 0;
 38     this.z = z || 0;
 39     this.w = w || 0;
 40 };
 41 
 42 ///< Returns pOut, sets pOut to the conjugate of pIn
 43 cc.kmQuaternionConjugate = function (pOut, pIn) {
 44     pOut.x = -pIn.x;
 45     pOut.y = -pIn.y;
 46     pOut.z = -pIn.z;
 47     pOut.w = pIn.w;
 48 
 49     return pOut;
 50 };
 51 
 52 ///< Returns the dot product of the 2 quaternions
 53 cc.kmQuaternionDot = function (q1, q2) {
 54     // A dot B = B dot A = AtBt + AxBx + AyBy + AzBz
 55     return (q1.w * q2.w +
 56         q1.x * q2.x +
 57         q1.y * q2.y +
 58         q1.z * q2.z);
 59 };
 60 
 61 ///< Returns the exponential of the quaternion
 62 cc.kmQuaternionExp = function (pOut, pIn) {
 63     //TODO not implement
 64     //cc.assert(0);
 65     return pOut;
 66 };
 67 
 68 ///< Makes the passed quaternion an identity quaternion
 69 cc.kmQuaternionIdentity = function (pOut) {
 70     pOut.x = 0.0;
 71     pOut.y = 0.0;
 72     pOut.z = 0.0;
 73     pOut.w = 1.0;
 74 
 75     return pOut;
 76 };
 77 
 78 ///< Returns the inverse of the passed Quaternion
 79 cc.kmQuaternionInverse = function (pOut, pIn) {
 80     var l = cc.kmQuaternionLength(pIn);
 81     var tmp = new cc.kmQuaternion();
 82 
 83     if (Math.abs(l) > cc.kmEpsilon) {
 84         pOut.x = 0.0;
 85         pOut.y = 0.0;
 86         pOut.z = 0.0;
 87         pOut.w = 0.0;
 88         return pOut;
 89     }
 90 
 91     ///Get the conjugute and divide by the length
 92     cc.kmQuaternionScale(pOut,
 93         cc.kmQuaternionConjugate(tmp, pIn), 1.0 / l);
 94 
 95     return pOut;
 96 };
 97 
 98 ///< Returns true if the quaternion is an identity quaternion
 99 cc.kmQuaternionIsIdentity = function (pIn) {
100     return (pIn.x == 0.0 && pIn.y == 0.0 && pIn.z == 0.0 &&
101         pIn.w == 1.0);
102 };
103 
104 ///< Returns the length of the quaternion
105 cc.kmQuaternionLength = function (pIn) {
106     return Math.sqrt(cc.kmQuaternionLengthSq(pIn));
107 };
108 
109 ///< Returns the length of the quaternion squared (prevents a sqrt)
110 cc.kmQuaternionLengthSq = function (pIn) {
111     return pIn.x * pIn.x + pIn.y * pIn.y +
112         pIn.z * pIn.z + pIn.w * pIn.w;
113 };
114 
115 ///< Returns the natural logarithm
116 cc.kmQuaternionLn = function (pOut, pIn) {
117     /*
118      A unit quaternion, is defined by:
119      Q == (cos(theta), sin(theta) * v) where |v| = 1
120      The natural logarithm of Q is, ln(Q) = (0, theta * v)
121      */
122     //assert(0);
123     //TODO not implement
124     return pOut;
125 };
126 
127 ///< Multiplies 2 quaternions together
128 cc.kmQuaternionMultiply = function (pOut, q1, q2) {
129     pOut.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
130     pOut.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
131     pOut.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
132     pOut.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
133 
134     return pOut;
135 };
136 
137 ///< Normalizes a quaternion
138 cc.kmQuaternionNormalize = function (pOut, pIn) {
139     var length = cc.kmQuaternionLength(pIn);
140     if(Math.abs(length) <= cc.kmEpsilon)
141         throw "cc.kmQuaternionNormalize(): pIn is an invalid value";
142     cc.kmQuaternionScale(pOut, pIn, 1.0 / length);
143 
144     return pOut;
145 };
146 
147 ///< Rotates a quaternion around an axis
148 cc.kmQuaternionRotationAxis = function (pOut, pV, angle) {
149     var rad = angle * 0.5;
150     var scale = Math.sin(rad);
151 
152     pOut.w = Math.cos(rad);
153     pOut.x = pV.x * scale;
154     pOut.y = pV.y * scale;
155     pOut.z = pV.z * scale;
156 
157     return pOut;
158 };
159 
160 ///< Creates a quaternion from a rotation matrix
161 cc.kmQuaternionRotationMatrix = function (pOut, pIn) {
162     /*
163      Note: The OpenGL matrices are transposed from the description below
164      taken from the Matrix and Quaternion FAQ
165 
166      if ( mat[0] > mat[5] && mat[0] > mat[10] )  {    // Column 0:
167      S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
168      X = 0.25 * S;
169      Y = (mat[4] + mat[1] ) / S;
170      Z = (mat[2] + mat[8] ) / S;
171      W = (mat[9] - mat[6] ) / S;
172      } else if ( mat[5] > mat[10] ) {            // Column 1:
173      S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
174      X = (mat[4] + mat[1] ) / S;
175      Y = 0.25 * S;
176      Z = (mat[9] + mat[6] ) / S;
177      W = (mat[2] - mat[8] ) / S;
178      } else {                        // Column 2:
179      S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
180      X = (mat[2] + mat[8] ) / S;
181      Y = (mat[9] + mat[6] ) / S;
182      Z = 0.25 * S;
183      W = (mat[4] - mat[1] ) / S;
184      }
185      */
186     var x, y, z, w;
187     var m4x4 = [];
188     var scale = 0.0;
189     var diagonal = 0.0;
190 
191     if (!pIn) {
192         return null;
193     }
194 
195     /*    0 3 6
196      1 4 7
197      2 5 8
198 
199      0 1 2 3
200      4 5 6 7
201      8 9 10 11
202      12 13 14 15*/
203 
204     m4x4[0] = pIn.mat[0];
205     m4x4[1] = pIn.mat[3];
206     m4x4[2] = pIn.mat[6];
207     m4x4[4] = pIn.mat[1];
208     m4x4[5] = pIn.mat[4];
209     m4x4[6] = pIn.mat[7];
210     m4x4[8] = pIn.mat[2];
211     m4x4[9] = pIn.mat[5];
212     m4x4[10] = pIn.mat[8];
213     m4x4[15] = 1;
214     var pMatrix = m4x4[0];
215 
216     diagonal = pMatrix[0] + pMatrix[5] + pMatrix[10] + 1;
217 
218     if (diagonal > cc.kmEpsilon) {
219         // Calculate the scale of the diagonal
220         scale = Math.sqrt(diagonal) * 2;
221 
222         // Calculate the x, y, x and w of the quaternion through the respective equation
223         x = ( pMatrix[9] - pMatrix[6] ) / scale;
224         y = ( pMatrix[2] - pMatrix[8] ) / scale;
225         z = ( pMatrix[4] - pMatrix[1] ) / scale;
226         w = 0.25 * scale;
227     } else {
228         // If the first element of the diagonal is the greatest value
229         if (pMatrix[0] > pMatrix[5] && pMatrix[0] > pMatrix[10]) {
230             // Find the scale according to the first element, and double that value
231             scale = Math.sqrt(1.0 + pMatrix[0] - pMatrix[5] - pMatrix[10]) * 2.0;
232 
233             // Calculate the x, y, x and w of the quaternion through the respective equation
234             x = 0.25 * scale;
235             y = (pMatrix[4] + pMatrix[1] ) / scale;
236             z = (pMatrix[2] + pMatrix[8] ) / scale;
237             w = (pMatrix[9] - pMatrix[6] ) / scale;
238         }
239         // Else if the second element of the diagonal is the greatest value
240         else if (pMatrix[5] > pMatrix[10]) {
241             // Find the scale according to the second element, and double that value
242             scale = Math.sqrt(1.0 + pMatrix[5] - pMatrix[0] - pMatrix[10]) * 2.0;
243 
244             // Calculate the x, y, x and w of the quaternion through the respective equation
245             x = (pMatrix[4] + pMatrix[1] ) / scale;
246             y = 0.25 * scale;
247             z = (pMatrix[9] + pMatrix[6] ) / scale;
248             w = (pMatrix[2] - pMatrix[8] ) / scale;
249         } else {
250             // Else the third element of the diagonal is the greatest value
251 
252             // Find the scale according to the third element, and double that value
253             scale = Math.sqrt(1.0 + pMatrix[10] - pMatrix[0] - pMatrix[5]) * 2.0;
254 
255             // Calculate the x, y, x and w of the quaternion through the respective equation
256             x = (pMatrix[2] + pMatrix[8] ) / scale;
257             y = (pMatrix[9] + pMatrix[6] ) / scale;
258             z = 0.25 * scale;
259             w = (pMatrix[4] - pMatrix[1] ) / scale;
260         }
261     }
262 
263     pOut.x = x;
264     pOut.y = y;
265     pOut.z = z;
266     pOut.w = w;
267 
268     return pOut;
269 };
270 
271 ///< Create a quaternion from yaw, pitch and roll
272 cc.kmQuaternionRotationYawPitchRoll = function (pOut, yaw, pitch, roll) {
273     var ex, ey, ez;        // temp half euler angles
274     var cr, cp, cy, sr, sp, sy, cpcy, spsy;        // temp vars in roll,pitch yaw
275 
276     ex = cc.kmDegreesToRadians(pitch) / 2.0;    // convert to rads and half them
277     ey = cc.kmDegreesToRadians(yaw) / 2.0;
278     ez = cc.kmDegreesToRadians(roll) / 2.0;
279 
280     cr = Math.cos(ex);
281     cp = Math.cos(ey);
282     cy = Math.cos(ez);
283 
284     sr = Math.sin(ex);
285     sp = Math.sin(ey);
286     sy = Math.sin(ez);
287 
288     cpcy = cp * cy;
289     spsy = sp * sy;
290 
291     pOut.w = cr * cpcy + sr * spsy;
292 
293     pOut.x = sr * cpcy - cr * spsy;
294     pOut.y = cr * sp * cy + sr * cp * sy;
295     pOut.z = cr * cp * sy - sr * sp * cy;
296 
297     cc.kmQuaternionNormalize(pOut, pOut);
298 
299     return pOut;
300 };
301 
302 ///< Interpolate between 2 quaternions
303 cc.kmQuaternionSlerp = function (pOut, q1, q2, t) {
304     /*float CosTheta = Q0.DotProd(Q1);
305      float Theta = acosf(CosTheta);
306      float SinTheta = sqrtf(1.0f-CosTheta*CosTheta);
307 
308      float Sin_T_Theta = sinf(T*Theta)/SinTheta;
309      float Sin_OneMinusT_Theta = sinf((1.0f-T)*Theta)/SinTheta;
310 
311      Quaternion Result = Q0*Sin_OneMinusT_Theta;
312      Result += (Q1*Sin_T_Theta);
313 
314      return Result;*/
315 
316     if (q1.x == q2.x &&
317         q1.y == q2.y &&
318         q1.z == q2.z &&
319         q1.w == q2.w) {
320 
321         pOut.x = q1.x;
322         pOut.y = q1.y;
323         pOut.z = q1.z;
324         pOut.w = q1.w;
325 
326         return pOut;
327     }
328 
329     var ct = cc.kmQuaternionDot(q1, q2);
330     var theta = Math.acos(ct);
331     var st = Math.sqrt(1.0 - cc.kmSQR(ct));
332 
333     var stt = Math.sin(t * theta) / st;
334     var somt = Math.sin((1.0 - t) * theta) / st;
335 
336     var temp = new cc.kmQuaternion(), temp2 = new cc.kmQuaternion();
337     cc.kmQuaternionScale(temp, q1, somt);
338     cc.kmQuaternionScale(temp2, q2, stt);
339     cc.kmQuaternionAdd(pOut, temp, temp2);
340 
341     return pOut;
342 };
343 
344 ///< Get the axis and angle of rotation from a quaternion
345 cc.kmQuaternionToAxisAngle = function (pIn, pAxis, pAngle) {
346     var tempAngle;        // temp angle
347     var scale;            // temp vars
348 
349     tempAngle = Math.acos(pIn.w);
350     scale = Math.sqrt(cc.kmSQR(pIn.x) + cc.kmSQR(pIn.y) + cc.kmSQR(pIn.z));
351 
352     if (((scale > -cc.kmEpsilon) && scale < cc.kmEpsilon)
353         || (scale < 2 * cc.kmPI + cc.kmEpsilon && scale > 2 * cc.kmPI - cc.kmEpsilon)) {       // angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0
354         pAngle = 0.0;
355 
356         pAxis.x = 0.0;
357         pAxis.y = 0.0;
358         pAxis.z = 1.0;
359     } else {
360         pAngle = tempAngle * 2.0;        // angle in radians
361 
362         pAxis.x = pIn.x / scale;
363         pAxis.y = pIn.y / scale;
364         pAxis.z = pIn.z / scale;
365         cc.kmVec3Normalize(pAxis, pAxis);
366     }
367 };
368 
369 ///< Scale a quaternion
370 cc.kmQuaternionScale = function (pOut, pIn, s) {
371     pOut.x = pIn.x * s;
372     pOut.y = pIn.y * s;
373     pOut.z = pIn.z * s;
374     pOut.w = pIn.w * s;
375 
376     return pOut;
377 };
378 
379 cc.kmQuaternionAssign = function (pOut, pIn) {
380     pOut.x = pIn.x;
381     pOut.y = pIn.y;
382     pOut.z = pIn.z;
383     pOut.w = pIn.w;
384 
385     return pOut;
386 };
387 
388 cc.kmQuaternionAdd = function (pOut, pQ1, pQ2) {
389     pOut.x = pQ1.x + pQ2.x;
390     pOut.y = pQ1.y + pQ2.y;
391     pOut.z = pQ1.z + pQ2.z;
392     pOut.w = pQ1.w + pQ2.w;
393 
394     return pOut;
395 };
396 
397 /** Adapted from the OGRE engine!
398 
399  Gets the shortest arc quaternion to rotate this vector to the destination
400  vector.
401  @remarks
402  If you call this with a dest vector that is close to the inverse
403  of this vector, we will rotate 180 degrees around the 'fallbackAxis'
404  (if specified, or a generated axis if not) since in this case
405  ANY axis of rotation is valid.
406  */
407 cc.kmQuaternionRotationBetweenVec3 = function (pOut, vec1, vec2, fallback) {
408     var v1 = new cc.kmVec3(), v2 = new cc.kmVec3();
409     var a;
410 
411     cc.kmVec3Assign(v1, vec1);
412     cc.kmVec3Assign(v2, vec2);
413 
414     cc.kmVec3Normalize(v1, v1);
415     cc.kmVec3Normalize(v2, v2);
416 
417     a = cc.kmVec3Dot(v1, v2);
418 
419     if (a >= 1.0) {
420         cc.kmQuaternionIdentity(pOut);
421         return pOut;
422     }
423 
424     if (a < (1e-6 - 1.0)) {
425         if (Math.abs(cc.kmVec3LengthSq(fallback)) < cc.kmEpsilon) {
426             cc.kmQuaternionRotationAxis(pOut, fallback, cc.kmPI);
427         } else {
428             var axis = new cc.kmVec3();
429             var X = new cc.kmVec3();
430             X.x = 1.0;
431             X.y = 0.0;
432             X.z = 0.0;
433 
434             cc.kmVec3Cross(axis, X, vec1);
435 
436             //If axis is zero
437             if (Math.abs(cc.kmVec3LengthSq(axis)) < cc.kmEpsilon) {
438                 var Y = new cc.kmVec3();
439                 Y.x = 0.0;
440                 Y.y = 1.0;
441                 Y.z = 0.0;
442 
443                 cc.kmVec3Cross(axis, Y, vec1);
444             }
445 
446             cc.kmVec3Normalize(axis, axis);
447             cc.kmQuaternionRotationAxis(pOut, axis, cc.kmPI);
448         }
449     } else {
450         var s = Math.sqrt((1 + a) * 2);
451         var invs = 1 / s;
452 
453         var c = new cc.kmVec3();
454         cc.kmVec3Cross(c, v1, v2);
455 
456         pOut.x = c.x * invs;
457         pOut.y = c.y * invs;
458         pOut.z = c.z * invs;
459         pOut.w = s * 0.5;
460 
461         cc.kmQuaternionNormalize(pOut, pOut);
462     }
463     return pOut;
464 };
465 
466 cc.kmQuaternionMultiplyVec3 = function (pOut, q, v) {
467     var uv = new cc.kmVec3(), uuv = new cc.kmVec3(), qvec = new cc.kmVec3();
468 
469     qvec.x = q.x;
470     qvec.y = q.y;
471     qvec.z = q.z;
472 
473     cc.kmVec3Cross(uv, qvec, v);
474     cc.kmVec3Cross(uuv, qvec, uv);
475 
476     cc.kmVec3Scale(uv, uv, (2.0 * q.w));
477     cc.kmVec3Scale(uuv, uuv, 2.0);
478 
479     cc.kmVec3Add(pOut, v, uv);
480     cc.kmVec3Add(pOut, pOut, uuv);
481 
482     return pOut;
483 };
484 
485 
486 
487 
488 
489 
490 
491 
492 
493 
494 
495 
496