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