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