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