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