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