FBX SDK Reference Guide: kmath.h Source File
00001 #ifndef FBXFILESDK_COMPONENTS_KBASELIB_KMATH_KMATH_H 00002 #define FBXFILESDK_COMPONENTS_KBASELIB_KMATH_KMATH_H 00003 00004 /************************************************************************************** 00005 00006 Copyright © 2001 - 2008 Autodesk, Inc. and/or its licensors. 00007 All Rights Reserved. 00008 00009 The coded instructions, statements, computer programs, and/or related material 00010 (collectively the "Data") in these files contain unpublished information 00011 proprietary to Autodesk, Inc. and/or its licensors, which is protected by 00012 Canada and United States of America federal copyright law and by international 00013 treaties. 00014 00015 The Data may not be disclosed or distributed to third parties, in whole or in 00016 part, without the prior written consent of Autodesk, Inc. ("Autodesk"). 00017 00018 THE DATA IS PROVIDED "AS IS" AND WITHOUT WARRANTY. 00019 ALL WARRANTIES ARE EXPRESSLY EXCLUDED AND DISCLAIMED. AUTODESK MAKES NO 00020 WARRANTY OF ANY KIND WITH RESPECT TO THE DATA, EXPRESS, IMPLIED OR ARISING 00021 BY CUSTOM OR TRADE USAGE, AND DISCLAIMS ANY IMPLIED WARRANTIES OF TITLE, 00022 NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE OR USE. 00023 WITHOUT LIMITING THE FOREGOING, AUTODESK DOES NOT WARRANT THAT THE OPERATION 00024 OF THE DATA WILL BE UNINTERRUPTED OR ERROR FREE. 00025 00026 IN NO EVENT SHALL AUTODESK, ITS AFFILIATES, PARENT COMPANIES, LICENSORS 00027 OR SUPPLIERS ("AUTODESK GROUP") BE LIABLE FOR ANY LOSSES, DAMAGES OR EXPENSES 00028 OF ANY KIND (INCLUDING WITHOUT LIMITATION PUNITIVE OR MULTIPLE DAMAGES OR OTHER 00029 SPECIAL, DIRECT, INDIRECT, EXEMPLARY, INCIDENTAL, LOSS OF PROFITS, REVENUE 00030 OR DATA, COST OF COVER OR CONSEQUENTIAL LOSSES OR DAMAGES OF ANY KIND), 00031 HOWEVER CAUSED, AND REGARDLESS OF THE THEORY OF LIABILITY, WHETHER DERIVED 00032 FROM CONTRACT, TORT (INCLUDING, BUT NOT LIMITED TO, NEGLIGENCE), OR OTHERWISE, 00033 ARISING OUT OF OR RELATING TO THE DATA OR ITS USE OR ANY OTHER PERFORMANCE, 00034 WHETHER OR NOT AUTODESK HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH LOSS 00035 OR DAMAGE. 00036 00037 **************************************************************************************/ 00038 00039 #include <fbxfilesdk/components/kbaselib/kbaselib_h.h> 00040 00041 #include <string.h> // memcmp 00042 #include <fbxfilesdk/components/kbaselib/kmath/maths.h> 00043 00044 #define OGLFLOAT 1 00045 typedef float kOGLFloat; 00046 typedef kOGLFloat kOGLVector2[2]; 00047 typedef kOGLFloat kOGLVector3[3]; 00048 typedef kOGLFloat kOGLVector4[4]; 00049 00050 #define KM_TOLERANCE (1.0e-6) 00051 #define KM_EPSILON (1.0e-10) 00052 00053 #ifdef __cplusplus 00054 #define K_DEFAULT(arg, val) arg = val 00055 #else 00056 #define K_DEFAULT(arg, val) arg 00057 #endif 00058 00059 #ifndef KM_ASSERT 00060 //#define KM_ASSERT(cond, msg) assert(cond) 00061 #define KM_ASSERT(cond, msg) // K_ASSERT_MSG(cond,msg) 00062 #endif 00063 00064 // to remove potential side effects for in-place calls 00065 #ifndef KM_CONST 00066 #define KM_CONST const 00067 #endif 00068 00069 00070 #include <fbxfilesdk/fbxfilesdk_nsbegin.h> 00071 00072 /* 00073 * LEGEND 00074 * - N stands for scalar 00075 * - V stands for vector 00076 * - Q stands for quaternion 00077 * - M stands for matrix 00078 * - LU stands for LU matrix 00079 * - A stands for affine matrix (TRS) 00080 * - T stands for translation matrix 00081 * - R stands for rotation matrix 00082 * - S stands for scaling matrix 00083 * - P stands for projection matrix 00084 * - D stands for direction vector 00085 */ 00086 00087 /* 00088 * For KmVtoRord() and KmRtoVord() 00089 */ 00090 enum { 00091 KM_EULER_REPEAT_NO = 0, 00092 KM_EULER_REPEAT_YES = 1 00093 }; 00094 00095 enum { 00096 KM_EULER_PARITY_EVEN = 0, 00097 KM_EULER_PARITY_ODD = 2 00098 }; 00099 00100 enum { 00101 KM_EULER_AXIS_X = 0, 00102 KM_EULER_AXIS_Y = 1, 00103 KM_EULER_AXIS_Z = 2 00104 }; 00105 00106 #define KM_EULER_ORDER(Axis, Parity, Repeat) (((Axis) << 2) | (Parity) | (Repeat)) 00107 00108 enum { 00109 KM_EULER_XYZ = KM_EULER_ORDER(KM_EULER_AXIS_X, KM_EULER_PARITY_EVEN, KM_EULER_REPEAT_NO), 00110 KM_EULER_XYX = KM_EULER_ORDER(KM_EULER_AXIS_X, KM_EULER_PARITY_EVEN, KM_EULER_REPEAT_YES), 00111 KM_EULER_XZY = KM_EULER_ORDER(KM_EULER_AXIS_X, KM_EULER_PARITY_ODD, KM_EULER_REPEAT_NO), 00112 KM_EULER_XZX = KM_EULER_ORDER(KM_EULER_AXIS_X, KM_EULER_PARITY_ODD, KM_EULER_REPEAT_YES), 00113 KM_EULER_YZX = KM_EULER_ORDER(KM_EULER_AXIS_Y, KM_EULER_PARITY_EVEN, KM_EULER_REPEAT_NO), 00114 KM_EULER_YZY = KM_EULER_ORDER(KM_EULER_AXIS_Y, KM_EULER_PARITY_EVEN, KM_EULER_REPEAT_YES), 00115 KM_EULER_YXZ = KM_EULER_ORDER(KM_EULER_AXIS_Y, KM_EULER_PARITY_ODD, KM_EULER_REPEAT_NO), 00116 KM_EULER_YXY = KM_EULER_ORDER(KM_EULER_AXIS_Y, KM_EULER_PARITY_ODD, KM_EULER_REPEAT_YES), 00117 KM_EULER_ZXY = KM_EULER_ORDER(KM_EULER_AXIS_Z, KM_EULER_PARITY_EVEN, KM_EULER_REPEAT_NO), 00118 KM_EULER_ZXZ = KM_EULER_ORDER(KM_EULER_AXIS_Z, KM_EULER_PARITY_EVEN, KM_EULER_REPEAT_YES), 00119 KM_EULER_ZYX = KM_EULER_ORDER(KM_EULER_AXIS_Z, KM_EULER_PARITY_ODD, KM_EULER_REPEAT_NO), 00120 KM_EULER_ZYZ = KM_EULER_ORDER(KM_EULER_AXIS_Z, KM_EULER_PARITY_ODD, KM_EULER_REPEAT_YES) 00121 }; 00122 00123 00125 // 00126 // Constants 00127 // 00129 00130 extern KFBX_DLL const int KmEulerAxis[][3]; 00131 extern KFBX_DLL const double KmIdentity[4][4]; 00132 00134 // 00135 // Vector Utilities 00136 // 00138 00139 /* 00140 * KmVlength : Vector Length 00141 * 00142 * INPUT 00143 * pVi: input vector Vi 00144 * 00145 * RETURN VALUE 00146 * |Vi| 00147 */ 00148 K_INLINE double KmVlength(const double *const pVi) 00149 { 00150 return kNorm(pVi[0], pVi[1], pVi[2]); 00151 } 00152 00153 K_INLINE float Kmvlength(const kOGLFloat *const pVi) 00154 { 00155 return kNorm(pVi[0], pVi[1], pVi[2]); 00156 } 00157 00158 /* 00159 * KmVlength2 : Vector Squared Length 00160 * 00161 * INPUT 00162 * pVi: input vector Vi 00163 * 00164 * RETURN VALUE 00165 * |Vi|^2 00166 */ 00167 K_INLINE double KmVlength2(const double *const pVi) 00168 { 00169 double X, Y, Z; 00170 00171 X = pVi[0]; 00172 Y = pVi[1]; 00173 Z = pVi[2]; 00174 00175 return X * X + Y * Y + Z * Z; 00176 } 00177 00178 /* 00179 * KmVdist2 : Vector Squared Distance 00180 * 00181 * INPUT 00182 * pViA: input vector ViA 00183 * pViB: input vector ViB 00184 * 00185 * RETURN VALUE 00186 * | ViA - ViB |^2 00187 */ 00188 K_INLINE double KmVdist2(const double *const pViA, const double *const pViB) 00189 { 00190 double X, Y, Z; 00191 00192 X = pViA[0] - pViB[0]; 00193 Y = pViA[1] - pViB[1]; 00194 Z = pViA[2] - pViB[2]; 00195 00196 return X * X + Y * Y + Z * Z; 00197 } 00198 00199 /* 00200 * KmVdist : Vector Distance 00201 * 00202 * INPUT 00203 * pViA: input vector ViA 00204 * pViB: input vector ViB 00205 * 00206 * RETURN VALUE 00207 * | ViA - ViB | 00208 */ 00209 K_INLINE double KmVdist(const double *const pViA, const double *const pViB) 00210 { 00211 return kSqrt(KmVdist2(pViA, pViB)); 00212 } 00213 00214 /* 00215 * KmVdotV : Vector Inner Product 00216 * 00217 * INPUT 00218 * pViA: input vector ViA 00219 * pViB: input vector ViB 00220 * 00221 * RETURN VALUE 00222 * ViA . ViB 00223 */ 00224 K_INLINE double KmVdotV(const double *const pViA, const double *const pViB) 00225 { 00226 return pViA[0] * pViB[0] + pViA[1] * pViB[1] + pViA[2] * pViB[2]; 00227 } 00228 00229 K_INLINE float KmVdotV(const float *const pViA, const float *const pViB) 00230 { 00231 return pViA[0] * pViB[0] + pViA[1] * pViB[1] + pViA[2] * pViB[2]; 00232 } 00233 /* 00234 * KmVcmp : Vector Compare 00235 * 00236 * INPUT 00237 * pViA: input vector ViA 00238 * pViB: input vector ViB 00239 * 00240 * RETURN VALUE 00241 * ViA != ViB 00242 */ 00243 K_INLINE int KmVcmp(const double *const pViA, const double *const pViB, K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 00244 { 00245 return 00246 (pThreshold == 0.0) ? 00247 memcmp((const void *) pViA, (const void *) pViB, 3 * sizeof(double)) : 00248 (kAbs(pViA[0] - pViB[0]) > pThreshold) || 00249 (kAbs(pViA[1] - pViB[1]) > pThreshold) || 00250 (kAbs(pViA[2] - pViB[2]) > pThreshold); 00251 } 00252 00253 K_INLINE int Kmvcmp(const kOGLFloat *const pViA, const kOGLFloat *const pViB, K_DEFAULT(const kOGLFloat pThreshold, KM_TOLERANCE)) 00254 { 00255 return 00256 (pThreshold == 0.0) ? 00257 memcmp((const void *) pViA, (const void *) pViB, 3 * sizeof(kOGLFloat)) : 00258 (kAbs(pViA[0] - pViB[0]) > pThreshold) || 00259 (kAbs(pViA[1] - pViB[1]) > pThreshold) || 00260 (kAbs(pViA[2] - pViB[2]) > pThreshold); 00261 } 00262 00263 00264 /* 00265 * KmRVcmp : Rotation Vector Compare 00266 * 00267 * INPUT 00268 * pViA: input vector ViA 00269 * pViB: input vector ViB 00270 * 00271 * RETURN VALUE 00272 * ViA != ViB 00273 */ 00274 K_INLINE int KmRVcmp(const double *const pViA, const double *const pViB, K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 00275 { 00276 double ViA[3], ViB[3]; 00277 00278 ViA[0] = kMod(pViA[0], 360.0); 00279 ViA[1] = kMod(pViA[1], 360.0); 00280 ViA[2] = kMod(pViA[2], 360.0); 00281 ViB[0] = kMod(pViB[0], 360.0); 00282 ViB[1] = kMod(pViB[1], 360.0); 00283 ViB[2] = kMod(pViB[2], 360.0); 00284 00285 return ( 00286 ((kAbs(pViA[0] - pViB[0]) > pThreshold) || 00287 (kAbs(pViA[1] - pViB[1]) > pThreshold) || 00288 (kAbs(pViA[2] - pViB[2]) > pThreshold)) && 00289 ((kAbs(kAbs(ViA[0] - ViB[0]) - 180.0) > pThreshold) || 00290 (kAbs(kAbs(ViA[1] + ViB[1]) - 180.0) > pThreshold) || 00291 (kAbs(kAbs(ViA[2] - ViB[2]) - 180.0) > pThreshold))); 00292 } 00293 00295 // 00296 // Vector Initialization 00297 // 00299 00300 /* 00301 * KmV : Set Vector 00302 * 00303 * INPUT 00304 * pN0: input value N0 00305 * pN1: input value N1 00306 * pN2: input value N2 00307 * 00308 * OUTPUT 00309 * pVo: output vector Vo = | N0 N1 N2 |^T 00310 * 00311 * RETURN VALUE 00312 * pVo 00313 */ 00314 K_INLINE double *KmV(double *const pVo, const double pN0, const double pN1, const double pN2) 00315 { 00316 pVo[0] = pN0; 00317 pVo[1] = pN1; 00318 pVo[2] = pN2; 00319 00320 return pVo; 00321 } 00322 00323 K_INLINE float *KmV(float *const pVo, const float pN0, const float pN1, const float pN2) 00324 { 00325 pVo[0] = pN0; 00326 pVo[1] = pN1; 00327 pVo[2] = pN2; 00328 00329 return pVo; 00330 } 00331 00332 /* 00333 * KmVset : Set Vector 00334 * 00335 * INPUT 00336 * pVi: input vector Vi 00337 * 00338 * OUTPUT 00339 * pVo: output vector Vo = Vi 00340 * 00341 * RETURN VALUE 00342 * pVo 00343 * 00344 * REMARKS 00345 * - Works in-place 00346 */ 00347 K_INLINE double *KmVset(double *const pVo, const double *const pVi) 00348 { 00349 pVo[0] = pVi[0]; 00350 pVo[1] = pVi[1]; 00351 pVo[2] = pVi[2]; 00352 00353 return pVo; 00354 } 00355 00356 00357 K_INLINE float *KmVset(float *const pVo, const float *const pVi) 00358 { 00359 pVo[0] = pVi[0]; 00360 pVo[1] = pVi[1]; 00361 pVo[2] = pVi[2]; 00362 00363 return pVo; 00364 } 00365 00366 K_INLINE float *KmVset(float *const pVo, const double *const pVi) 00367 { 00368 pVo[0] = (float)pVi[0]; 00369 pVo[1] = (float)pVi[1]; 00370 pVo[2] = (float)pVi[2]; 00371 00372 return pVo; 00373 } 00374 00375 K_INLINE double *KmVset(double *const pVo, const float *const pVi) 00376 { 00377 pVo[0] = pVi[0]; 00378 pVo[1] = pVi[1]; 00379 pVo[2] = pVi[2]; 00380 00381 return pVo; 00382 } 00383 00384 00385 00387 // 00388 // Vector Operations 00389 // 00391 00392 /* 00393 * KmVneg : Vector Negate 00394 * 00395 * INPUT 00396 * pVi: input vector Vi 00397 * 00398 * OUTPUT 00399 * pVo: output vector Vo = -Vi 00400 * 00401 * RETURN VALUE 00402 * pVo 00403 * 00404 * REMARKS 00405 * - Works in-place 00406 */ 00407 K_INLINE double *KmVneg(double *const pVo, KM_CONST double *const pVi) 00408 { 00409 pVo[0] = -pVi[0]; 00410 pVo[1] = -pVi[1]; 00411 pVo[2] = -pVi[2]; 00412 00413 return pVo; 00414 } 00415 00416 /* 00417 * KmVadd : Vector Add 00418 * 00419 * INPUT 00420 * pViA: input vector ViA 00421 * pViB: input vector ViB 00422 * 00423 * OUTPUT 00424 * pVo: output vector Vo = ViA + ViB 00425 * 00426 * RETURN VALUE 00427 * pVo 00428 * 00429 * REMARKS 00430 * - Works in-place 00431 */ 00432 K_INLINE double *KmVadd(double *const pVo, KM_CONST double *const pViA, KM_CONST double *const pViB) 00433 { 00434 pVo[0] = pViA[0] + pViB[0]; 00435 pVo[1] = pViA[1] + pViB[1]; 00436 pVo[2] = pViA[2] + pViB[2]; 00437 00438 return pVo; 00439 } 00440 00441 K_INLINE float *KmVadd(float *const pVo, KM_CONST float *const pViA, KM_CONST float *const pViB) 00442 { 00443 pVo[0] = pViA[0] + pViB[0]; 00444 pVo[1] = pViA[1] + pViB[1]; 00445 pVo[2] = pViA[2] + pViB[2]; 00446 00447 return pVo; 00448 } 00449 00450 /* 00451 * KmVsub : Vector Substract 00452 * 00453 * INPUT 00454 * pViA: input vector ViA 00455 * pViB: input vector ViB 00456 * 00457 * OUTPUT 00458 * pVo: output vector Vo = ViA - ViB 00459 * 00460 * RETURN VALUE 00461 * pVo 00462 * 00463 * REMARKS 00464 * - Works in-place 00465 */ 00466 K_INLINE double *KmVsub(double *const pVo, KM_CONST double *const pViA, KM_CONST double *const pViB) 00467 { 00468 pVo[0] = pViA[0] - pViB[0]; 00469 pVo[1] = pViA[1] - pViB[1]; 00470 pVo[2] = pViA[2] - pViB[2]; 00471 00472 return pVo; 00473 } 00474 00475 K_INLINE double *KmVsub(double *const pVo, KM_CONST float *const pViA, KM_CONST float *const pViB) 00476 { 00477 pVo[0] = pViA[0] - pViB[0]; 00478 pVo[1] = pViA[1] - pViB[1]; 00479 pVo[2] = pViA[2] - pViB[2]; 00480 00481 return pVo; 00482 } 00483 00484 K_INLINE float *KmVsub(float *const pVo, KM_CONST float *const pViA, KM_CONST float *const pViB) 00485 { 00486 pVo[0] = pViA[0] - pViB[0]; 00487 pVo[1] = pViA[1] - pViB[1]; 00488 pVo[2] = pViA[2] - pViB[2]; 00489 00490 return pVo; 00491 } 00492 00493 /* 00494 * KmVrev : Vector Reverse 00495 * 00496 * INPUT 00497 * pVi: input vector Vi 00498 * 00499 * OUTPUT 00500 * pVo: output vector Vo = | Vi[2] Vi[1] Vi[0] |^T 00501 * 00502 * RETURN VALUE 00503 * pVo 00504 * 00505 * REMARKS 00506 * - Works in-place 00507 */ 00508 K_INLINE double *KmVrev(double *const pVo, KM_CONST double *const pVi) 00509 { 00510 if(pVo == pVi) { 00511 00512 double T; 00513 00514 kSwap(pVo[0], pVo[2], T); 00515 00516 } else { 00517 00518 pVo[0] = pVi[2]; 00519 pVo[1] = pVi[1]; 00520 pVo[2] = pVi[0]; 00521 } 00522 00523 return pVo; 00524 } 00525 00526 /* 00527 * KmVV : Vector Cross Product 00528 * 00529 * INPUT 00530 * pViA: input vector ViA 00531 * pViB: input vector ViB 00532 * 00533 * OUTPUT 00534 * pVo: output vector Vo = ViA x ViB 00535 * 00536 * RETURN VALUE 00537 * pVo 00538 * 00539 * REMARKS 00540 * - Works in-place 00541 */ 00542 K_INLINE double *KmVV(double *const pVo, KM_CONST double *const pViA, KM_CONST double *const pViB) 00543 { 00544 double T0, T1, T2; 00545 00546 T0 = pViA[1] * pViB[2] - pViA[2] * pViB[1]; 00547 T1 = pViA[2] * pViB[0] - pViA[0] * pViB[2]; 00548 T2 = pViA[0] * pViB[1] - pViA[1] * pViB[0]; 00549 00550 pVo[0] = T0; 00551 pVo[1] = T1; 00552 pVo[2] = T2; 00553 00554 return pVo; 00555 } 00556 00557 K_INLINE kOGLFloat *Kmvv( kOGLFloat *const pVo, KM_CONST kOGLFloat *const pViA, KM_CONST kOGLFloat *const pViB) 00558 { 00559 kOGLFloat T0, T1, T2; 00560 00561 T0 = pViA[1] * pViB[2] - pViA[2] * pViB[1]; 00562 T1 = pViA[2] * pViB[0] - pViA[0] * pViB[2]; 00563 T2 = pViA[0] * pViB[1] - pViA[1] * pViB[0]; 00564 00565 pVo[0] = T0; 00566 pVo[1] = T1; 00567 pVo[2] = T2; 00568 00569 return pVo; 00570 } 00571 00572 00573 /* 00574 * KmVs : Vector Times Scalar 00575 * 00576 * INPUT 00577 * pVi: input vector Vi 00578 * pNi: input scalar Ni 00579 * 00580 * OUTPUT 00581 * pVo: output vector Vo = Vi * Ni 00582 * 00583 * RETURN VALUE 00584 * pVo 00585 * 00586 * REMARKS 00587 * - Works in-place 00588 */ 00589 K_INLINE double *KmVN(double *const pVo, KM_CONST double *const pVi, const double pNi) 00590 { 00591 pVo[0] = pVi[0] * pNi; 00592 pVo[1] = pVi[1] * pNi; 00593 pVo[2] = pVi[2] * pNi; 00594 00595 return pVo; 00596 } 00597 00598 K_INLINE float *Kmvn(float *const pVo, KM_CONST float *const pVi, const float pNi) 00599 { 00600 pVo[0] = pVi[0] * pNi; 00601 pVo[1] = pVi[1] * pNi; 00602 pVo[2] = pVi[2] * pNi; 00603 00604 return pVo; 00605 } 00606 00607 /* 00608 * KmVnorm : Vector Normalize 00609 * 00610 * INPUT 00611 * pVi: input vector Vi 00612 * 00613 * OUTPUT 00614 * pVo: output vector Vo = Vi / |Vi| 00615 * 00616 * RETURN VALUE 00617 * pVo 00618 * 00619 * REMARKS 00620 * - Works in-place 00621 */ 00622 K_INLINE double *KmVnorm(double *const pVo, KM_CONST double *const pVi) 00623 { 00624 double Len = KmVlength(pVi); 00625 if (Len==0.0) { 00626 pVo[0] = 0.0; 00627 pVo[1] = 0.0; 00628 pVo[2] = 0.0; 00629 return pVo; 00630 } else { 00631 return KmVN(pVo, pVi, 1.0 / Len); 00632 } 00633 } 00634 00635 K_INLINE float *KmVnorm(float *const pVo, KM_CONST float *const pVi) 00636 { 00637 float Len = Kmvlength(pVi); 00638 if (Len==0.0) { 00639 pVo[0] = 0.0; 00640 pVo[1] = 0.0; 00641 pVo[2] = 0.0; 00642 return pVo; 00643 } else { 00644 return Kmvn(pVo, pVi, 1.0f / Len); 00645 } 00646 } 00647 00648 /* 00649 * KmMV : Matrix by Vector Product 00650 * 00651 * INPUT 00652 * pMi: input matrix Mi 00653 * pVi: input vector Vi 00654 * 00655 * OUTPUT 00656 * pVo: output vector Vo = Mi * Vi 00657 * 00658 * RETURN VALUE 00659 * pVo 00660 * 00661 * REMARKS 00662 * - Works in-place 00663 */ 00664 K_INLINE double *KmMV( 00665 double *const pVo, 00666 const double (*const pMi)[4], 00667 KM_CONST double *const pVi) 00668 { 00669 double V0, V1, V2, W; 00670 00671 V0 = pVi[0]; 00672 V1 = pVi[1]; 00673 V2 = pVi[2]; 00674 00675 W = 1.0 / (pMi[0][3] * V0 + pMi[1][3] * V1 + pMi[2][3] * V2 + pMi[3][3]); 00676 00677 pVo[0] = (pMi[0][0] * V0 + pMi[1][0] * V1 + pMi[2][0] * V2 + pMi[3][0]) * W; 00678 pVo[1] = (pMi[0][1] * V0 + pMi[1][1] * V1 + pMi[2][1] * V2 + pMi[3][1]) * W; 00679 pVo[2] = (pMi[0][2] * V0 + pMi[1][2] * V1 + pMi[2][2] * V2 + pMi[3][2]) * W; 00680 00681 return pVo; 00682 } 00683 00684 /* 00685 * KmMv : Matrix by Vector Product 00686 * 00687 * INPUT 00688 * pMi: input matrix Mi 00689 * pVi: input vector Vi 00690 * 00691 * OUTPUT 00692 * pVo: output vector Vo = Mi * Vi 00693 * 00694 * RETURN VALUE 00695 * pVo 00696 * 00697 * REMARKS 00698 * - Works in-place 00699 */ 00700 K_INLINE kOGLFloat *KmMv( 00701 kOGLFloat *const pVo, 00702 const double (*const pMi)[4], 00703 KM_CONST kOGLFloat *const pVi) 00704 { 00705 kOGLFloat V0, V1, V2, W; 00706 00707 V0 = pVi[0]; 00708 V1 = pVi[1]; 00709 V2 = pVi[2]; 00710 00711 W = (kOGLFloat)(1.0 / (pMi[0][3] * V0 + pMi[1][3] * V1 + pMi[2][3] * V2 + pMi[3][3])); 00712 00713 pVo[0] = (kOGLFloat)((pMi[0][0] * V0 + pMi[1][0] * V1 + pMi[2][0] * V2 + pMi[3][0]) * W); 00714 pVo[1] = (kOGLFloat)((pMi[0][1] * V0 + pMi[1][1] * V1 + pMi[2][1] * V2 + pMi[3][1]) * W); 00715 pVo[2] = (kOGLFloat)((pMi[0][2] * V0 + pMi[1][2] * V1 + pMi[2][2] * V2 + pMi[3][2]) * W); 00716 00717 return pVo; 00718 } 00719 00720 00721 00722 00723 /* 00724 * KmAV : Affine Matrix by Vector Product 00725 * 00726 * INPUT 00727 * pAi: input matrix Ai 00728 * pVi: input vector Vi 00729 * 00730 * OUTPUT 00731 * pVo: output vector Vo = Ai * Vi 00732 * 00733 * RETURN VALUE 00734 * pVo 00735 * 00736 * REMARKS 00737 * - Works in-place 00738 */ 00739 K_INLINE double *KmAV( 00740 double *const pVo, 00741 const double (*const pAi)[4], 00742 KM_CONST double *const pVi) 00743 { 00744 double V0, V1, V2; 00745 00746 V0 = pVi[0]; 00747 V1 = pVi[1]; 00748 V2 = pVi[2]; 00749 00750 pVo[0] = pAi[0][0] * V0 + pAi[1][0] * V1 + pAi[2][0] * V2 + pAi[3][0]; 00751 pVo[1] = pAi[0][1] * V0 + pAi[1][1] * V1 + pAi[2][1] * V2 + pAi[3][1]; 00752 pVo[2] = pAi[0][2] * V0 + pAi[1][2] * V1 + pAi[2][2] * V2 + pAi[3][2]; 00753 00754 return pVo; 00755 } 00756 00757 /* 00758 * KmAv : Affine Matrix by Vertex Product 00759 * 00760 * INPUT 00761 * pAi: input matrix Ai 00762 * pvi: input vertex vi 00763 * 00764 * OUTPUT 00765 * pvo: output vector vo = Ai * vi 00766 * 00767 * RETURN VALUE 00768 * pvo 00769 * 00770 * REMARKS 00771 * - Works in-place 00772 */ 00773 K_INLINE kOGLFloat *KmAv( 00774 kOGLFloat *const pVo, 00775 const double (*const pAi)[4], 00776 KM_CONST kOGLFloat *const pVi) 00777 { 00778 const kOGLFloat V0 = pVi[0]; 00779 const kOGLFloat V1 = pVi[1]; 00780 const kOGLFloat V2 = pVi[2]; 00781 00782 pVo[0] = kOGLFloat(pAi[0][0] * V0 + pAi[1][0] * V1 + pAi[2][0] * V2 + pAi[3][0]); 00783 pVo[1] = kOGLFloat(pAi[0][1] * V0 + pAi[1][1] * V1 + pAi[2][1] * V2 + pAi[3][1]); 00784 pVo[2] = kOGLFloat(pAi[0][2] * V0 + pAi[1][2] * V1 + pAi[2][2] * V2 + pAi[3][2]); 00785 00786 return pVo; 00787 } 00788 00789 /* 00790 * KmTV : Translation Matrix by Vector Product 00791 * 00792 * INPUT 00793 * pTi: input matrix Ti 00794 * pVi: input vector Vi 00795 * 00796 * OUTPUT 00797 * pVo: output vector Vo = Ti * Vi 00798 * 00799 * RETURN VALUE 00800 * pVo 00801 * 00802 * REMARKS 00803 * - Works in-place 00804 */ 00805 K_INLINE double *KmTV( 00806 double *const pVo, 00807 const double (*const pTi)[4], 00808 KM_CONST double *const pVi) 00809 { 00810 pVo[0] = pTi[3][0] + pVi[0]; 00811 pVo[1] = pTi[3][1] + pVi[1]; 00812 pVo[2] = pTi[3][2] + pVi[2]; 00813 00814 return pVo; 00815 } 00816 00817 /* 00818 * KmRV : Rotation Matrix by Vector Product 00819 * 00820 * INPUT 00821 * pRi: input matrix Ri 00822 * pVi: input vector Vi 00823 * 00824 * OUTPUT 00825 * pVo: output vector Vo = Ri * Vi 00826 * 00827 * RETURN VALUE 00828 * pVo 00829 * 00830 * REMARKS 00831 * - Works in-place 00832 */ 00833 K_INLINE double *KmRV( 00834 double *const pVo, 00835 const double (*const pRi)[4], 00836 KM_CONST double *const pVi) 00837 { 00838 double V0, V1, V2; 00839 00840 V0 = pVi[0]; 00841 V1 = pVi[1]; 00842 V2 = pVi[2]; 00843 00844 pVo[0] = pRi[0][0] * V0 + pRi[1][0] * V1 + pRi[2][0] * V2; 00845 pVo[1] = pRi[0][1] * V0 + pRi[1][1] * V1 + pRi[2][1] * V2; 00846 pVo[2] = pRi[0][2] * V0 + pRi[1][2] * V1 + pRi[2][2] * V2; 00847 00848 return pVo; 00849 } 00850 00851 /* 00852 * KmRV : Rotation Matrix by Vertex Product 00853 * 00854 * INPUT 00855 * pRi: input matrix Ri 00856 * pVi: input vector Vi 00857 * 00858 * OUTPUT 00859 * pVo: output vector Vo = Ri * Vi 00860 * 00861 * RETURN VALUE 00862 * pVo 00863 * 00864 * REMARKS 00865 * - Works in-place 00866 */ 00867 K_INLINE kOGLFloat *KmRV( 00868 kOGLFloat *const pVo, 00869 const double (*const pRi)[4], 00870 KM_CONST kOGLFloat *const pVi) 00871 { 00872 const kOGLFloat V0 = pVi[0]; 00873 const kOGLFloat V1 = pVi[1]; 00874 const kOGLFloat V2 = pVi[2]; 00875 00876 pVo[0] = (kOGLFloat)(pRi[0][0] * V0 + pRi[1][0] * V1 + pRi[2][0] * V2); 00877 pVo[1] = (kOGLFloat)(pRi[0][1] * V0 + pRi[1][1] * V1 + pRi[2][1] * V2); 00878 pVo[2] = (kOGLFloat)(pRi[0][2] * V0 + pRi[1][2] * V1 + pRi[2][2] * V2); 00879 00880 return pVo; 00881 } 00882 00883 00884 00885 /* 00886 * KmSV : Scaling Matrix by Vector Product 00887 * 00888 * INPUT 00889 * pSi: input matrix Si 00890 * pVi: input vector Vi 00891 * 00892 * OUTPUT 00893 * pVo: output vector Vo = Si * Vi 00894 * 00895 * RETURN VALUE 00896 * pVo 00897 * 00898 * REMARKS 00899 * - Works in-place 00900 */ 00901 K_INLINE double *KmSV( 00902 double *const pVo, 00903 const double (*const pSi)[4], 00904 KM_CONST double *const pVi) 00905 { 00906 pVo[0] = pSi[0][0] * pVi[0]; 00907 pVo[1] = pSi[1][1] * pVi[1]; 00908 pVo[2] = pSi[2][2] * pVi[2]; 00909 00910 return pVo; 00911 } 00912 00914 // 00915 // Quaternion Utilities 00916 // 00918 00919 /* 00920 * KmQlength : Quaternion Length 00921 * 00922 * INPUT 00923 * pQi: input quaternion Qi 00924 * 00925 * RETURN VALUE 00926 * |Qi| 00927 */ 00928 K_INLINE double KmQlength(const double *const pQi) 00929 { 00930 return kNorm(pQi[0], pQi[1], pQi[2], pQi[3]); 00931 } 00932 00933 /* 00934 * KmQlength2 : Quaternion Squared Length 00935 * 00936 * INPUT 00937 * pQi: input quaternion Qi 00938 * 00939 * RETURN VALUE 00940 * |Qi|^2 00941 */ 00942 K_INLINE double KmQlength2(const double *const pQi) 00943 { 00944 double X, Y, Z, W; 00945 00946 X = pQi[0]; 00947 Y = pQi[1]; 00948 Z = pQi[2]; 00949 W = pQi[3]; 00950 00951 return X * X + Y * Y + Z * Z + W * W; 00952 } 00953 00954 /* 00955 * KmQdotQ : Quaternion Dot Product 00956 * 00957 * INPUT 00958 * pQiA: input quaternion QiA 00959 * pQiB: input quaternion QiB 00960 * 00961 * RETURN VALUE 00962 * pQiA . pQiB 00963 */ 00964 K_INLINE double KmQdotQ(const double *const pQiA, const double *const pQiB) 00965 { 00966 return pQiA[0] * pQiB[0] + pQiA[1] * pQiB[1] + pQiA[2] * pQiB[2] + pQiA[3] * pQiB[3]; 00967 } 00968 00969 /* 00970 * KmQcmp : Quaternion Compare 00971 * 00972 * INPUT 00973 * pQiA: input vector QiA 00974 * pQiB: input vector QiB 00975 * 00976 * RETURN VALUE 00977 * QiA != QiB 00978 */ 00979 K_INLINE int KmQcmp(const double *const pQiA, const double *const pQiB, K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 00980 { 00981 return 00982 (pThreshold == 0.0) ? 00983 memcmp((const void *) pQiA, (const void *) pQiB, 4 * sizeof(double)) : 00984 (kAbs(pQiA[0] - pQiB[0]) > pThreshold) || 00985 (kAbs(pQiA[1] - pQiB[1]) > pThreshold) || 00986 (kAbs(pQiA[2] - pQiB[2]) > pThreshold) || 00987 (kAbs(pQiA[3] - pQiB[3]) > pThreshold); 00988 } 00989 00991 // 00992 // Quaternion Initialization 00993 // 00995 00996 /* 00997 * KmQ : Set Quaternion 00998 * 00999 * INPUT 01000 * pX: input value X 01001 * pY: input value Y 01002 * pZ: input value Z 01003 * pW: input value W 01004 * 01005 * OUTPUT 01006 * pQo: output quaternion Qo = | X Y Z W | 01007 * 01008 * RETURN VALUE 01009 * pQo 01010 */ 01011 K_INLINE double *KmQ( 01012 double *const pQo, 01013 const double pX, const double pY, const double pZ, const double pW) 01014 { 01015 pQo[0] = pX; 01016 pQo[1] = pY; 01017 pQo[2] = pZ; 01018 pQo[3] = pW; 01019 01020 return pQo; 01021 } 01022 01023 /* 01024 * KmQset : Set Quaternion 01025 * 01026 * INPUT 01027 * pQi: input quaternion Qi 01028 * 01029 * OUTPUT 01030 * pQo: output quaternion Qo = Qi 01031 * 01032 * RETURN VALUE 01033 * pQo 01034 * 01035 * REMARKS 01036 * - Works in-place 01037 */ 01038 K_INLINE double *KmQset(double *const pQo, const double *const pQi) 01039 { 01040 pQo[0] = pQi[0]; 01041 pQo[1] = pQi[1]; 01042 pQo[2] = pQi[2]; 01043 pQo[3] = pQi[3]; 01044 01045 return pQo; 01046 } 01047 01049 // 01050 // Quaternion Operations 01051 // 01053 01054 /* 01055 * KmQconj : Quaternion Conjugate 01056 * 01057 * INPUT 01058 * pQi: input quaternion Qi 01059 * 01060 * OUTPUT 01061 * pQo: output quaternion Qo = ~Qi 01062 * 01063 * RETURN VALUE 01064 * pQo 01065 * 01066 * REMARKS 01067 * - Works in-place 01068 */ 01069 K_INLINE double *KmQconj(double *const pQo, KM_CONST double *const pQi) 01070 { 01071 pQo[0] = -pQi[0]; 01072 pQo[1] = -pQi[1]; 01073 pQo[2] = -pQi[2]; 01074 pQo[3] = pQi[3]; 01075 01076 return pQo; 01077 } 01078 01079 /* 01080 * KmQs : Quaternion Times Scalar 01081 * 01082 * INPUT 01083 * pQi: input vector Qi 01084 * pNi: input scalar Ni 01085 * 01086 * OUTPUT 01087 * pQo: output vector Qo = Qi * Ni 01088 * 01089 * RETURN VALUE 01090 * pQo 01091 * 01092 * REMARKS 01093 * - Works in-place 01094 */ 01095 K_INLINE double *KmQN(double *const pQo, KM_CONST double *const pQi, const double pNi) 01096 { 01097 pQo[0] = pQi[0] * pNi; 01098 pQo[1] = pQi[1] * pNi; 01099 pQo[2] = pQi[2] * pNi; 01100 pQo[3] = pQi[3] * pNi; 01101 01102 return pQo; 01103 } 01104 01105 /* 01106 * KmQnorm : Quaternion Normalize 01107 * 01108 * INPUT 01109 * pQi: input quaternion Qi 01110 * 01111 * OUTPUT 01112 * pQo: output quaternion Qo = Qi / |Qi| 01113 * 01114 * RETURN VALUE 01115 * pQo 01116 * 01117 * REMARKS 01118 * - Works in-place 01119 */ 01120 K_INLINE double *KmQnorm(double *const pQo, KM_CONST double *const pQi) 01121 { 01122 return KmQN(pQo, pQi, 1.0 / KmQlength(pQi)); 01123 } 01124 01125 /* 01126 * KmQadd : Quaternion Add 01127 * 01128 * INPUT 01129 * pQiA: input quaternion QiA 01130 * pQiB: input quaternion QiB 01131 * 01132 * OUTPUT 01133 * pQo: output quaternion Qo = QiA + QiB 01134 * 01135 * RETURN VALUE 01136 * pQo 01137 * 01138 * REMARKS 01139 * - Works in-place 01140 */ 01141 K_INLINE double *KmQadd(double *const pQo, KM_CONST double *const pQiA, KM_CONST double *const pQiB) 01142 { 01143 pQo[0] = pQiA[0] + pQiB[0]; 01144 pQo[1] = pQiA[1] + pQiB[1]; 01145 pQo[2] = pQiA[2] + pQiB[2]; 01146 pQo[3] = pQiA[3] + pQiB[3]; 01147 01148 return pQo; 01149 } 01150 01151 /* 01152 * KmQQ : Quaternion Product 01153 * 01154 * INPUT 01155 * pQiA: input quaternion QiL 01156 * pQiB: input quaternion QiR 01157 * 01158 * OUTPUT 01159 * pQo: output quaternion Qo = QiL * QiR 01160 * 01161 * RETURN VALUE 01162 * pQo 01163 * 01164 * REMARKS 01165 * - Works in-place 01166 */ 01167 K_INLINE double *KmQQ(double *const pQo, KM_CONST double *const pQiA, KM_CONST double *const pQiB) 01168 { 01169 double Q0, Q1, Q2, Q3; 01170 01171 if(pQo == pQiA) { 01172 01173 Q0 = pQiA[0]; 01174 Q1 = pQiA[1]; 01175 Q2 = pQiA[2]; 01176 Q3 = pQiA[3]; 01177 01178 pQo[0] = Q3 * pQiB[0] + Q0 * pQiB[3] + Q1 * pQiB[2] - Q2 * pQiB[1]; 01179 pQo[1] = Q3 * pQiB[1] - Q0 * pQiB[2] + Q1 * pQiB[3] + Q2 * pQiB[0]; 01180 pQo[2] = Q3 * pQiB[2] + Q0 * pQiB[1] - Q1 * pQiB[0] + Q2 * pQiB[3]; 01181 pQo[3] = Q3 * pQiB[3] - Q0 * pQiB[0] - Q1 * pQiB[1] - Q2 * pQiB[2]; 01182 01183 } else { 01184 01185 Q0 = pQiB[0]; 01186 Q1 = pQiB[1]; 01187 Q2 = pQiB[2]; 01188 Q3 = pQiB[3]; 01189 01190 pQo[0] = Q0 * pQiA[3] + Q3 * pQiA[0] + Q2 * pQiA[1] - Q1 * pQiA[2]; 01191 pQo[1] = Q1 * pQiA[3] - Q2 * pQiA[0] + Q3 * pQiA[1] + Q0 * pQiA[2]; 01192 pQo[2] = Q2 * pQiA[3] + Q1 * pQiA[0] - Q0 * pQiA[1] + Q3 * pQiA[2]; 01193 pQo[3] = Q3 * pQiA[3] - Q0 * pQiA[0] - Q1 * pQiA[1] - Q2 * pQiA[2]; 01194 } 01195 01196 return pQo; 01197 } 01198 01200 // 01201 // Matrix To Quaternion Decompositions 01202 // 01204 01205 /* 01206 * KmRtoQ : Rotation Matrix To Quaternion 01207 * 01208 * INPUT 01209 * pRi: input matrix Ri 01210 * 01211 * OUTPUT 01212 * pQo: output quaternion Qo = Q(Ri) 01213 * 01214 * RETURN VALUE 01215 * pQo 01216 * 01217 * REMARKS 01218 */ 01219 K_INLINE double *KmRtoQ(double *const pQo, const double (*const pRi)[4]) 01220 { 01221 double T; 01222 01223 if((T = 0.25 * (pRi[0][0] + pRi[1][1] + pRi[2][2] + 1.0)) > KM_EPSILON) { 01224 01225 pQo[3] = T = kSqrt(T); T = 0.25 / T; 01226 pQo[0] = T * (pRi[1][2] - pRi[2][1]); 01227 pQo[1] = T * (pRi[2][0] - pRi[0][2]); 01228 pQo[2] = T * (pRi[0][1] - pRi[1][0]); 01229 01230 } else { 01231 01232 pQo[3] = 0.0; 01233 if((T = -0.5 * (pRi[1][1] + pRi[2][2])) > KM_EPSILON) { 01234 01235 pQo[0] = T = kSqrt(T); T = 0.5 / T; 01236 pQo[1] = T * pRi[0][1]; 01237 pQo[2] = T * pRi[0][2]; 01238 01239 } else { 01240 01241 pQo[0] = 0.0; 01242 if((T = 0.5 * (1.0 - pRi[2][2])) > KM_EPSILON) { 01243 01244 pQo[1] = T = kSqrt(T); T = 0.5 / T; 01245 pQo[2] = T * pRi[1][2]; 01246 01247 } else { 01248 01249 pQo[1] = 0.0; 01250 pQo[2] = 1.0; 01251 } 01252 } 01253 } 01254 01255 return pQo; 01256 } 01257 01259 // 01260 // Matrix Utilities 01261 // 01263 01264 /* 01265 * KmMdet : Matrix Determinant 01266 * 01267 * INPUT 01268 * pMi: input matrix Mi 01269 * 01270 * RETURN VALUE 01271 * det(Mi) 01272 */ 01273 K_INLINE double KmMdet(const double (*const pMi)[4]) 01274 { 01275 double D01, D02, D03, D12, D13, D23; 01276 01277 D01 = pMi[0][2] * pMi[1][3] - pMi[0][3] * pMi[1][2]; 01278 D02 = pMi[0][2] * pMi[2][3] - pMi[0][3] * pMi[2][2]; 01279 D03 = pMi[0][2] * pMi[3][3] - pMi[0][3] * pMi[3][2]; 01280 D12 = pMi[1][2] * pMi[2][3] - pMi[1][3] * pMi[2][2]; 01281 D13 = pMi[1][2] * pMi[3][3] - pMi[1][3] * pMi[3][2]; 01282 D23 = pMi[2][2] * pMi[3][3] - pMi[2][3] * pMi[3][2]; 01283 01284 return 01285 pMi[0][0] * (pMi[1][1] * D23 - pMi[2][1] * D13 + pMi[3][1] * D12) - 01286 pMi[1][0] * (pMi[0][1] * D23 - pMi[2][1] * D03 + pMi[3][1] * D02) + 01287 pMi[2][0] * (pMi[0][1] * D13 - pMi[1][1] * D03 + pMi[3][1] * D01) - 01288 pMi[3][0] * (pMi[0][1] * D12 - pMi[1][1] * D02 + pMi[2][1] * D01); 01289 } 01290 01291 /* 01292 * KmAdet : Affine Matrix Determinant 01293 * 01294 * INPUT 01295 * pAi: input matrix Ai 01296 * 01297 * RETURN VALUE 01298 * det(Ai) 01299 */ 01300 K_INLINE double KmAdet(const double (*const pAi)[4]) 01301 { 01302 return 01303 pAi[0][0] * (pAi[1][1] * pAi[2][2] - pAi[1][2] * pAi[2][1]) - 01304 pAi[1][0] * (pAi[0][1] * pAi[2][2] - pAi[0][2] * pAi[2][1]) + 01305 pAi[2][0] * (pAi[0][1] * pAi[1][2] - pAi[0][2] * pAi[1][1]); 01306 } 01307 01308 /* 01309 * KmTdet : Translation Matrix Determinant 01310 * 01311 * INPUT 01312 * pTi: input matrix Ti 01313 * 01314 * RETURN VALUE 01315 * det(Ti) 01316 */ 01317 K_INLINE double KmTdet(const double (*const)[4]) 01318 { 01319 return 1.0; 01320 } 01321 01322 /* 01323 * KmRdet : Rotation Matrix Determinant 01324 * 01325 * INPUT 01326 * pRi: input matrix Ri 01327 * 01328 * RETURN VALUE 01329 * det(Ri) 01330 */ 01331 K_INLINE double KmRdet(const double (*const /*pRi*/)[4]) 01332 { 01333 return 1.0; 01334 } 01335 01336 /* 01337 * KmSdet : Scaling Matrix Determinant 01338 * 01339 * INPUT 01340 * pSi: input matrix Si 01341 * 01342 * RETURN VALUE 01343 * det(Si) 01344 */ 01345 K_INLINE double KmSdet(const double (*const pSi)[4]) 01346 { 01347 return pSi[0][0] * pSi[1][1] * pSi[2][2]; 01348 } 01349 01350 /* 01351 * KmMtrace : Matrix Trace 01352 * 01353 * INPUT 01354 * pMi: input matrix Mi 01355 * 01356 * RETURN VALUE 01357 * trace(Mi) 01358 */ 01359 K_INLINE double KmMtrace(const double (*const pMi)[4]) 01360 { 01361 return pMi[0][0] + pMi[1][1] + pMi[2][2] + pMi[3][3]; 01362 } 01363 01364 /* 01365 * KmAtrace : Affine Matrix Trace 01366 * 01367 * INPUT 01368 * pAi: input matrix Ai 01369 * 01370 * RETURN VALUE 01371 * trace(Ai) 01372 */ 01373 K_INLINE double KmAtrace(const double (*const pAi)[4]) 01374 { 01375 return pAi[0][0] + pAi[1][1] + pAi[2][2] + 1.0; 01376 } 01377 01378 /* 01379 * KmTtrace : Translation Matrix Trace 01380 * 01381 * INPUT 01382 * pTi: input matrix Ti 01383 * 01384 * RETURN VALUE 01385 * trace(Ti) 01386 */ 01387 K_INLINE double KmTtrace(const double (*const /*pTi*/)[4]) 01388 { 01389 return 4.0; 01390 } 01391 01392 /* 01393 * KmRtrace : Rotation Matrix Trace 01394 * 01395 * INPUT 01396 * pRi: input matrix Ri 01397 * 01398 * RETURN VALUE 01399 * trace(Ri) 01400 */ 01401 K_INLINE double KmRtrace(const double (*const pRi)[4]) 01402 { 01403 return KmAtrace(pRi); 01404 } 01405 01406 /* 01407 * KmStrace : Scaling Matrix Trace 01408 * 01409 * INPUT 01410 * pSi: input matrix Si 01411 * 01412 * RETURN VALUE 01413 * trace(Si) 01414 */ 01415 K_INLINE double KmStrace(const double (*const pSi)[4]) 01416 { 01417 return KmAtrace(pSi); 01418 } 01419 01420 /* 01421 * KmMcmp : Matrix Compare 01422 * 01423 * INPUT 01424 * pMiA: input vector MiA 01425 * pMiB: input vector MiB 01426 * 01427 * RETURN VALUE 01428 * MiA != MiB 01429 */ 01430 K_INLINE int KmMcmp(const double (*const pMiA)[4], const double (*const pMiB)[4], K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 01431 { 01432 return 01433 (pThreshold == 0.0) ? 01434 memcmp((const void *) pMiA, (const void *) pMiB, 16 * sizeof(double)) : 01435 (kAbs(pMiA[0][0] - pMiB[0][0]) > pThreshold) || 01436 (kAbs(pMiA[0][1] - pMiB[0][1]) > pThreshold) || 01437 (kAbs(pMiA[0][2] - pMiB[0][2]) > pThreshold) || 01438 (kAbs(pMiA[0][3] - pMiB[0][3]) > pThreshold) || 01439 (kAbs(pMiA[1][0] - pMiB[1][0]) > pThreshold) || 01440 (kAbs(pMiA[1][1] - pMiB[1][1]) > pThreshold) || 01441 (kAbs(pMiA[1][2] - pMiB[1][2]) > pThreshold) || 01442 (kAbs(pMiA[1][3] - pMiB[1][3]) > pThreshold) || 01443 (kAbs(pMiA[2][0] - pMiB[2][0]) > pThreshold) || 01444 (kAbs(pMiA[2][1] - pMiB[2][1]) > pThreshold) || 01445 (kAbs(pMiA[2][2] - pMiB[2][2]) > pThreshold) || 01446 (kAbs(pMiA[2][3] - pMiB[2][3]) > pThreshold) || 01447 (kAbs(pMiA[3][0] - pMiB[3][0]) > pThreshold) || 01448 (kAbs(pMiA[3][1] - pMiB[3][1]) > pThreshold) || 01449 (kAbs(pMiA[3][2] - pMiB[3][2]) > pThreshold) || 01450 (kAbs(pMiA[3][3] - pMiB[3][3]) > pThreshold); 01451 } 01452 01453 /* 01454 * KmAcmp : Affine Matrix Compare 01455 * 01456 * INPUT 01457 * pAiA: input vector AiA 01458 * pAiB: input vector AiB 01459 * 01460 * RETURN VALUE 01461 * AiA != AiB 01462 */ 01463 K_INLINE int KmAcmp(const double (*const pAiA)[4], const double (*const pAiB)[4], K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 01464 { 01465 return 01466 (pThreshold == 0.0) ? 01467 memcmp((const void *) pAiA, (const void *) pAiB, 15 * sizeof(double)) : 01468 (kAbs(pAiA[0][0] - pAiB[0][0]) > pThreshold) || 01469 (kAbs(pAiA[0][1] - pAiB[0][1]) > pThreshold) || 01470 (kAbs(pAiA[0][2] - pAiB[0][2]) > pThreshold) || 01471 (kAbs(pAiA[1][0] - pAiB[1][0]) > pThreshold) || 01472 (kAbs(pAiA[1][1] - pAiB[1][1]) > pThreshold) || 01473 (kAbs(pAiA[1][2] - pAiB[1][2]) > pThreshold) || 01474 (kAbs(pAiA[2][0] - pAiB[2][0]) > pThreshold) || 01475 (kAbs(pAiA[2][1] - pAiB[2][1]) > pThreshold) || 01476 (kAbs(pAiA[2][2] - pAiB[2][2]) > pThreshold) || 01477 (kAbs(pAiA[3][0] - pAiB[3][0]) > pThreshold) || 01478 (kAbs(pAiA[3][1] - pAiB[3][1]) > pThreshold) || 01479 (kAbs(pAiA[3][2] - pAiB[3][2]) > pThreshold); 01480 } 01481 01482 /* 01483 * KmTcmp : Affine Matrix Compare 01484 * 01485 * INPUT 01486 * pTiA: input vector TiA 01487 * pTiB: input vector TiB 01488 * 01489 * RETURN VALUE 01490 * TiA == TiB 01491 */ 01492 K_INLINE int KmTcmp(const double (*const pTiA)[4], const double (*const pTiB)[4], K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 01493 { 01494 return 01495 (pThreshold == 0.0) ? 01496 (pTiA[3][0] == pTiB[3][0]) && (pTiA[3][1] == pTiB[3][1]) && (pTiA[3][2] == pTiB[3][2]) : 01497 (kAbs(pTiA[3][0] - pTiB[3][0]) < pThreshold) && 01498 (kAbs(pTiA[3][1] - pTiB[3][1]) < pThreshold) && 01499 (kAbs(pTiA[3][2] - pTiB[3][2]) < pThreshold); 01500 } 01501 01502 /* 01503 * KmRcmp : Rotation Matrix Compare 01504 * 01505 * INPUT 01506 * pRiA: input vector RiA 01507 * pRiB: input vector RiB 01508 * 01509 * RETURN VALUE 01510 * RiA != RiB 01511 */ 01512 K_INLINE int KmRcmp(const double (*const pRiA)[4], const double (*const pRiB)[4], K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 01513 { 01514 return 01515 (pThreshold == 0.0) ? 01516 memcmp((const void *) pRiA, (const void *) pRiB, 11 * sizeof(double)) : 01517 (kAbs(pRiA[0][0] - pRiB[0][0]) > pThreshold) || 01518 (kAbs(pRiA[0][1] - pRiB[0][1]) > pThreshold) || 01519 (kAbs(pRiA[0][2] - pRiB[0][2]) > pThreshold) || 01520 (kAbs(pRiA[1][0] - pRiB[1][0]) > pThreshold) || 01521 (kAbs(pRiA[1][1] - pRiB[1][1]) > pThreshold) || 01522 (kAbs(pRiA[1][2] - pRiB[1][2]) > pThreshold) || 01523 (kAbs(pRiA[2][0] - pRiB[2][0]) > pThreshold) || 01524 (kAbs(pRiA[2][1] - pRiB[2][1]) > pThreshold) || 01525 (kAbs(pRiA[2][2] - pRiB[2][2]) > pThreshold); 01526 } 01527 01528 /* 01529 * KmScmp : Scaling Matrix Compare 01530 * 01531 * INPUT 01532 * pSiA: input vector SiA 01533 * pSiB: input vector SiB 01534 * 01535 * RETURN VALUE 01536 * SiA == SiB 01537 */ 01538 K_INLINE int KmScmp(const double (*const pSiA)[4], const double (*const pSiB)[4], K_DEFAULT(const double pThreshold, KM_TOLERANCE)) 01539 { 01540 return 01541 (pThreshold == 0.0) ? 01542 (pSiA[0][0] == pSiB[0][0]) && (pSiA[1][1] == pSiB[1][1]) && (pSiA[2][2] == pSiB[2][2]) : 01543 (kAbs(pSiA[0][0] - pSiB[0][0]) < pThreshold) && 01544 (kAbs(pSiA[1][1] - pSiB[1][1]) < pThreshold) && 01545 (kAbs(pSiA[2][2] - pSiB[2][2]) < pThreshold); 01546 } 01547 01549 // 01550 // Matrix Initialization 01551 // 01553 01554 /* 01555 * KmId : Matrix Identity 01556 * 01557 * OUTPUT 01558 * pMo: output vector Mo = I 01559 * 01560 * RETURN VALUE 01561 * pMo 01562 * 01563 * REMARKS 01564 * - Should try out memset() under Win32 with intrinsic optimization 01565 */ 01566 K_INLINE double (*KmId(double (*const pMo)[4]))[4] 01567 { 01568 pMo[0][0] = pMo[1][1] = pMo[2][2] = pMo[3][3] = 1.0; 01569 pMo[0][1] = pMo[0][2] = pMo[0][3] = 01570 pMo[1][0] = pMo[1][2] = pMo[1][3] = 01571 pMo[2][0] = pMo[2][1] = pMo[2][3] = 01572 pMo[3][0] = pMo[3][1] = pMo[3][2] = 0.0; 01573 01574 return pMo; 01575 } 01576 01577 /* 01578 * KmAinit : Affine Matrix Initialize 01579 * 01580 * OUTPUT 01581 * pAo: output vector Ao 01582 * 01583 * RETURN VALUE 01584 * pAo 01585 * 01586 * REMARKS 01587 * - Should try out memset() under Win32 with intrinsic optimization 01588 */ 01589 K_INLINE double (*KmAinit(double (*const pAo)[4]))[4] 01590 { 01591 pAo[0][3] = pAo[1][3] = pAo[2][3] = 0.0; 01592 pAo[3][3] = 1.0; 01593 01594 return pAo; 01595 } 01596 01597 /* 01598 * KmTinit : Translation Matrix Initialize 01599 * 01600 * OUTPUT 01601 * pTo: output vector To 01602 * 01603 * RETURN VALUE 01604 * pTo 01605 * 01606 * REMARKS 01607 * - Should try out memset() under Win32 with intrinsic optimization 01608 */ 01609 K_INLINE double (*KmTinit(double (*const pTo)[4]))[4] 01610 { 01611 pTo[0][1] = pTo[0][2] = pTo[0][3] = 01612 pTo[1][0] = pTo[1][2] = pTo[1][3] = 01613 pTo[2][0] = pTo[2][1] = pTo[2][3] = 0.0; 01614 pTo[0][0] = pTo[1][1] = pTo[2][2] = pTo[3][3] = 1.0; 01615 01616 return pTo; 01617 } 01618 01619 /* 01620 * KmRinit : Rotation Matrix Initialize 01621 * 01622 * OUTPUT 01623 * pRo: output vector Ro 01624 * 01625 * RETURN VALUE 01626 * pRo 01627 * 01628 * REMARKS 01629 * - Should try out memset() under Win32 with intrinsic optimization 01630 */ 01631 K_INLINE double (*KmRinit(double (*const pRo)[4]))[4] 01632 { 01633 pRo[0][3] = pRo[1][3] = pRo[2][3] = 01634 pRo[3][0] = pRo[3][1] = pRo[3][2] = 0.0; 01635 pRo[3][3] = 1.0; 01636 01637 return pRo; 01638 } 01639 01640 /* 01641 * KmSinit : Scaling Matrix Initialize 01642 * 01643 * OUTPUT 01644 * pSo: output vector So 01645 * 01646 * RETURN VALUE 01647 * pSo 01648 * 01649 * REMARKS 01650 * - Should try out memset() under Win32 with intrinsic optimization 01651 */ 01652 K_INLINE double (*KmSinit(double (*const pSo)[4]))[4] 01653 { 01654 pSo[0][1] = pSo[0][2] = pSo[0][3] = 01655 pSo[1][0] = pSo[1][2] = pSo[1][3] = 01656 pSo[2][0] = pSo[2][1] = pSo[2][3] = 01657 pSo[3][0] = pSo[3][1] = pSo[3][2] = 0.0; 01658 pSo[3][3] = 1.0; 01659 01660 return pSo; 01661 } 01662 01663 /* 01664 * KmMset : Set Matrix 01665 * 01666 * INPUT 01667 * pMi: input matrix Mi 01668 * 01669 * OUTPUT 01670 * pMo: output matrix Mo = Mi 01671 * 01672 * RETURN VALUE 01673 * pMo 01674 * 01675 * REMARKS 01676 * - Should try out memcpy() under Win32 with intrinsic optimization 01677 */ 01678 K_INLINE double (*KmMset(double (*const pMo)[4], const double (*const pMi)[4]))[4] 01679 { 01680 pMo[0][0] = pMi[0][0]; 01681 pMo[0][1] = pMi[0][1]; 01682 pMo[0][2] = pMi[0][2]; 01683 pMo[0][3] = pMi[0][3]; 01684 01685 pMo[1][0] = pMi[1][0]; 01686 pMo[1][1] = pMi[1][1]; 01687 pMo[1][2] = pMi[1][2]; 01688 pMo[1][3] = pMi[1][3]; 01689 01690 pMo[2][0] = pMi[2][0]; 01691 pMo[2][1] = pMi[2][1]; 01692 pMo[2][2] = pMi[2][2]; 01693 pMo[2][3] = pMi[2][3]; 01694 01695 pMo[3][0] = pMi[3][0]; 01696 pMo[3][1] = pMi[3][1]; 01697 pMo[3][2] = pMi[3][2]; 01698 pMo[3][3] = pMi[3][3]; 01699 01700 return pMo; 01701 } 01702 01703 /* 01704 * KmAset : Set Affine Matrix 01705 * 01706 * INPUT 01707 * pAi: input matrix Ai 01708 * 01709 * OUTPUT 01710 * pAo: output matrix Ao = Ai 01711 * 01712 * RETURN VALUE 01713 * pAo 01714 * 01715 * REMARKS 01716 * - Should try out memcpy() under Win32 with intrinsic optimization 01717 */ 01718 K_INLINE double (*KmAset(double (*const pAo)[4], const double (*const pAi)[4]))[4] 01719 { 01720 pAo[0][0] = pAi[0][0]; 01721 pAo[0][1] = pAi[0][1]; 01722 pAo[0][2] = pAi[0][2]; 01723 01724 pAo[1][0] = pAi[1][0]; 01725 pAo[1][1] = pAi[1][1]; 01726 pAo[1][2] = pAi[1][2]; 01727 01728 pAo[2][0] = pAi[2][0]; 01729 pAo[2][1] = pAi[2][1]; 01730 pAo[2][2] = pAi[2][2]; 01731 01732 pAo[3][0] = pAi[3][0]; 01733 pAo[3][1] = pAi[3][1]; 01734 pAo[3][2] = pAi[3][2]; 01735 01736 return pAo; 01737 } 01738 01739 /* 01740 * KmTset : Set Translation Matrix 01741 * 01742 * INPUT 01743 * pTi: input matrix Ti 01744 * 01745 * OUTPUT 01746 * pTo: output matrix To = Ti 01747 * 01748 * RETURN VALUE 01749 * pTo 01750 */ 01751 K_INLINE double (*KmTset(double (*const pTo)[4], const double (*const pTi)[4]))[4] 01752 { 01753 pTo[3][0] = pTi[3][0]; 01754 pTo[3][1] = pTi[3][1]; 01755 pTo[3][2] = pTi[3][2]; 01756 01757 return pTo; 01758 } 01759 01760 /* 01761 * KmRset : Set Rotation Matrix 01762 * 01763 * INPUT 01764 * pRi: input matrix Ri 01765 * 01766 * OUTPUT 01767 * pRo: output matrix Ro = Ri 01768 * 01769 * RETURN VALUE 01770 * pRo 01771 */ 01772 K_INLINE double (*KmRset(double (*const pRo)[4], const double (*const pRi)[4]))[4] 01773 { 01774 pRo[0][0] = pRi[0][0]; 01775 pRo[0][1] = pRi[0][1]; 01776 pRo[0][2] = pRi[0][2]; 01777 01778 pRo[1][0] = pRi[1][0]; 01779 pRo[1][1] = pRi[1][1]; 01780 pRo[1][2] = pRi[1][2]; 01781 01782 pRo[2][0] = pRi[2][0]; 01783 pRo[2][1] = pRi[2][1]; 01784 pRo[2][2] = pRi[2][2]; 01785 01786 return pRo; 01787 } 01788 01789 /* 01790 * KmSset : Set Scaling Matrix 01791 * 01792 * INPUT 01793 * pSi: input matrix Si 01794 * 01795 * OUTPUT 01796 * pSo: output matrix So = Si 01797 * 01798 * RETURN VALUE 01799 * pSo 01800 */ 01801 K_INLINE double (*KmSset(double (*const pSo)[4], const double (*const pSi)[4]))[4] 01802 { 01803 pSo[0][0] = pSi[0][0]; 01804 pSo[1][1] = pSi[1][1]; 01805 pSo[2][2] = pSi[2][2]; 01806 01807 return pSo; 01808 } 01809 01811 // 01812 // Matrix Operations 01813 // 01815 01816 /* 01817 * KmMt : Matrix Transpose 01818 * 01819 * INPUT 01820 * pMi: input matrix Mi 01821 * 01822 * OUTPUT 01823 * pMo: output matrix Mo = Mi^t 01824 * 01825 * RETURN VALUE 01826 * pMo 01827 * 01828 * REMARKS 01829 * - Works in-place 01830 */ 01831 K_INLINE double (*KmMt( 01832 double (*const pMo)[4], 01833 KM_CONST double (*const pMi)[4]))[4] 01834 { 01835 if(pMo == pMi) { 01836 01837 double T; 01838 01839 kSwap(pMo[0][1], pMo[1][0], T); 01840 kSwap(pMo[0][2], pMo[2][0], T); 01841 kSwap(pMo[0][3], pMo[3][0], T); 01842 kSwap(pMo[1][2], pMo[2][1], T); 01843 kSwap(pMo[1][3], pMo[3][1], T); 01844 kSwap(pMo[2][3], pMo[3][2], T); 01845 01846 } else { 01847 01848 pMo[0][0] = pMi[0][0]; 01849 pMo[0][1] = pMi[1][0]; 01850 pMo[0][2] = pMi[2][0]; 01851 pMo[0][3] = pMi[3][0]; 01852 01853 pMo[1][0] = pMi[0][1]; 01854 pMo[1][1] = pMi[1][1]; 01855 pMo[1][2] = pMi[2][1]; 01856 pMo[1][3] = pMi[3][1]; 01857 01858 pMo[2][0] = pMi[0][2]; 01859 pMo[2][1] = pMi[1][2]; 01860 pMo[2][2] = pMi[2][2]; 01861 pMo[2][3] = pMi[3][2]; 01862 01863 pMo[3][0] = pMi[0][3]; 01864 pMo[3][1] = pMi[1][3]; 01865 pMo[3][2] = pMi[2][3]; 01866 pMo[3][3] = pMi[3][3]; 01867 } 01868 01869 return pMo; 01870 } 01871 01872 /* 01873 * KmMadj : Matrix Adjoint 01874 * 01875 * INPUT 01876 * pMi: input matrix Mi 01877 * 01878 * OUTPUT 01879 * pMo: output matrix Mo = adj(Mi) 01880 * 01881 * RETURN VALUE 01882 * pMo 01883 * 01884 * REMARKS 01885 * - Doesn't work in-place 01886 */ 01887 K_INLINE double (*KmMadj( 01888 double (*const pMo)[4], 01889 const double (*const pMi)[4]))[4] 01890 { 01891 double D01, D02, D03, D12, D13, D23; 01892 01893 KM_ASSERT(pMo != (double (*)[4]) pMi, "KmMadj() doesn't work in-place"); 01894 01895 D01 = pMi[0][2] * pMi[1][3] - pMi[0][3] * pMi[1][2]; 01896 D02 = pMi[0][2] * pMi[2][3] - pMi[0][3] * pMi[2][2]; 01897 D03 = pMi[0][2] * pMi[3][3] - pMi[0][3] * pMi[3][2]; 01898 D12 = pMi[1][2] * pMi[2][3] - pMi[1][3] * pMi[2][2]; 01899 D13 = pMi[1][2] * pMi[3][3] - pMi[1][3] * pMi[3][2]; 01900 D23 = pMi[2][2] * pMi[3][3] - pMi[2][3] * pMi[3][2]; 01901 01902 pMo[0][0] = pMi[1][1] * D23 - pMi[2][1] * D13 + pMi[3][1] * D12; 01903 pMo[0][1] = pMi[0][1] * D23 - pMi[2][1] * D03 + pMi[3][1] * D02; 01904 pMo[0][2] = pMi[0][1] * D13 - pMi[1][1] * D03 + pMi[3][1] * D01; 01905 pMo[0][3] = pMi[0][1] * D12 - pMi[1][1] * D02 + pMi[2][1] * D01; 01906 pMo[1][0] = pMi[1][0] * D23 - pMi[2][0] * D13 + pMi[3][0] * D12; 01907 pMo[1][1] = pMi[0][0] * D23 - pMi[2][0] * D03 + pMi[3][0] * D02; 01908 pMo[1][2] = pMi[0][0] * D13 - pMi[1][0] * D03 + pMi[3][0] * D01; 01909 pMo[1][3] = pMi[0][0] * D12 - pMi[1][0] * D02 + pMi[2][0] * D01; 01910 01911 D01 = pMi[0][0] * pMi[1][1] - pMi[0][1] * pMi[1][0]; 01912 D02 = pMi[0][0] * pMi[2][1] - pMi[0][1] * pMi[2][0]; 01913 D03 = pMi[0][0] * pMi[3][1] - pMi[0][1] * pMi[3][0]; 01914 D12 = pMi[1][0] * pMi[2][1] - pMi[1][1] * pMi[2][0]; 01915 D13 = pMi[1][0] * pMi[3][1] - pMi[1][1] * pMi[3][0]; 01916 D23 = pMi[2][0] * pMi[3][1] - pMi[2][1] * pMi[3][0]; 01917 01918 pMo[2][0] = pMi[1][3] * D23 - pMi[2][3] * D13 + pMi[3][3] * D12; 01919 pMo[2][1] = pMi[0][3] * D23 - pMi[2][3] * D03 + pMi[3][3] * D02; 01920 pMo[2][2] = pMi[0][3] * D13 - pMi[1][3] * D03 + pMi[3][3] * D01; 01921 pMo[2][3] = pMi[0][3] * D12 - pMi[1][3] * D02 + pMi[2][3] * D01; 01922 pMo[3][0] = pMi[1][2] * D23 - pMi[2][2] * D13 + pMi[3][2] * D12; 01923 pMo[3][1] = pMi[0][2] * D23 - pMi[2][2] * D03 + pMi[3][2] * D02; 01924 pMo[3][2] = pMi[0][2] * D13 - pMi[1][2] * D03 + pMi[3][2] * D01; 01925 pMo[3][3] = pMi[0][2] * D12 - pMi[1][2] * D02 + pMi[2][2] * D01; 01926 01927 return pMo; 01928 } 01929 01930 /* 01931 * KmLU : Matrix LU 01932 * 01933 * RETURN VALUE 01934 * 0.0 if singular 01935 */ 01936 #ifdef KARCH_DEV_MSC 01937 #pragma warning(disable: 4706) 01938 #endif 01939 K_INLINE double (*KmLU(double (*const pM)[4]))[4] 01940 { 01941 double T0, T1; 01942 int I0, I1; 01943 int Index[4]; 01944 01945 if((T0 = kAbs(pM[0][0])) > (T1 = kAbs(pM[0][1]))) { 01946 if(T0 > (T1 = kAbs(pM[0][2]))) { 01947 I0 = (T0 > kAbs(pM[0][3])) ? 0 : 3; 01948 } else { 01949 I0 = (kAbs(pM[0][3]) > T1) ? 3 : 2; 01950 } 01951 } else { 01952 if((T0 = kAbs(pM[0][2])) > T1) { 01953 I0 = (T0 > kAbs(pM[0][3])) ? 2 : 3; 01954 } else { 01955 I0 = (kAbs(pM[0][3]) > T1) ? 3 : 1; 01956 } 01957 } 01958 01959 Index[0] = 0; 01960 Index[1] = 1; 01961 Index[2] = 2; 01962 Index[3] = 3; 01963 01964 if(I0 != 0) { 01965 kSwap(Index[0], Index[I0], I1); 01966 } 01967 01968 if((T0 = pM[0][Index[0]]) != 0) { 01969 01970 T0 = 1.0 / T0; 01971 01972 pM[0][Index[1]] = pM[0][Index[1]] * T0; 01973 pM[1][Index[1]] -= pM[0][Index[1]] * pM[1][Index[0]]; 01974 pM[2][Index[1]] -= pM[0][Index[1]] * pM[2][Index[0]]; 01975 pM[3][Index[1]] -= pM[0][Index[1]] * pM[3][Index[0]]; 01976 01977 pM[0][Index[2]] = pM[0][Index[2]] * T0; 01978 pM[1][Index[2]] -= pM[0][Index[2]] * pM[1][Index[0]]; 01979 pM[2][Index[2]] -= pM[0][Index[2]] * pM[2][Index[0]]; 01980 pM[3][Index[2]] -= pM[0][Index[2]] * pM[3][Index[0]]; 01981 01982 pM[0][Index[3]] = pM[0][Index[3]] * T0; 01983 pM[1][Index[3]] -= pM[0][Index[3]] * pM[1][Index[0]]; 01984 pM[2][Index[3]] -= pM[0][Index[3]] * pM[2][Index[0]]; 01985 pM[3][Index[3]] -= pM[0][Index[3]] * pM[3][Index[0]]; 01986 01987 if((T0 = kAbs(pM[1][Index[1]])) > (T1 = kAbs(pM[1][Index[2]]))) { 01988 I0 = (T0 > kAbs(pM[1][Index[3]])) ? 1 : 3; 01989 } else { 01990 I0 = (kAbs(pM[1][Index[3]]) > T1) ? 3 : 2; 01991 } 01992 01993 if(I0 != 1) { 01994 kSwap(Index[1], Index[I0], I1); 01995 } 01996 01997 if((T0 = pM[1][Index[1]]) != 0) { 01998 01999 T0 = 1.0 / T0; 02000 02001 pM[1][Index[2]] = pM[1][Index[2]] * T0; 02002 pM[2][Index[2]] -= pM[1][Index[2]] * pM[2][Index[1]]; 02003 pM[3][Index[2]] -= pM[1][Index[2]] * pM[3][Index[1]]; 02004 02005 pM[1][Index[3]] = pM[1][Index[3]] * T0; 02006 pM[2][Index[3]] -= pM[1][Index[3]] * pM[2][Index[1]]; 02007 pM[3][Index[3]] -= pM[1][Index[3]] * pM[3][Index[1]]; 02008 02009 I0 = (kAbs(pM[2][Index[2]]) > kAbs(pM[2][Index[3]])) ? 2 : 3; 02010 02011 if(I0 != 2) { 02012 kSwap(Index[2], Index[I0], I1); 02013 } 02014 02015 if((T0 = pM[2][Index[2]]) != 0) { 02016 02017 pM[2][Index[3]] = pM[2][Index[3]] / T0; 02018 pM[3][Index[3]] -= pM[2][Index[3]] * pM[3][Index[2]]; 02019 02020 pM[4][0] = (double) Index[0]; 02021 pM[4][1] = (double) Index[1]; 02022 pM[4][2] = (double) Index[2]; 02023 pM[4][3] = (double) Index[3]; 02024 02025 return pM[3][Index[3]] ? pM : NULL; 02026 } 02027 } 02028 } 02029 02030 return NULL; 02031 } 02032 #ifdef KARCH_DEV_MSC 02033 #pragma warning(default: 4706) 02034 #endif 02035 02036 /* 02037 * KmMneg : Matrix Negate 02038 * 02039 * INPUT 02040 * pMi: input matrix Mi 02041 * 02042 * OUTPUT 02043 * pMo: output matrix Mo = -Mi 02044 * 02045 * RETURN VALUE 02046 * pMo 02047 * 02048 * REMARKS 02049 * - Works in-place 02050 */ 02051 K_INLINE double (*KmMneg( 02052 double (*const pMo)[4], 02053 KM_CONST double (*const pMi)[4]))[4] 02054 { 02055 pMo[0][0] = -pMi[0][0]; 02056 pMo[0][1] = -pMi[0][1]; 02057 pMo[0][2] = -pMi[0][2]; 02058 pMo[0][3] = -pMi[0][3]; 02059 02060 pMo[1][0] = -pMi[1][0]; 02061 pMo[1][1] = -pMi[1][1]; 02062 pMo[1][2] = -pMi[1][2]; 02063 pMo[1][3] = -pMi[1][3]; 02064 02065 pMo[2][0] = -pMi[2][0]; 02066 pMo[2][1] = -pMi[2][1]; 02067 pMo[2][2] = -pMi[2][2]; 02068 pMo[2][3] = -pMi[2][3]; 02069 02070 pMo[3][0] = -pMi[3][0]; 02071 pMo[3][1] = -pMi[3][1]; 02072 pMo[3][2] = -pMi[3][2]; 02073 pMo[3][3] = -pMi[3][3]; 02074 02075 return pMo; 02076 } 02077 02078 /* 02079 * KmLUM : LU Matrix Times Matrix 02080 * 02081 * INPUT 02082 * pLUi: input LU matrix LUi 02083 * pMi: input matrix Mi 02084 * 02085 * OUTPUT 02086 * pMo: output matrix Mo = LU * Mi 02087 * 02088 * RETURN VALUE 02089 * pMo 02090 * 02091 * REMARKS 02092 * - Doesn't work in-place 02093 * - pMi is destroyed 02094 */ 02095 K_INLINE double (*KmLUM(double (*const pMo)[4], const double (*const pLUi)[4], double (*const pMi)[4]))[4] 02096 { 02097 int I0, I1, I2, I3; 02098 02099 KM_ASSERT((pMo != (double (*)[4]) pLUi) && (pMo != pMi), "KmLUM() doesn't work in-place"); 02100 02101 I0 = (int) pLUi[4][0]; 02102 I1 = (int) pLUi[4][1]; 02103 I2 = (int) pLUi[4][2]; 02104 I3 = (int) pLUi[4][3]; 02105 02106 pMi[0][I1] -= pLUi[0][I1] * pMi[0][I0]; 02107 pMi[0][I2] -= pLUi[0][I2] * pMi[0][I0]; 02108 pMi[0][I3] -= pLUi[0][I3] * pMi[0][I0]; 02109 pMi[0][I2] -= pLUi[1][I2] * pMi[0][I1]; 02110 pMi[0][I3] -= pLUi[1][I3] * pMi[0][I1]; 02111 pMi[0][I3] -= pLUi[2][I3] * pMi[0][I2]; 02112 02113 pMo[0][3] = pMi[0][I3] / pLUi[3][I3]; 02114 pMo[0][2] = (pMi[0][I2] - pLUi[3][I2] * pMo[0][3]) / pLUi[2][I2]; 02115 pMo[0][1] = (pMi[0][I1] - pLUi[2][I1] * pMo[0][2] - pLUi[3][I1] * pMo[0][3]) / pLUi[1][I1]; 02116 pMo[0][0] = (pMi[0][I0] - pLUi[1][I0] * pMo[0][1] - pLUi[2][I0] * pMo[0][2] - pLUi[3][I0] * pMo[0][3]) / pLUi[0][I0]; 02117 02118 pMi[1][I1] -= pLUi[0][I1] * pMi[1][I0]; 02119 pMi[1][I2] -= pLUi[0][I2] * pMi[1][I0]; 02120 pMi[1][I3] -= pLUi[0][I3] * pMi[1][I0]; 02121 pMi[1][I2] -= pLUi[1][I2] * pMi[1][I1]; 02122 pMi[1][I3] -= pLUi[1][I3] * pMi[1][I1]; 02123 pMi[1][I3] -= pLUi[2][I3] * pMi[1][I2]; 02124 02125 pMo[1][3] = pMi[1][I3] / pLUi[3][I3]; 02126 pMo[1][2] = (pMi[1][I2] - pLUi[3][I2] * pMo[1][3]) / pLUi[2][I2]; 02127 pMo[1][1] = (pMi[1][I1] - pLUi[2][I1] * pMo[1][2] - pLUi[3][I1] * pMo[1][3]) / pLUi[1][I1]; 02128 pMo[1][0] = (pMi[1][I0] - pLUi[1][I0] * pMo[1][1] - pLUi[2][I0] * pMo[1][2] - pLUi[3][I0] * pMo[1][3]) / pLUi[0][I0]; 02129 02130 pMi[2][I1] -= pLUi[0][I1] * pMi[2][I0]; 02131 pMi[2][I2] -= pLUi[0][I2] * pMi[2][I0]; 02132 pMi[2][I3] -= pLUi[0][I3] * pMi[2][I0]; 02133 pMi[2][I2] -= pLUi[1][I2] * pMi[2][I1]; 02134 pMi[2][I3] -= pLUi[1][I3] * pMi[2][I1]; 02135 pMi[2][I3] -= pLUi[2][I3] * pMi[2][I2]; 02136 02137 pMo[2][3] = pMi[2][I3] / pLUi[3][I3]; 02138 pMo[2][2] = (pMi[2][I2] - pLUi[3][I2] * pMo[2][3]) / pLUi[2][I2]; 02139 pMo[2][1] = (pMi[2][I1] - pLUi[2][I1] * pMo[2][2] - pLUi[3][I1] * pMo[2][3]) / pLUi[1][I1]; 02140 pMo[2][0] = (pMi[2][I0] - pLUi[1][I0] * pMo[2][1] - pLUi[2][I0] * pMo[2][2] - pLUi[3][I0] * pMo[2][3]) / pLUi[0][I0]; 02141 02142 pMi[3][I1] -= pLUi[0][I1] * pMi[3][I0]; 02143 pMi[3][I2] -= pLUi[0][I2] * pMi[3][I0]; 02144 pMi[3][I3] -= pLUi[0][I3] * pMi[3][I0]; 02145 pMi[3][I2] -= pLUi[1][I2] * pMi[3][I1]; 02146 pMi[3][I3] -= pLUi[1][I3] * pMi[3][I1]; 02147 pMi[3][I3] -= pLUi[2][I3] * pMi[3][I2]; 02148 02149 pMo[3][3] = pMi[3][I3] / pLUi[3][I3]; 02150 pMo[3][2] = (pMi[3][I2] - pLUi[3][I2] * pMo[3][3]) / pLUi[2][I2]; 02151 pMo[3][1] = (pMi[3][I1] - pLUi[2][I1] * pMo[3][2] - pLUi[3][I1] * pMo[3][3]) / pLUi[1][I1]; 02152 pMo[3][0] = (pMi[3][I0] - pLUi[1][I0] * pMo[3][1] - pLUi[2][I0] * pMo[3][2] - pLUi[3][I0] * pMo[3][3]) / pLUi[0][I0]; 02153 02154 return pMo; 02155 } 02156 02157 /* 02158 * KmMi : Matrix Inverse 02159 * 02160 * INPUT 02161 * pMi: input matrix Mi 02162 * 02163 * OUTPUT 02164 * pMo: output matrix Mo = Mi^-1 02165 * 02166 * RETURN VALUE 02167 * pMo 02168 * 02169 * REMARKS 02170 * - Works in-place 02171 */ 02172 K_INLINE double (*KmMi(double (*const pMo)[4], KM_CONST double (*const pMi)[4]))[4] 02173 { 02174 double LU[5][4]; 02175 02176 (void) KmMset(LU, (const double (*)[4]) pMi); 02177 02178 if(KmLU(LU)) { 02179 02180 double B[4][4]; 02181 02182 (void) KmId(B); 02183 (void) KmLUM(pMo, (const double (*)[4]) LU, B); 02184 02185 return pMo; 02186 } 02187 02188 return NULL; 02189 } 02190 02191 /* 02192 * KmAi : Affine Matrix Inverse 02193 * 02194 * INPUT 02195 * pAi: input matrix Ai 02196 * 02197 * OUTPUT 02198 * pAo: output matrix Ao = Ai^-1 02199 * 02200 * RETURN VALUE 02201 * pAo 02202 * 02203 * REMARKS 02204 * - Works in-place 02205 * 02206 * BUGS 02207 * - Doesn't work in-place with 'assume no aliasing' MVC's optimization 02208 */ 02209 #if defined(_MSC_VER) && (_MSC_VER < 1400) 02210 #pragma optimize("a", off) 02211 #endif 02212 02213 K_INLINE double (*KmAi( 02214 double (*const pAo)[4], 02215 KM_CONST double (*const pAi)[4]))[4] 02216 { 02217 #if 1 02218 double A0, A1, A2; 02219 double S0, S1, S2; 02220 02221 A0 = pAi[0][0]; 02222 A1 = pAi[0][1]; 02223 A2 = pAi[0][2]; 02224 S0 = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 02225 02226 A0 = pAi[1][0]; 02227 A1 = pAi[1][1]; 02228 A2 = pAi[1][2]; 02229 S1 = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 02230 02231 A0 = pAi[2][0]; 02232 A1 = pAi[2][1]; 02233 A2 = pAi[2][2]; 02234 S2 = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 02235 02236 A2 = pAi[1][0]; 02237 02238 pAo[1][0] = S0 * pAi[0][1]; 02239 pAo[2][0] = S0 * pAi[0][2]; 02240 pAo[2][1] = S1 * pAi[1][2]; 02241 02242 pAo[0][1] = S1 * A2; 02243 pAo[0][2] = S2 * A0; 02244 pAo[1][2] = S2 * A1; 02245 02246 pAo[0][0] = S0 * pAi[0][0]; 02247 pAo[1][1] = S1 * pAi[1][1]; 02248 pAo[2][2] = S2 * pAi[2][2]; 02249 02250 A0 = -pAi[3][0]; 02251 A1 = -pAi[3][1]; 02252 A2 = -pAi[3][2]; 02253 02254 pAo[3][2] = pAo[0][2] * A0 + pAo[1][2] * A1 + pAo[2][2] * A2; 02255 pAo[3][0] = pAo[0][0] * A0 + pAo[1][0] * A1 + pAo[2][0] * A2; 02256 pAo[3][1] = pAo[0][1] * A0 + pAo[1][1] * A1 + pAo[2][1] * A2; 02257 #else 02258 // check this on MIPS 02259 double A0, A1, A2, T0, T1, TX, TY, TZ, S; 02260 02261 T0 = pAi[1][0]; 02262 T1 = pAi[2][0]; 02263 02264 TX = -pAi[3][0]; 02265 TY = -pAi[3][1]; 02266 TZ = -pAi[3][2]; 02267 02268 A0 = pAi[0][0]; 02269 A1 = pAi[0][1]; 02270 A2 = pAi[0][2]; 02271 S = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 02272 02273 A0 *= S; 02274 A1 *= S; 02275 A2 *= S; 02276 pAo[0][0] = A0; 02277 pAo[1][0] = A1; 02278 pAo[2][0] = A2; 02279 pAo[3][0] = A0 * TX + A1 * TY + A2 * TZ; 02280 02281 A0 = T0; 02282 A1 = pAi[1][1]; 02283 A2 = pAi[1][2]; 02284 S = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 02285 02286 T0 = pAi[2][1]; 02287 02288 A0 *= S; 02289 A1 *= S; 02290 A2 *= S; 02291 pAo[0][1] = A0; 02292 pAo[1][1] = A1; 02293 pAo[2][1] = A2; 02294 pAo[3][1] = A0 * TX + A1 * TY + A2 * TZ; 02295 02296 A0 = T1; 02297 A1 = T0; 02298 A2 = pAi[2][2]; 02299 S = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 02300 02301 A0 *= S; 02302 A1 *= S; 02303 A2 *= S; 02304 pAo[0][2] = A0; 02305 pAo[1][2] = A1; 02306 pAo[2][2] = A2; 02307 pAo[3][2] = A0 * TX + A1 * TY + A2 * TZ; 02308 #endif 02309 return pAo; 02310 } 02311 #ifdef KARCH_DEV_MSC 02312 #pragma optimize("", on) 02313 #endif 02314 02315 /* 02316 * KmTi : Translation Matrix Inverse 02317 * 02318 * INPUT 02319 * pTi: input matrix Ti 02320 * 02321 * OUTPUT 02322 * pTo: output matrix To = Ti^-1 02323 * 02324 * RETURN VALUE 02325 * pTo 02326 * 02327 * REMARKS 02328 * - Works in-place 02329 */ 02330 K_INLINE double (*KmTi( 02331 double (*const pTo)[4], 02332 KM_CONST double (*const pTi)[4]))[4] 02333 { 02334 pTo[3][0] = -pTi[3][0]; 02335 pTo[3][1] = -pTi[3][1]; 02336 pTo[3][2] = -pTi[3][2]; 02337 02338 return pTo; 02339 } 02340 02341 /* 02342 * KmRi : Rotation Matrix Inverse 02343 * 02344 * INPUT 02345 * pRi: input matrix Ri 02346 * 02347 * OUTPUT 02348 * pRo: output matrix Ro = Ri^-1 02349 * 02350 * RETURN VALUE 02351 * pRo 02352 * 02353 * REMARKS 02354 * - Works in-place 02355 */ 02356 K_INLINE double (*KmRi( 02357 double (*const pRo)[4], 02358 KM_CONST double (*const pRi)[4]))[4] 02359 { 02360 if(pRo == pRi) { 02361 02362 double T; 02363 02364 kSwap(pRo[0][1], pRo[1][0], T); 02365 kSwap(pRo[0][2], pRo[2][0], T); 02366 kSwap(pRo[1][2], pRo[2][1], T); 02367 02368 } else { 02369 02370 pRo[0][0] = pRi[0][0]; 02371 pRo[0][1] = pRi[1][0]; 02372 pRo[0][2] = pRi[2][0]; 02373 02374 pRo[1][0] = pRi[0][1]; 02375 pRo[1][1] = pRi[1][1]; 02376 pRo[1][2] = pRi[2][1]; 02377 02378 pRo[2][0] = pRi[0][2]; 02379 pRo[2][1] = pRi[1][2]; 02380 pRo[2][2] = pRi[2][2]; 02381 } 02382 02383 return pRo; 02384 } 02385 02386 /* 02387 * KmSi : Scaling Matrix Inverse 02388 * 02389 * INPUT 02390 * pSi: input matrix Si 02391 * 02392 * OUTPUT 02393 * pSo: output matrix So = Si^-1 02394 * 02395 * RETURN VALUE 02396 * pSo 02397 * 02398 * REMARKS 02399 * - Works in-place 02400 */ 02401 K_INLINE double (*KmSi( 02402 double (*const pSo)[4], 02403 KM_CONST double (*const pSi)[4]))[4] 02404 { 02405 pSo[0][0] = 1.0 / pSi[0][0]; 02406 pSo[1][1] = 1.0 / pSi[1][1]; 02407 pSo[2][2] = 1.0 / pSi[2][2]; 02408 02409 return pSo; 02410 } 02411 02412 /* 02413 * KmMadd : Matrix Add 02414 * 02415 * INPUT 02416 * pMiA: input matrix MiA 02417 * pMiB: input matrix MiB 02418 * 02419 * OUTPUT 02420 * pMo: output matrix Mo = MiA + MiB 02421 * 02422 * RETURN VALUE 02423 * pMo 02424 * 02425 * REMARKS 02426 * - Works in-place 02427 */ 02428 K_INLINE double (*KmMadd( 02429 double (*const pMo)[4], 02430 KM_CONST double (*const pMiA)[4], KM_CONST double (*const pMiB)[4]))[4] 02431 { 02432 pMo[0][0] = pMiA[0][0] + pMiB[0][0]; 02433 pMo[0][1] = pMiA[0][1] + pMiB[0][1]; 02434 pMo[0][2] = pMiA[0][2] + pMiB[0][2]; 02435 pMo[0][3] = pMiA[0][3] + pMiB[0][3]; 02436 02437 pMo[1][0] = pMiA[1][0] + pMiB[1][0]; 02438 pMo[1][1] = pMiA[1][1] + pMiB[1][1]; 02439 pMo[1][2] = pMiA[1][2] + pMiB[1][2]; 02440 pMo[1][3] = pMiA[1][3] + pMiB[1][3]; 02441 02442 pMo[2][0] = pMiA[2][0] + pMiB[2][0]; 02443 pMo[2][1] = pMiA[2][1] + pMiB[2][1]; 02444 pMo[2][2] = pMiA[2][2] + pMiB[2][2]; 02445 pMo[2][3] = pMiA[2][3] + pMiB[2][3]; 02446 02447 pMo[3][0] = pMiA[3][0] + pMiB[3][0]; 02448 pMo[3][1] = pMiA[3][1] + pMiB[3][1]; 02449 pMo[3][2] = pMiA[3][2] + pMiB[3][2]; 02450 pMo[3][3] = pMiA[3][3] + pMiB[3][3]; 02451 02452 return pMo; 02453 } 02454 02455 /* 02456 * KmMadd : Matrix Substract 02457 * 02458 * INPUT 02459 * pMiA: input matrix MiA 02460 * pMiB: input matrix MiB 02461 * 02462 * OUTPUT 02463 * pMo: output matrix Mo = MiA - MiB 02464 * 02465 * RETURN VALUE 02466 * pMo 02467 * 02468 * REMARKS 02469 * - Works in-place 02470 */ 02471 K_INLINE double (*KmMsub( 02472 double (*const pMo)[4], 02473 KM_CONST double (*const pMiA)[4], KM_CONST double (*const pMiB)[4]))[4] 02474 { 02475 pMo[0][0] = pMiA[0][0] - pMiB[0][0]; 02476 pMo[0][1] = pMiA[0][1] - pMiB[0][1]; 02477 pMo[0][2] = pMiA[0][2] - pMiB[0][2]; 02478 pMo[0][3] = pMiA[0][3] - pMiB[0][3]; 02479 02480 pMo[1][0] = pMiA[1][0] - pMiB[1][0]; 02481 pMo[1][1] = pMiA[1][1] - pMiB[1][1]; 02482 pMo[1][2] = pMiA[1][2] - pMiB[1][2]; 02483 pMo[1][3] = pMiA[1][3] - pMiB[1][3]; 02484 02485 pMo[2][0] = pMiA[2][0] - pMiB[2][0]; 02486 pMo[2][1] = pMiA[2][1] - pMiB[2][1]; 02487 pMo[2][2] = pMiA[2][2] - pMiB[2][2]; 02488 pMo[2][3] = pMiA[2][3] - pMiB[2][3]; 02489 02490 pMo[3][0] = pMiA[3][0] - pMiB[3][0]; 02491 pMo[3][1] = pMiA[3][1] - pMiB[3][1]; 02492 pMo[3][2] = pMiA[3][2] - pMiB[3][2]; 02493 pMo[3][3] = pMiA[3][3] - pMiB[3][3]; 02494 02495 return pMo; 02496 } 02497 02498 /* 02499 * KmMN : Matrix Scale 02500 * 02501 * INPUT 02502 * pMi: input matrix Mi 02503 * pNi: input scalar Ni 02504 * 02505 * OUTPUT 02506 * pMo: output matrix Mo = Mi * Ni 02507 * 02508 * RETURN VALUE 02509 * pMo 02510 * 02511 * REMARKS 02512 * - Works in-place 02513 */ 02514 K_INLINE double (*KmMN( 02515 double (*const pMo)[4], 02516 KM_CONST double (*const pMiA)[4], const double pNi))[4] 02517 { 02518 pMo[0][0] = pMiA[0][0] * pNi; 02519 pMo[0][1] = pMiA[0][1] * pNi; 02520 pMo[0][2] = pMiA[0][2] * pNi; 02521 pMo[0][3] = pMiA[0][3] * pNi; 02522 02523 pMo[1][0] = pMiA[1][0] * pNi; 02524 pMo[1][1] = pMiA[1][1] * pNi; 02525 pMo[1][2] = pMiA[1][2] * pNi; 02526 pMo[1][3] = pMiA[1][3] * pNi; 02527 02528 pMo[2][0] = pMiA[2][0] * pNi; 02529 pMo[2][1] = pMiA[2][1] * pNi; 02530 pMo[2][2] = pMiA[2][2] * pNi; 02531 pMo[2][3] = pMiA[2][3] * pNi; 02532 02533 pMo[3][0] = pMiA[3][0] * pNi; 02534 pMo[3][1] = pMiA[3][1] * pNi; 02535 pMo[3][2] = pMiA[3][2] * pNi; 02536 pMo[3][3] = pMiA[3][3] * pNi; 02537 02538 return pMo; 02539 } 02540 02541 /* 02542 * KmMM : Matrix Product 02543 * 02544 * INPUT 02545 * pMiA: input matrix MiA 02546 * pMiB: input matrix MiB 02547 * 02548 * OUTPUT 02549 * pMo: output matrix Mo = MiA * MiB 02550 * 02551 * RETURN VALUE 02552 * pMo 02553 * 02554 * REMARKS 02555 * - Works in-place 02556 */ 02557 K_INLINE double (*KmMM( 02558 double (*const pMo)[4], 02559 KM_CONST double (*const pMiA)[4], KM_CONST double (*const pMiB)[4]))[4] 02560 { 02561 double M0, M1, M2, M3; 02562 02563 if(pMo == pMiA) { 02564 02565 M0 = pMiA[0][0]; 02566 M1 = pMiA[1][0]; 02567 M2 = pMiA[2][0]; 02568 M3 = pMiA[3][0]; 02569 02570 pMo[0][0] = pMiB[0][0] * M0 + pMiB[0][1] * M1 + pMiB[0][2] * M2 + pMiB[0][3] * M3; 02571 pMo[1][0] = pMiB[1][0] * M0 + pMiB[1][1] * M1 + pMiB[1][2] * M2 + pMiB[1][3] * M3; 02572 pMo[2][0] = pMiB[2][0] * M0 + pMiB[2][1] * M1 + pMiB[2][2] * M2 + pMiB[2][3] * M3; 02573 pMo[3][0] = pMiB[3][0] * M0 + pMiB[3][1] * M1 + pMiB[3][2] * M2 + pMiB[3][3] * M3; 02574 02575 M0 = pMiA[0][1]; 02576 M1 = pMiA[1][1]; 02577 M2 = pMiA[2][1]; 02578 M3 = pMiA[3][1]; 02579 02580 pMo[0][1] = pMiB[0][0] * M0 + pMiB[0][1] * M1 + pMiB[0][2] * M2 + pMiB[0][3] * M3; 02581 pMo[1][1] = pMiB[1][0] * M0 + pMiB[1][1] * M1 + pMiB[1][2] * M2 + pMiB[1][3] * M3; 02582 pMo[2][1] = pMiB[2][0] * M0 + pMiB[2][1] * M1 + pMiB[2][2] * M2 + pMiB[2][3] * M3; 02583 pMo[3][1] = pMiB[3][0] * M0 + pMiB[3][1] * M1 + pMiB[3][2] * M2 + pMiB[3][3] * M3; 02584 02585 M0 = pMiA[0][2]; 02586 M1 = pMiA[1][2]; 02587 M2 = pMiA[2][2]; 02588 M3 = pMiA[3][2]; 02589 02590 pMo[0][2] = pMiB[0][0] * M0 + pMiB[0][1] * M1 + pMiB[0][2] * M2 + pMiB[0][3] * M3; 02591 pMo[1][2] = pMiB[1][0] * M0 + pMiB[1][1] * M1 + pMiB[1][2] * M2 + pMiB[1][3] * M3; 02592 pMo[2][2] = pMiB[2][0] * M0 + pMiB[2][1] * M1 + pMiB[2][2] * M2 + pMiB[2][3] * M3; 02593 pMo[3][2] = pMiB[3][0] * M0 + pMiB[3][1] * M1 + pMiB[3][2] * M2 + pMiB[3][3] * M3; 02594 02595 M0 = pMiA[0][3]; 02596 M1 = pMiA[1][3]; 02597 M2 = pMiA[2][3]; 02598 M3 = pMiA[3][3]; 02599 02600 pMo[0][3] = pMiB[0][0] * M0 + pMiB[0][1] * M1 + pMiB[0][2] * M2 + pMiB[0][3] * M3; 02601 pMo[1][3] = pMiB[1][0] * M0 + pMiB[1][1] * M1 + pMiB[1][2] * M2 + pMiB[1][3] * M3; 02602 pMo[2][3] = pMiB[2][0] * M0 + pMiB[2][1] * M1 + pMiB[2][2] * M2 + pMiB[2][3] * M3; 02603 pMo[3][3] = pMiB[3][0] * M0 + pMiB[3][1] * M1 + pMiB[3][2] * M2 + pMiB[3][3] * M3; 02604 02605 } else { 02606 02607 M0 = pMiB[0][0]; 02608 M1 = pMiB[0][1]; 02609 M2 = pMiB[0][2]; 02610 M3 = pMiB[0][3]; 02611 02612 pMo[0][0] = pMiA[0][0] * M0 + pMiA[1][0] * M1 + pMiA[2][0] * M2 + pMiA[3][0] * M3; 02613 pMo[0][1] = pMiA[0][1] * M0 + pMiA[1][1] * M1 + pMiA[2][1] * M2 + pMiA[3][1] * M3; 02614 pMo[0][2] = pMiA[0][2] * M0 + pMiA[1][2] * M1 + pMiA[2][2] * M2 + pMiA[3][2] * M3; 02615 pMo[0][3] = pMiA[0][3] * M0 + pMiA[1][3] * M1 + pMiA[2][3] * M2 + pMiA[3][3] * M3; 02616 02617 M0 = pMiB[1][0]; 02618 M1 = pMiB[1][1]; 02619 M2 = pMiB[1][2]; 02620 M3 = pMiB[1][3]; 02621 02622 pMo[1][0] = pMiA[0][0] * M0 + pMiA[1][0] * M1 + pMiA[2][0] * M2 + pMiA[3][0] * M3; 02623 pMo[1][1] = pMiA[0][1] * M0 + pMiA[1][1] * M1 + pMiA[2][1] * M2 + pMiA[3][1] * M3; 02624 pMo[1][2] = pMiA[0][2] * M0 + pMiA[1][2] * M1 + pMiA[2][2] * M2 + pMiA[3][2] * M3; 02625 pMo[1][3] = pMiA[0][3] * M0 + pMiA[1][3] * M1 + pMiA[2][3] * M2 + pMiA[3][3] * M3; 02626 02627 M0 = pMiB[2][0]; 02628 M1 = pMiB[2][1]; 02629 M2 = pMiB[2][2]; 02630 M3 = pMiB[2][3]; 02631 02632 pMo[2][0] = pMiA[0][0] * M0 + pMiA[1][0] * M1 + pMiA[2][0] * M2 + pMiA[3][0] * M3; 02633 pMo[2][1] = pMiA[0][1] * M0 + pMiA[1][1] * M1 + pMiA[2][1] * M2 + pMiA[3][1] * M3; 02634 pMo[2][2] = pMiA[0][2] * M0 + pMiA[1][2] * M1 + pMiA[2][2] * M2 + pMiA[3][2] * M3; 02635 pMo[2][3] = pMiA[0][3] * M0 + pMiA[1][3] * M1 + pMiA[2][3] * M2 + pMiA[3][3] * M3; 02636 02637 M0 = pMiB[3][0]; 02638 M1 = pMiB[3][1]; 02639 M2 = pMiB[3][2]; 02640 M3 = pMiB[3][3]; 02641 02642 pMo[3][0] = pMiA[0][0] * M0 + pMiA[1][0] * M1 + pMiA[2][0] * M2 + pMiA[3][0] * M3; 02643 pMo[3][1] = pMiA[0][1] * M0 + pMiA[1][1] * M1 + pMiA[2][1] * M2 + pMiA[3][1] * M3; 02644 pMo[3][2] = pMiA[0][2] * M0 + pMiA[1][2] * M1 + pMiA[2][2] * M2 + pMiA[3][2] * M3; 02645 pMo[3][3] = pMiA[0][3] * M0 + pMiA[1][3] * M1 + pMiA[2][3] * M2 + pMiA[3][3] * M3; 02646 } 02647 02648 return pMo; 02649 } 02650 02651 /* 02652 * KmAA : Affine Matrix Product 02653 * 02654 * INPUT 02655 * pAiA: input matrix AiA 02656 * pAiB: input matrix AiB 02657 * 02658 * OUTPUT 02659 * pAo: output matrix Ao = AiA * AiB 02660 * 02661 * RETURN VALUE 02662 * pAo 02663 * 02664 * REMARKS 02665 * - Works in-place 02666 */ 02667 K_INLINE double (*KmAA( 02668 double (*const pAo)[4], 02669 KM_CONST double (*const pAiA)[4], KM_CONST double (*const pAiB)[4]))[4] 02670 { 02671 double A0, A1, A2; 02672 02673 if(pAo == pAiA) { 02674 02675 A0 = pAiA[0][0]; 02676 A1 = pAiA[1][0]; 02677 A2 = pAiA[2][0]; 02678 02679 pAo[0][0] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 02680 pAo[1][0] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 02681 pAo[2][0] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 02682 pAo[3][0] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 + pAiA[3][0]; 02683 02684 A0 = pAiA[0][1]; 02685 A1 = pAiA[1][1]; 02686 A2 = pAiA[2][1]; 02687 02688 pAo[0][1] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 02689 pAo[1][1] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 02690 pAo[2][1] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 02691 pAo[3][1] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 + pAiA[3][1]; 02692 02693 A0 = pAiA[0][2]; 02694 A1 = pAiA[1][2]; 02695 A2 = pAiA[2][2]; 02696 02697 pAo[0][2] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 02698 pAo[1][2] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 02699 pAo[2][2] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 02700 pAo[3][2] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 + pAiA[3][2]; 02701 02702 } else { 02703 02704 A0 = pAiB[0][0]; 02705 A1 = pAiB[0][1]; 02706 A2 = pAiB[0][2]; 02707 02708 pAo[0][0] = pAiA[0][0] * A0 + pAiA[1][0] * A1 + pAiA[2][0] * A2; 02709 pAo[0][1] = pAiA[0][1] * A0 + pAiA[1][1] * A1 + pAiA[2][1] * A2; 02710 pAo[0][2] = pAiA[0][2] * A0 + pAiA[1][2] * A1 + pAiA[2][2] * A2; 02711 02712 A0 = pAiB[1][0]; 02713 A1 = pAiB[1][1]; 02714 A2 = pAiB[1][2]; 02715 02716 pAo[1][0] = pAiA[0][0] * A0 + pAiA[1][0] * A1 + pAiA[2][0] * A2; 02717 pAo[1][1] = pAiA[0][1] * A0 + pAiA[1][1] * A1 + pAiA[2][1] * A2; 02718 pAo[1][2] = pAiA[0][2] * A0 + pAiA[1][2] * A1 + pAiA[2][2] * A2; 02719 02720 A0 = pAiB[2][0]; 02721 A1 = pAiB[2][1]; 02722 A2 = pAiB[2][2]; 02723 02724 pAo[2][0] = pAiA[0][0] * A0 + pAiA[1][0] * A1 + pAiA[2][0] * A2; 02725 pAo[2][1] = pAiA[0][1] * A0 + pAiA[1][1] * A1 + pAiA[2][1] * A2; 02726 pAo[2][2] = pAiA[0][2] * A0 + pAiA[1][2] * A1 + pAiA[2][2] * A2; 02727 02728 A0 = pAiB[3][0]; 02729 A1 = pAiB[3][1]; 02730 A2 = pAiB[3][2]; 02731 02732 pAo[3][0] = pAiA[0][0] * A0 + pAiA[1][0] * A1 + pAiA[2][0] * A2 + pAiA[3][0]; 02733 pAo[3][1] = pAiA[0][1] * A0 + pAiA[1][1] * A1 + pAiA[2][1] * A2 + pAiA[3][1]; 02734 pAo[3][2] = pAiA[0][2] * A0 + pAiA[1][2] * A1 + pAiA[2][2] * A2 + pAiA[3][2]; 02735 } 02736 02737 return pAo; 02738 } 02739 02740 /* 02741 * KmTT : Translation Matrix Product 02742 * 02743 * INPUT 02744 * pTiA: input matrix TiA 02745 * pTiB: input matrix TiB 02746 * 02747 * OUTPUT 02748 * pTo: output matrix To = TiA * TiB 02749 * 02750 * RETURN VALUE 02751 * pTo 02752 * 02753 * REMARKS 02754 * - Works in-place 02755 */ 02756 K_INLINE double (*KmTT( 02757 double (*const pTo)[4], 02758 KM_CONST double (*const pTiA)[4], KM_CONST double (*const pTiB)[4]))[4] 02759 { 02760 pTo[3][0] = pTiA[3][0] + pTiB[3][0]; 02761 pTo[3][1] = pTiA[3][1] + pTiB[3][1]; 02762 pTo[3][2] = pTiA[3][2] + pTiB[3][2]; 02763 02764 return pTo; 02765 } 02766 02767 /* 02768 * KmRR : Rotation Matrix Product 02769 * 02770 * INPUT 02771 * pRiA: input matrix RiA 02772 * pRiB: input matrix RiB 02773 * 02774 * OUTPUT 02775 * pRo: output matrix Ro = RiA * RiB 02776 * 02777 * RETURN VALUE 02778 * pRo 02779 * 02780 * REMARKS 02781 * - Works in-place 02782 */ 02783 K_INLINE double (*KmRR( 02784 double (*const pRo)[4], 02785 KM_CONST double (*const pRiA)[4], KM_CONST double (*const pRiB)[4]))[4] 02786 { 02787 double R0, R1, R2; 02788 02789 if(pRo == pRiA) { 02790 02791 R0 = pRiA[0][0]; 02792 R1 = pRiA[1][0]; 02793 R2 = pRiA[2][0]; 02794 02795 pRo[0][0] = pRiB[0][0] * R0 + pRiB[0][1] * R1 + pRiB[0][2] * R2; 02796 pRo[1][0] = pRiB[1][0] * R0 + pRiB[1][1] * R1 + pRiB[1][2] * R2; 02797 pRo[2][0] = pRiB[2][0] * R0 + pRiB[2][1] * R1 + pRiB[2][2] * R2; 02798 02799 R0 = pRiA[0][1]; 02800 R1 = pRiA[1][1]; 02801 R2 = pRiA[2][1]; 02802 02803 pRo[0][1] = pRiB[0][0] * R0 + pRiB[0][1] * R1 + pRiB[0][2] * R2; 02804 pRo[1][1] = pRiB[1][0] * R0 + pRiB[1][1] * R1 + pRiB[1][2] * R2; 02805 pRo[2][1] = pRiB[2][0] * R0 + pRiB[2][1] * R1 + pRiB[2][2] * R2; 02806 02807 R0 = pRiA[0][2]; 02808 R1 = pRiA[1][2]; 02809 R2 = pRiA[2][2]; 02810 02811 pRo[0][2] = pRiB[0][0] * R0 + pRiB[0][1] * R1 + pRiB[0][2] * R2; 02812 pRo[1][2] = pRiB[1][0] * R0 + pRiB[1][1] * R1 + pRiB[1][2] * R2; 02813 pRo[2][2] = pRiB[2][0] * R0 + pRiB[2][1] * R1 + pRiB[2][2] * R2; 02814 02815 } else { 02816 02817 R0 = pRiB[0][0]; 02818 R1 = pRiB[0][1]; 02819 R2 = pRiB[0][2]; 02820 02821 pRo[0][0] = pRiA[0][0] * R0 + pRiA[1][0] * R1 + pRiA[2][0] * R2; 02822 pRo[0][1] = pRiA[0][1] * R0 + pRiA[1][1] * R1 + pRiA[2][1] * R2; 02823 pRo[0][2] = pRiA[0][2] * R0 + pRiA[1][2] * R1 + pRiA[2][2] * R2; 02824 02825 R0 = pRiB[1][0]; 02826 R1 = pRiB[1][1]; 02827 R2 = pRiB[1][2]; 02828 02829 pRo[1][0] = pRiA[0][0] * R0 + pRiA[1][0] * R1 + pRiA[2][0] * R2; 02830 pRo[1][1] = pRiA[0][1] * R0 + pRiA[1][1] * R1 + pRiA[2][1] * R2; 02831 pRo[1][2] = pRiA[0][2] * R0 + pRiA[1][2] * R1 + pRiA[2][2] * R2; 02832 02833 R0 = pRiB[2][0]; 02834 R1 = pRiB[2][1]; 02835 R2 = pRiB[2][2]; 02836 02837 pRo[2][0] = pRiA[0][0] * R0 + pRiA[1][0] * R1 + pRiA[2][0] * R2; 02838 pRo[2][1] = pRiA[0][1] * R0 + pRiA[1][1] * R1 + pRiA[2][1] * R2; 02839 pRo[2][2] = pRiA[0][2] * R0 + pRiA[1][2] * R1 + pRiA[2][2] * R2; 02840 } 02841 02842 return pRo; 02843 } 02844 02845 /* 02846 * KmSS : Scaling Matrix Product 02847 * 02848 * INPUT 02849 * pSiA: input matrix SiA 02850 * pSiB: input matrix SiB 02851 * 02852 * OUTPUT 02853 * pSo: output matrix So = SiA * SiB 02854 * 02855 * RETURN VALUE 02856 * pSo 02857 * 02858 * REMARKS 02859 * - Works in-place 02860 */ 02861 K_INLINE double (*KmSS( 02862 double (*const pSo)[4], 02863 KM_CONST double (*const pSiA)[4], 02864 KM_CONST double (*const pSiB)[4]))[4] 02865 { 02866 pSo[0][0] = pSiA[0][0] * pSiB[0][0]; 02867 pSo[1][1] = pSiA[1][1] * pSiB[1][1]; 02868 pSo[2][2] = pSiA[2][2] * pSiB[2][2]; 02869 02870 return pSo; 02871 } 02872 02873 /* 02874 * KmTR : T by R Matrix Product 02875 * 02876 * INPUT 02877 * pTi: input matrix Ti 02878 * pRi: input matrix Ri 02879 * 02880 * OUTPUT 02881 * pAo: output matrix Ao = Ti * Ri 02882 * 02883 * RETURN VALUE 02884 * pAo 02885 * 02886 * REMARKS 02887 * - Doesn't work in-place 02888 */ 02889 K_INLINE double (*KmTR( 02890 double (*const pAo)[4], 02891 const double (*const pTi)[4], const double (*const pRi)[4]))[4] 02892 { 02893 KM_ASSERT((pAo != (double (*)[4]) pTi) && (pAo != (double (*)[4]) pRi), "KmTR() doesn't work in-place"); 02894 02895 return KmTset(KmRset(pAo, pRi), pTi); 02896 } 02897 02898 /* 02899 * KmTRS : T by R by S Matrix Product 02900 * 02901 * INPUT 02902 * pTi: input matrix Ti 02903 * pRi: input matrix Ri 02904 * pSi: input matrix Si 02905 * 02906 * OUTPUT 02907 * pAo: output matrix Ao = Ti * Ri * Si 02908 * 02909 * RETURN VALUE 02910 * pAo 02911 * 02912 * REMARKS 02913 * - Doesn't work in-place 02914 */ 02915 K_INLINE double (*KmTRS( 02916 double (*const pAo)[4], 02917 const double (*const pTi)[4], 02918 const double (*const pRi)[4], 02919 const double (*const pSi)[4]))[4] 02920 { 02921 double T; 02922 02923 KM_ASSERT((pAo != (double (*)[4]) pTi) && (pAo != (double (*)[4]) pRi) && (pAo != (double (*)[4]) pSi), "KmTRS() doesn't work in-place"); 02924 02925 (void) KmTset(pAo, pTi); 02926 02927 T = pSi[0][0]; 02928 02929 pAo[0][0] = pRi[0][0] * T; 02930 pAo[0][1] = pRi[0][1] * T; 02931 pAo[0][2] = pRi[0][2] * T; 02932 02933 T = pSi[1][1]; 02934 02935 pAo[1][0] = pRi[1][0] * T; 02936 pAo[1][1] = pRi[1][1] * T; 02937 pAo[1][2] = pRi[1][2] * T; 02938 02939 T = pSi[2][2]; 02940 02941 pAo[2][0] = pRi[2][0] * T; 02942 pAo[2][1] = pRi[2][1] * T; 02943 pAo[2][2] = pRi[2][2] * T; 02944 02945 return pAo; 02946 } 02947 02948 /* 02949 * KmMiM : Matrix Inverse Product 02950 * 02951 * INPUT 02952 * pMiA: input matrix MiA 02953 * pMiB: input matrix MiB 02954 * 02955 * OUTPUT 02956 * pMo: output matrix Mo = MiA^-1 * MiB 02957 * 02958 * RETURN VALUE 02959 * pMo 02960 * 02961 * REMARKS 02962 * - Works in-place 02963 */ 02964 K_INLINE double (*KmMiM( 02965 double (*const pMo)[4], 02966 KM_CONST double (*const pMiA)[4], 02967 KM_CONST double (*const pMiB)[4]))[4] 02968 { 02969 double LU[5][4]; 02970 02971 (void) KmMset(LU, (const double (*)[4]) pMiA); 02972 02973 if(KmLU(LU)) { 02974 02975 double B[4][4]; 02976 02977 (void) KmMset(B, (const double (*)[4]) pMiB); 02978 (void) KmLUM(pMo, (const double (*)[4]) LU, B); 02979 02980 return pMo; 02981 } 02982 02983 return NULL; 02984 } 02985 02986 /* 02987 * KmAiA : Affine Matrix Inverse Product 02988 * 02989 * INPUT 02990 * pAiA: input matrix AiA 02991 * pAiB: input matrix AiB 02992 * 02993 * OUTPUT 02994 * pAo: output matrix Ao = AiA^-1 * AiB 02995 * 02996 * RETURN VALUE 02997 * pAo 02998 * 02999 * REMARKS 03000 * - Works in-place 03001 */ 03002 K_INLINE double (*KmAiA( 03003 double (*const pAo)[4], 03004 KM_CONST double (*const pAiA)[4], 03005 KM_CONST double (*const pAiB)[4]))[4] 03006 { 03007 double T[4][4]; 03008 double A0, A1, A2; 03009 double S; 03010 03011 if(pAo == pAiB) { 03012 03013 (void) KmAi(T, pAiA); 03014 (void) KmAA(pAo, T, pAiB); 03015 03016 } else if(pAo == pAiA) { 03017 03018 double T0, T1, T2, T3; 03019 03020 A0 = pAiA[0][0]; 03021 A1 = pAiA[0][1]; 03022 A2 = pAiA[0][2]; 03023 S = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 03024 03025 A0 = S * pAiA[0][0]; 03026 A1 = S * pAiA[0][1]; 03027 A2 = S * pAiA[0][2]; 03028 03029 T0 = pAiA[1][0]; 03030 T1 = pAiA[2][0]; 03031 T2 = pAiA[3][0]; 03032 T3 = pAiA[3][1]; 03033 03034 pAo[0][0] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 03035 pAo[1][0] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 03036 pAo[2][0] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 03037 pAo[3][0] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 - (A0 * T2 + A1 * T3 + A2 * pAiA[3][2]); 03038 03039 A1 = pAiA[1][1]; 03040 A2 = pAiA[1][2]; 03041 S = 1.0 / (T0 * T0 + A1 * A1 + A2 * A2); 03042 03043 A0 = S * T0; 03044 A1 *= S; 03045 A2 *= S; 03046 03047 T0 = pAiA[2][1]; 03048 03049 pAo[0][1] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 03050 pAo[1][1] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 03051 pAo[2][1] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 03052 pAo[3][1] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 - (A0 * T2 + A1 * T3 + A2 * pAiA[3][2]); 03053 03054 A2 = pAiA[2][2]; 03055 S = 1.0 / (T1 * T1 + T0 * T0 + A2 * A2); 03056 03057 A0 = S * T1; 03058 A1 = S * T0; 03059 A2 = S * A2; 03060 03061 pAo[0][2] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 03062 pAo[1][2] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 03063 pAo[2][2] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 03064 pAo[3][2] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 - (A0 * T2 + A1 * T3 + A2 * pAiA[3][2]); 03065 03066 } else { 03067 03068 double A0, A1, A2, T0, T1, T2, S; 03069 03070 T0 = pAiA[3][0]; 03071 T1 = pAiA[3][1]; 03072 T2 = pAiA[3][2]; 03073 03074 A0 = pAiA[0][0]; 03075 A1 = pAiA[0][1]; 03076 A2 = pAiA[0][2]; 03077 S = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 03078 03079 A0 *= S; 03080 A1 *= S; 03081 A2 *= S; 03082 03083 pAo[0][0] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 03084 pAo[1][0] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 03085 pAo[2][0] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 03086 pAo[3][0] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 - (A0 * T0 + A1 * T1 + A2 * T2); 03087 03088 A0 = pAiA[1][0]; 03089 A1 = pAiA[1][1]; 03090 A2 = pAiA[1][2]; 03091 S = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 03092 03093 A0 *= S; 03094 A1 *= S; 03095 A2 *= S; 03096 03097 pAo[0][1] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 03098 pAo[1][1] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 03099 pAo[2][1] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 03100 pAo[3][1] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 - (A0 * T0 + A1 * T1 + A2 * T2); 03101 03102 A0 = pAiA[2][0]; 03103 A1 = pAiA[2][1]; 03104 A2 = pAiA[2][2]; 03105 S = 1.0 / (A0 * A0 + A1 * A1 + A2 * A2); 03106 03107 A0 *= S; 03108 A1 *= S; 03109 A2 *= S; 03110 03111 pAo[0][2] = pAiB[0][0] * A0 + pAiB[0][1] * A1 + pAiB[0][2] * A2; 03112 pAo[1][2] = pAiB[1][0] * A0 + pAiB[1][1] * A1 + pAiB[1][2] * A2; 03113 pAo[2][2] = pAiB[2][0] * A0 + pAiB[2][1] * A1 + pAiB[2][2] * A2; 03114 pAo[3][2] = pAiB[3][0] * A0 + pAiB[3][1] * A1 + pAiB[3][2] * A2 - (A0 * T0 + A1 * T1 + A2 * T2); 03115 } 03116 03117 return pAo; 03118 03119 } 03120 03121 /* 03122 * KmTiT : Translation Matrix Inverse Product 03123 * 03124 * INPUT 03125 * pTiA: input matrix TiA 03126 * pTiB: input matrix TiB 03127 * 03128 * OUTPUT 03129 * pTo: output matrix To = TiA^-1 * TiB 03130 * 03131 * RETURN VALUE 03132 * pTo 03133 * 03134 * REMARKS 03135 * - Works in-place 03136 */ 03137 K_INLINE double (*KmTiT( 03138 double (*const pTo)[4], 03139 KM_CONST double (*const pTiA)[4], 03140 KM_CONST double (*const pTiB)[4]))[4] 03141 { 03142 pTo[3][0] = pTiB[3][0] - pTiA[3][0]; 03143 pTo[3][1] = pTiB[3][1] - pTiA[3][1]; 03144 pTo[3][2] = pTiB[3][2] - pTiA[3][2]; 03145 03146 return pTo; 03147 } 03148 03149 /* 03150 * KmRiR : Rotation Matrix Inverse Product 03151 * 03152 * INPUT 03153 * pRiA: input matrix RiA 03154 * pRiB: input matrix RiB 03155 * 03156 * OUTPUT 03157 * pRo: output matrix Ro = RiA^-1 * RiB 03158 * 03159 * RETURN VALUE 03160 * pRo 03161 * 03162 * REMARKS 03163 * - Works in-place 03164 */ 03165 K_INLINE double (*KmRiR( 03166 double (*const pRo)[4], 03167 KM_CONST double (*const pRiA)[4], 03168 KM_CONST double (*const pRiB)[4]))[4] 03169 { 03170 double R0, R1, R2; 03171 03172 if(pRo == pRiA) { 03173 03174 double T0, T1; 03175 03176 T0 = pRiA[0][2]; 03177 T1 = pRiA[1][2]; 03178 03179 R0 = pRiA[2][0]; 03180 R1 = pRiA[2][1]; 03181 R2 = pRiA[2][2]; 03182 03183 pRo[2][2] = R0 * pRiB[2][0] + R1 * pRiB[2][1] + R2 * pRiB[2][2]; 03184 pRo[1][2] = R0 * pRiB[1][0] + R1 * pRiB[1][1] + R2 * pRiB[1][2]; 03185 pRo[0][2] = R0 * pRiB[0][0] + R1 * pRiB[0][1] + R2 * pRiB[0][2]; 03186 03187 R0 = pRiA[1][0]; 03188 R1 = pRiA[0][1]; 03189 R2 = pRiA[1][1]; 03190 03191 pRo[2][1] = R0 * pRiB[2][0] + R2 * pRiB[2][1] + T1 * pRiB[2][2]; 03192 pRo[1][1] = R0 * pRiB[1][0] + R2 * pRiB[1][1] + T1 * pRiB[1][2]; 03193 pRo[0][1] = R0 * pRiB[0][0] + R2 * pRiB[0][1] + T1 * pRiB[0][2]; 03194 03195 R0 = pRiA[0][0]; 03196 03197 pRo[2][0] = R0 * pRiB[2][0] + R1 * pRiB[2][1] + T0 * pRiB[2][2]; 03198 pRo[1][0] = R0 * pRiB[1][0] + R1 * pRiB[1][1] + T0 * pRiB[1][2]; 03199 pRo[0][0] = R0 * pRiB[0][0] + R1 * pRiB[0][1] + T0 * pRiB[0][2]; 03200 03201 } else { 03202 03203 R0 = pRiB[0][0]; 03204 R1 = pRiB[0][1]; 03205 R2 = pRiB[0][2]; 03206 03207 pRo[0][0] = pRiA[0][0] * R0 + pRiA[0][1] * R1 + pRiA[0][2] * R2; 03208 pRo[0][1] = pRiA[1][0] * R0 + pRiA[1][1] * R1 + pRiA[1][2] * R2; 03209 pRo[0][2] = pRiA[2][0] * R0 + pRiA[2][1] * R1 + pRiA[2][2] * R2; 03210 03211 R0 = pRiB[1][0]; 03212 R1 = pRiB[1][1]; 03213 R2 = pRiB[1][2]; 03214 03215 pRo[1][0] = pRiA[0][0] * R0 + pRiA[0][1] * R1 + pRiA[0][2] * R2; 03216 pRo[1][1] = pRiA[1][0] * R0 + pRiA[1][1] * R1 + pRiA[1][2] * R2; 03217 pRo[1][2] = pRiA[2][0] * R0 + pRiA[2][1] * R1 + pRiA[2][2] * R2; 03218 03219 R0 = pRiB[2][0]; 03220 R1 = pRiB[2][1]; 03221 R2 = pRiB[2][2]; 03222 03223 pRo[2][0] = pRiA[0][0] * R0 + pRiA[0][1] * R1 + pRiA[0][2] * R2; 03224 pRo[2][1] = pRiA[1][0] * R0 + pRiA[1][1] * R1 + pRiA[1][2] * R2; 03225 pRo[2][2] = pRiA[2][0] * R0 + pRiA[2][1] * R1 + pRiA[2][2] * R2; 03226 } 03227 return pRo; 03228 } 03229 03230 /* 03231 * KmSiS : Scaling Matrix Inverse Product 03232 * 03233 * INPUT 03234 * pSiA: input matrix SiA 03235 * pSiB: input matrix SiB 03236 * 03237 * OUTPUT 03238 * pSo: output matrix So = SiA^-1 * SiB 03239 * 03240 * RETURN VALUE 03241 * pSo 03242 * 03243 * REMARKS 03244 * - Works in-place 03245 */ 03246 K_INLINE double (*KmSiS( 03247 double (*const pSo)[4], 03248 KM_CONST double (*const pSiA)[4], 03249 KM_CONST double (*const pSiB)[4]))[4] 03250 { 03251 pSo[0][0] = pSiB[0][0] / pSiA[0][0]; 03252 pSo[1][1] = pSiB[1][1] / pSiA[1][1]; 03253 pSo[2][2] = pSiB[2][2] / pSiA[2][2]; 03254 03255 return pSo; 03256 } 03257 03259 // 03260 // Matrix To Vector Decompositions 03261 // 03263 03264 /* 03265 * KmTtoV : Translation Matrix To Vector 03266 * 03267 * INPUT 03268 * pTi: input matrix Ti 03269 * 03270 * OUTPUT 03271 * pVo: output vector Vo = T(Ti) 03272 * 03273 * RETURN VALUE 03274 * pTo 03275 */ 03276 K_INLINE double *KmTtoV(double *const pVo, const double (*const pTi)[4]) 03277 { 03278 pVo[0] = pTi[3][0]; 03279 pVo[1] = pTi[3][1]; 03280 pVo[2] = pTi[3][2]; 03281 03282 return pVo; 03283 } 03284 03285 /* 03286 * KmRtoVXYZ : Rotation Matrix to XYZ Euler Vector 03287 * 03288 * INPUT 03289 * pRi: input matrix Ri 03290 * 03291 * OUTPUT 03292 * pVo: output vector Vo = RXYZ(Ri) 03293 * 03294 * RETURN VALUE 03295 * pVo 03296 * 03297 * REMARKS 03298 * - Rotations are post-multiplied 03299 */ 03300 K_INLINE double *KmRtoVXYZ(double *const pVo, const double (*const pRi)[4]) 03301 { 03302 if(pRi[0][2] >= 1.0 - KM_TOLERANCE) { 03303 pVo[0] = kAtand(-pRi[2][1], -pRi[2][0]); 03304 pVo[1] = -90; 03305 pVo[2] = 0; 03306 } else if(pRi[0][2] <= KM_TOLERANCE - 1.0) { 03307 pVo[0] = kAtand(pRi[1][0], pRi[1][1]); 03308 pVo[1] = 90; 03309 pVo[2] = 0; 03310 } else { 03311 pVo[0] = kAtand(pRi[1][2], pRi[2][2]); 03312 pVo[1] = -kAsind(pRi[0][2]); 03313 pVo[2] = kAtand(pRi[0][1], pRi[0][0]); 03314 } 03315 return pVo; 03316 } 03317 03318 /* 03319 * KmRtoVZYX : Rotation Matrix To ZYX Euler Vector 03320 * 03321 * INPUT 03322 * pRi: input matrix Ri 03323 * 03324 * OUTPUT 03325 * pVo: output vector Vo = RZYX(Ri) 03326 * 03327 * RETURN VALUE 03328 * pVo 03329 * 03330 * REMARKS 03331 * - Rotations are post-multiplied 03332 */ 03333 K_INLINE double *KmRtoVZYX(double *const pVo, const double (*const pRi)[4]) 03334 { 03335 if(pRi[2][0] >= 1.0 - KM_TOLERANCE) { 03336 pVo[2] = kAtand(pRi[1][2], -pRi[0][2]); 03337 pVo[1] = 90.0; 03338 pVo[0] = 0.0; 03339 } else if(pRi[2][0] <= KM_TOLERANCE - 1.0) { 03340 pVo[2] = kAtand(-pRi[0][1], pRi[1][1]); 03341 pVo[1] = -90.0; 03342 pVo[0] = 0.0; 03343 } else { 03344 pVo[2] = kAtand(-pRi[2][1], pRi[2][2]); 03345 pVo[1] = kAsind(pRi[2][0]); 03346 pVo[0] = kAtand(-pRi[1][0], pRi[0][0]); 03347 } 03348 return pVo; 03349 } 03350 03351 /* 03352 * KmRtoVord : Rotation Matrix To Ordered Euler Vector 03353 * 03354 * INPUT 03355 * pRi: input matrix Ri 03356 * pOrd: order (Ord) of rotations 03357 * 03358 * OUTPUT 03359 * pVo: output vector Vo = R(Ri, Ord) 03360 * 03361 * RETURN VALUE 03362 * pVo 03363 * 03364 * REMARKS 03365 * - Rotations are post-multiplied 03366 */ 03367 K_INLINE double *KmRtoVord(double *const pVo, const double (*const pRi)[4], const int pOrd) 03368 { 03369 int I, J, K; 03370 double T; 03371 03372 I = KmEulerAxis[pOrd][0]; 03373 J = KmEulerAxis[pOrd][1]; 03374 K = KmEulerAxis[pOrd][2]; 03375 03376 if(pOrd & KM_EULER_REPEAT_YES) { 03377 03378 T = kHypot(pRi[J][I], pRi[K][I]); 03379 if(T > 16.0 * KM_EPSILON) { 03380 pVo[0] = kAtand(pRi[J][I], pRi[K][I]); 03381 pVo[1] = kAtand(T, pRi[I][I]); 03382 pVo[2] = kAtand(pRi[I][J], -pRi[I][K]); 03383 } else { 03384 pVo[0] = kAtand(-pRi[K][J], pRi[J][J]); 03385 pVo[1] = kAtand(T, pRi[I][I]); 03386 pVo[2] = 0.0; 03387 } 03388 03389 } else { 03390 03391 T = kHypot(pRi[I][I], pRi[I][J]); 03392 if(T > 16.0 * KM_EPSILON) { 03393 pVo[0] = kAtand(pRi[J][K], pRi[K][K]); 03394 pVo[1] = kAtand(-pRi[I][K], T); 03395 pVo[2] = kAtand(pRi[I][J], pRi[I][I]); 03396 } else { 03397 pVo[0] = kAtand(-pRi[K][J], pRi[J][J]); 03398 pVo[1] = kAtand(-pRi[I][K], T); 03399 pVo[2] = 0.0; 03400 } 03401 } 03402 03403 if(pOrd & KM_EULER_PARITY_ODD) { 03404 (void) KmVneg(pVo, pVo); 03405 } 03406 03407 return pVo; 03408 } 03409 03410 /* 03411 * KmRtoV : Default Order Rotation Matrix To Euler Vector 03412 * 03413 * INPUT 03414 * pRi: input matrix Ri 03415 * 03416 * OUTPUT 03417 * pVo: output vector Vo = RXYZ(Ri) 03418 * 03419 * RETURN VALUE 03420 * pVo 03421 * 03422 * REMARKS 03423 * - Rotations are post-multiplied 03424 */ 03425 K_INLINE double *KmRtoV(double *const pVo, const double (*const pRi)[4]) 03426 { 03427 return KmRtoVXYZ(pVo, pRi); 03428 } 03429 03430 /* 03431 * KmIsRightHand: Affine Matrix To Vector 03432 * 03433 * INPUT 03434 * pAi: input matrix Ai 03435 * 03436 * OUTPUT 03437 * pBool: if Matrix if right hand 03438 */ 03439 K_INLINE bool KmIsRightHand(const double (*pAi)[4]) 03440 { 03441 double VectorialProduct[4]; 03442 03443 KmVV(VectorialProduct,pAi[0],pAi[1]); 03444 return (KmVdotV(VectorialProduct,pAi[2])>=0); 03445 } 03446 03447 /* 03448 * KmAtoV : Affine Matrix To Vector 03449 * 03450 * INPUT 03451 * pAi: input matrix Ai 03452 * 03453 * OUTPUT 03454 * pTo: output vector To = T(Ai) 03455 * pRo: output vector Ro = R(Ai) 03456 * pSo: output vector So = S(Ai) 03457 */ 03458 K_INLINE void KmAtoV(double *pTo, double *pRo, double *pSo, const double (*pAi)[4]) 03459 { 03460 double A[4][4]; 03461 03462 KmAset(A, pAi); 03463 03464 if (KmIsRightHand(A)) { 03465 pSo[0] = KmVlength(A[0]); 03466 pSo[1] = KmVlength(A[1]); 03467 pSo[2] = KmVlength(A[2]); 03468 } else { 03469 pSo[0] = -KmVlength(A[0]); 03470 pSo[1] = -KmVlength(A[1]); 03471 pSo[2] = -KmVlength(A[2]); 03472 } 03473 03474 KM_ASSERT(pSo[0] * pSo[1] * pSo[2] != 0.0, "Zero scaling factor found in KmAtoV()"); 03475 03476 KmVN(A[0], A[0], 1.0 / pSo[0]); 03477 KmVN(A[1], A[1], 1.0 / pSo[1]); 03478 KmVN(A[2], A[2], 1.0 / pSo[2]); 03479 03480 (void) KmRtoV(pRo, (const double (*)[4]) A); 03481 03482 pTo[0] = A[3][0]; 03483 pTo[1] = A[3][1]; 03484 pTo[2] = A[3][2]; 03485 } 03486 03487 /* 03488 * KmAtoVord : Affine Matrix To Ordered Vector 03489 * 03490 * INPUT 03491 * pAi: input matrix Ai 03492 * pOrd: rotation order 03493 * 03494 * OUTPUT 03495 * pTo: output vector To = T(Ti) 03496 * pRo: output vector Ro = R(Ai) 03497 * pSo: output vector So = S(Ti) 03498 */ 03499 K_INLINE void KmAtoVord(double *pTo, double *pRo, double *pSo, const double (*pAi)[4], const int pOrd) 03500 { 03501 double A[4][4]; 03502 03503 KmAset(A, pAi); 03504 03505 if (KmIsRightHand(A)) { 03506 pSo[0] = KmVlength(A[0]); 03507 pSo[1] = KmVlength(A[1]); 03508 pSo[2] = KmVlength(A[2]); 03509 } else { 03510 pSo[0] = -KmVlength(A[0]); 03511 pSo[1] = -KmVlength(A[1]); 03512 pSo[2] = -KmVlength(A[2]); 03513 } 03514 03515 KM_ASSERT(pSo[0] * pSo[1] * pSo[2] != 0.0, "Zero scaling factor found in KmAtoV()"); 03516 03517 KmVN(A[0], A[0], 1.0 / pSo[0]); 03518 KmVN(A[1], A[1], 1.0 / pSo[1]); 03519 KmVN(A[2], A[2], 1.0 / pSo[2]); 03520 03521 (void) KmRtoVord(pRo, (const double (*)[4]) A, pOrd); 03522 03523 pTo[0] = A[3][0]; 03524 pTo[1] = A[3][1]; 03525 pTo[2] = A[3][2]; 03526 } 03527 03528 /* 03529 * KmAtoVT : Affine Matrix To Translation Vector 03530 * 03531 * INPUT 03532 * pAi: input matrix Ai 03533 * 03534 * OUTPUT 03535 * pTo: output vector To = T(Ai) 03536 */ 03537 K_INLINE double *KmAtoVT(double *pTo, const double (*pAi)[4]) 03538 { 03539 pTo[0] = pAi[3][0]; 03540 pTo[1] = pAi[3][1]; 03541 pTo[2] = pAi[3][2]; 03542 03543 return pTo; 03544 } 03545 03546 /* 03547 * KmAtoVR : Affine Matrix To Rotation Vector 03548 * 03549 * INPUT 03550 * pAi: input matrix Ai 03551 * 03552 * OUTPUT 03553 * pRo: output vector Ro = R(Ai) 03554 */ 03555 K_INLINE double *KmAtoVR(double *pRo, const double (*pAi)[4]) 03556 { 03557 double A[4][4]; 03558 double S0, S1, S2; 03559 03560 (void) KmAset(A, pAi); 03561 03562 if (KmIsRightHand(A)) { 03563 S0 = KmVlength(A[0]); 03564 S1 = KmVlength(A[1]); 03565 S2 = KmVlength(A[2]); 03566 } else { 03567 S0 = -KmVlength(A[0]); 03568 S1 = -KmVlength(A[1]); 03569 S2 = -KmVlength(A[2]); 03570 } 03571 03572 KM_ASSERT(S0 * S1 * S2 != 0.0, "Zero scaling factor found in KmAtoVR()"); 03573 03574 (void) KmVN(A[0], A[0], 1.0 / S0); 03575 (void) KmVN(A[1], A[1], 1.0 / S1); 03576 (void) KmVN(A[2], A[2], 1.0 / S2); 03577 03578 return KmRtoV(pRo, (const double (*)[4]) A); 03579 } 03580 03581 /* 03582 * KmAtoVRord : Affine Matrix To Ordered Rotation Vector 03583 * 03584 * INPUT 03585 * pAi: input matrix Ai 03586 * pOrd: rotation order 03587 * 03588 * OUTPUT 03589 * pRo: output vector Ro = R(Ai) 03590 */ 03591 K_INLINE double *KmAtoVRord(double *pRo, const double (*pAi)[4], const int pOrd) 03592 { 03593 double A[4][4]; 03594 double S0, S1, S2; 03595 03596 KmAset(A, pAi); 03597 03598 if (KmIsRightHand(A)) { 03599 S0 = KmVlength(A[0]); 03600 S1 = KmVlength(A[1]); 03601 S2 = KmVlength(A[2]); 03602 } else { 03603 S0 = -KmVlength(A[0]); 03604 S1 = -KmVlength(A[1]); 03605 S2 = -KmVlength(A[2]); 03606 } 03607 03608 KM_ASSERT(S0 * S1 * S2 != 0.0, "Zero scaling factor found in KmAtoVRord()"); 03609 03610 KmVN(A[0], A[0], 1.0 / S0); 03611 KmVN(A[1], A[1], 1.0 / S1); 03612 KmVN(A[2], A[2], 1.0 / S2); 03613 03614 return KmRtoVord(pRo, (const double (*)[4]) A, pOrd); 03615 } 03616 03617 /* 03618 * KmAtoVS : Affine Matrix To Scaling Vector 03619 * 03620 * INPUT 03621 * pAi: input matrix Ai 03622 * 03623 * OUTPUT 03624 * pSo: output vector So = S(Ai) 03625 */ 03626 K_INLINE double *KmAtoVS(double *pSo, const double (*pAi)[4]) 03627 { 03628 if (KmIsRightHand(pAi)) { 03629 pSo[0] = KmVlength(pAi[0]); 03630 pSo[1] = KmVlength(pAi[1]); 03631 pSo[2] = KmVlength(pAi[2]); 03632 } else { 03633 pSo[0] = -KmVlength(pAi[0]); 03634 pSo[1] = -KmVlength(pAi[1]); 03635 pSo[2] = -KmVlength(pAi[2]); 03636 } 03637 03638 KM_ASSERT(pSo[0] * pSo[1] * pSo[2] != 0.0, "Zero scaling factor found in KmAtoVS()"); 03639 03640 return pSo; 03641 } 03642 03644 // 03645 // Matrix Compositions 03646 // 03648 03649 /* 03650 * KmXYZtoT : Scalar To Translation Matrix 03651 * 03652 * INPUT 03653 * pX: input value X 03654 * pY: input value Y 03655 * pZ: input value Z 03656 * 03657 * OUTPUT 03658 * pTo: output matrix To = T(X, Y, Z) 03659 * 03660 * RETURN VALUE 03661 * pTo 03662 */ 03663 K_INLINE double (*KmXYZtoT(double (*const pTo)[4], const double pX, const double pY, const double pZ))[4] 03664 { 03665 pTo[3][0] = pX; 03666 pTo[3][1] = pY; 03667 pTo[3][2] = pZ; 03668 03669 return pTo; 03670 } 03671 03672 /* 03673 * KmVtoT : Vector To Translation Matrix 03674 * 03675 * INPUT 03676 * pVi: input vector Vi 03677 * 03678 * OUTPUT 03679 * pTo: output matrix To = T(Vi) 03680 * 03681 * RETURN VALUE 03682 * pTo 03683 */ 03684 K_INLINE double (*KmVtoT(double (*const pTo)[4], const double *const pVi))[4] 03685 { 03686 return KmXYZtoT(pTo, pVi[0], pVi[1], pVi[2]); 03687 } 03688 03689 /* 03690 * KmDtoR : Direction Vector To Rotation Matrix 03691 * 03692 * INPUT 03693 * pDi: input direction vector Vi 03694 * pRoll: optional roll angle 03695 * 03696 * OUTPUT 03697 * pRo: output matrix Ro = R(Vi) 03698 * 03699 * RETURN VALUE 03700 * pRo 03701 */ 03702 K_INLINE double (*KmDtoR(double (*const pRo)[4], const double *const pDi, K_DEFAULT(double pRoll, 0.0)))[4] 03703 { 03704 double CY, CZ, SZ, T, X, Y, Z; 03705 03706 T = KmVlength(pDi); 03707 03708 KM_ASSERT(T != 0.0, "Null vector as direction"); 03709 03710 T = 1.0 / T; 03711 03712 X = pDi[0] * T; 03713 Y = pDi[1] * T; 03714 Z = pDi[2] * T; 03715 03716 T = X * X + Y * Y; 03717 if(T != 0.0) { 03718 T = kRsqrt(T); 03719 CZ = X * T; 03720 SZ = Y * T; 03721 } else { 03722 CZ = 1.0; 03723 SZ = 0.0; 03724 } 03725 03726 CY = kSqrt(1.0 - Z * Z); 03727 03728 if(pRoll != 0.0) { 03729 03730 double CX, SX; 03731 03732 SX = kSinCosd(pRoll, &CX); 03733 03734 pRo[0][0] = X; 03735 pRo[0][1] = Y; 03736 pRo[0][2] = Z; 03737 pRo[1][0] = -SZ * CX - CZ * Z * SX; 03738 pRo[1][1] = CZ * CX - SZ * Z * SX; 03739 pRo[1][2] = CY * SX; 03740 pRo[2][0] = SZ * SX - CZ * Z; 03741 pRo[2][1] = -CZ * SX - SZ * Z; 03742 pRo[2][2] = CY * CX; 03743 03744 } else { 03745 03746 pRo[0][0] = X; 03747 pRo[0][1] = Y; 03748 pRo[0][2] = Z; 03749 pRo[1][0] = -SZ; 03750 pRo[1][1] = CZ; 03751 pRo[1][2] = 0.0; 03752 pRo[2][0] = -CZ * Z; 03753 pRo[2][1] = -SZ * Z; 03754 pRo[2][2] = CY; 03755 } 03756 03757 return pRo; 03758 } 03759 03760 /* 03761 * KmRtoD : Rotation Matrix To Direction Vector 03762 * 03763 * INPUT 03764 * pRi: input direction vector Ri 03765 * pLength: length of output vector 03766 * 03767 * OUTPUT 03768 * pDo: output vector Do = D(Ri) 03769 * 03770 * RETURN VALUE 03771 * pDo 03772 */ 03773 K_INLINE double *KmRtoD(double *const pDo, const double (*const pRi)[4], K_DEFAULT(const double pLength, 1.0)) 03774 { 03775 pDo[0] = pRi[0][0] * pLength; 03776 pDo[1] = pRi[0][1] * pLength; 03777 pDo[2] = pRi[0][2] * pLength; 03778 03779 return pDo; 03780 } 03781 03782 /* 03783 * KmRunroll : Euler Angle Un-Roller 03784 * 03785 * INPUT 03786 * pVi: input angle vector 03787 * pRi: reference angle vector 03788 * 03789 * OUTPUT 03790 * pVo: output vector Vo 03791 * 03792 * RETURN VALUE 03793 * pVo 03794 * 03795 * REMARKS 03796 * - Find nearest angle representation to reference vector 03797 */ 03798 K_INLINE double *KmRunroll( 03799 double *const pVo, 03800 KM_CONST double *const pVi, 03801 const double *const pRi) 03802 { 03803 double U0, U1, U2, V0, V1, V2; 03804 03805 U0 = V0 = pVi[0]; 03806 U1 = V1 = pVi[1]; 03807 U2 = V2 = pVi[2]; 03808 03809 U0 += 360.0 * floor((pRi[0] - V0) / 360.0 + 0.5); 03810 U1 += 360.0 * floor((pRi[1] - V1) / 360.0 + 0.5); 03811 U2 += 360.0 * floor((pRi[2] - V2) / 360.0 + 0.5); 03812 03813 V0 += 360.0 * floor((pRi[0] - V0 - 180.0) / 360.0 + 0.5) + 180.0; 03814 V1 = 360.0 * floor((pRi[1] + V1 - 180.0) / 360.0 + 0.5) + 180.0 - V1; 03815 V2 += 360.0 * floor((pRi[2] - V0 - 180.0) / 360.0 + 0.5) + 180.0; 03816 03817 if(kAbs(pRi[0] - U0) + kAbs(pRi[1] - U1) + kAbs(pRi[2] - U2) < kAbs(pRi[0] - V0) + kAbs(pRi[1] - V1) + kAbs(pRi[2] - V2)) { 03818 pVo[0] = U0; 03819 pVo[1] = U1; 03820 pVo[2] = U2; 03821 } else { 03822 pVo[0] = V0; 03823 pVo[1] = V1; 03824 pVo[2] = V2; 03825 } 03826 03827 return pVo; 03828 } 03829 03830 /* 03831 * KmXYZtoR : XYZ Euler Angles To Rotation Matrix 03832 * 03833 * INPUT 03834 * pX: input angle X 03835 * pY: input angle Y 03836 * pZ: input angle Z 03837 * 03838 * OUTPUT 03839 * pRo: output matrix Ro = RXYZ(X, Y, Z) 03840 * 03841 * RETURN VALUE 03842 * pRo 03843 * 03844 * REMARKS 03845 * - Rotations are post-multiplied 03846 */ 03847 K_INLINE double (*KmXYZtoR(double (*const pRo)[4], double pX, double pY, double pZ))[4] 03848 { 03849 double SX, CX, SY, CY, SZ, CZ; 03850 double SYSX, SYCX; 03851 03852 SX = kSinCosd(pX, &CX); 03853 SY = kSinCosd(pY, &CY); 03854 SZ = kSinCosd(pZ, &CZ); 03855 03856 SYSX = SY * SX; 03857 SYCX = SY * CX; 03858 03859 pRo[0][0] = CZ * CY; 03860 pRo[0][1] = SZ * CY; 03861 pRo[0][2] = -SY; 03862 03863 pRo[1][0] = -SZ * CX + CZ * SYSX; 03864 pRo[1][1] = CZ * CX + SZ * SYSX; 03865 pRo[1][2] = CY * SX; 03866 03867 pRo[2][0] = SZ * SX + CZ * SYCX; 03868 pRo[2][1] = -CZ * SX + SZ * SYCX; 03869 pRo[2][2] = CY * CX; 03870 03871 return pRo; 03872 } 03873 03874 /* 03875 * KmYZXtoR : YZX Euler Angles To Rotation Matrix 03876 * 03877 * INPUT 03878 * pY: input angle pY 03879 * pZ: input angle pZ 03880 * pX: input angle pX 03881 * 03882 * OUTPUT 03883 * pRo: output matrix Ro = RYZX(Y, Z, X) 03884 * 03885 * RETURN VALUE 03886 * pRo 03887 */ 03888 K_INLINE double (*KmYZXtoR(double (*const pRo)[4], double pY, double pZ, double pX))[4] 03889 { 03890 double SX, CX, SY, CY, SZ, CZ; 03891 03892 SX = kSinCosd(pX, &CX); 03893 SY = kSinCosd(pY, &CY); 03894 SZ = kSinCosd(pZ, &CZ); 03895 03896 pRo[0][0] = CZ * CY; 03897 pRo[0][1] = SZ * CY * CX + SY * SX; 03898 pRo[0][2] = CY * SX * SZ - SY * CX; 03899 03900 pRo[1][0] = -SZ; 03901 pRo[1][1] = CZ * CX; 03902 pRo[1][2] = CZ * SX; 03903 03904 pRo[2][0] = CZ * SY; 03905 pRo[2][1] = SZ * SY * CX - CY * SX; 03906 pRo[2][2] = SZ * SY * SX + CY * CX; 03907 03908 return pRo; 03909 } 03910 03911 /* 03912 * KmZXYtoR : ZXY Euler Angles To Rotation Matrix 03913 * 03914 * INPUT 03915 * pZ: input angle Z 03916 * pX: input angle X 03917 * pY: input angle Y 03918 * 03919 * OUTPUT 03920 * pRo: output matrix Ro = RZXY(Z, X, Y) 03921 * 03922 * RETURN VALUE 03923 * pRo 03924 * 03925 * REMARKS 03926 * - Rotations are post-multiplied 03927 */ 03928 K_INLINE double (*KmZXYtoR(double (*const pRo)[4], double pZ, double pX, double pY))[4] 03929 { 03930 double SX, CX, SY, CY, SZ, CZ; 03931 03932 SX = kSinCosd(pX, &CX); 03933 SY = kSinCosd(pY, &CY); 03934 SZ = kSinCosd(pZ, &CZ); 03935 03936 pRo[0][0] = CZ * CY + SZ * SY * SX; 03937 pRo[0][1] = SZ * CX; 03938 pRo[0][2] = -CZ * SY + CY * SX * SZ; 03939 03940 pRo[1][0] = -SZ * CY + CZ * SY * SX; 03941 pRo[1][1] = CZ * CX; 03942 pRo[1][2] = SZ * SY + CY * SX * CZ; 03943 03944 pRo[2][0] = SY * CX; 03945 pRo[2][1] = -SX; 03946 pRo[2][2] = CY * CX; 03947 03948 return pRo; 03949 } 03950 03951 /* 03952 * KmXZYtoR : XZY Euler Angles To Rotation Matrix 03953 * 03954 * INPUT 03955 * pX: input angle X 03956 * pZ: input angle Z 03957 * pY: input angle Y 03958 * 03959 * OUTPUT 03960 * pRo: output matrix Ro = RXZY(X, Z, Y) 03961 * 03962 * RETURN VALUE 03963 * pRo 03964 * 03965 * REMARKS 03966 * - Rotations are post-multiplied 03967 */ 03968 K_INLINE double (*KmXZYtoR(double (*const pRo)[4], double pX, double pZ, double pY))[4] 03969 { 03970 double SX, CX, SY, CY, SZ, CZ; 03971 03972 SX = kSinCosd(pX, &CX); 03973 SY = kSinCosd(pY, &CY); 03974 SZ = kSinCosd(pZ, &CZ); 03975 03976 pRo[0][0] = CZ * CY; 03977 pRo[0][1] = SZ; 03978 pRo[0][2] = -CZ * SY; 03979 03980 pRo[1][0] = -SZ * CY * CX + SY * SX; 03981 pRo[1][1] = CZ * CX; 03982 pRo[1][2] = SZ * SY * CX + CY * SX; 03983 03984 pRo[2][0] = CY * SX * SZ + SY * CX; 03985 pRo[2][1] = -CZ * SX; 03986 pRo[2][2] = -SZ * SY * SX + CY * CX; 03987 03988 return pRo; 03989 } 03990 03991 /* 03992 * KmYXZtoR : YXZ Euler Angles To Rotation Matrix 03993 * 03994 * INPUT 03995 * pY: input angle Y 03996 * pX: input angle X 03997 * pZ: input angle Z 03998 * 03999 * OUTPUT 04000 * pRo: output matrix Ro = RYXZ(Y, X, Z) 04001 * 04002 * RETURN VALUE 04003 * pRo 04004 * 04005 * REMARKS 04006 * - Rotations are post-multiplied 04007 */ 04008 K_INLINE double (*KmYXZtoR(double (*const pRo)[4], double pY, double pX, double pZ))[4] 04009 { 04010 double SX, CX, SY, CY, SZ, CZ; 04011 04012 SX = kSinCosd(pX, &CX); 04013 SY = kSinCosd(pY, &CY); 04014 SZ = kSinCosd(pZ, &CZ); 04015 04016 pRo[0][0] = CZ * CY - SZ * SY * SX; 04017 pRo[0][1] = SZ * CY + CZ * SY * SX; 04018 pRo[0][2] = -SY * CX; 04019 04020 pRo[1][0] = -SZ * CX; 04021 pRo[1][1] = CZ * CX; 04022 pRo[1][2] = SX; 04023 04024 pRo[2][0] = CZ * SY + CY * SX * SZ; 04025 pRo[2][1] = SZ * SY - CY * SX * CZ; 04026 pRo[2][2] = CY * CX; 04027 04028 return pRo; 04029 } 04030 04031 /* 04032 * KmZYXtoR : ZYX Euler Angle To Rotation Matrix 04033 * 04034 * INPUT 04035 * pZ: input angle Z 04036 * pY: input angle Y 04037 * pX: input angle X 04038 * 04039 * OUTPUT 04040 * pRo: output matrix Ro = RZYX(Z, Y, X) 04041 * 04042 * RETURN VALUE 04043 * pRo 04044 * 04045 * REMARKS 04046 * - Rotations are post-multiplied 04047 */ 04048 K_INLINE double (*KmZYXtoR(double (*const pRo)[4], double pZ, double pY, double pX))[4] 04049 { 04050 double SX, CX, SY, CY, SZ, CZ; 04051 04052 SX = kSinCosd(pX, &CX); 04053 SY = kSinCosd(pY, &CY); 04054 SZ = kSinCosd(pZ, &CZ); 04055 04056 pRo[0][0] = CZ * CY; 04057 pRo[0][1] = CZ * SY * SX + SZ * CX; 04058 pRo[0][2] = -CZ * SY * CX + SZ * SX; 04059 04060 pRo[1][0] = -SZ * CY; 04061 pRo[1][1] = -SZ * SY * SX + CZ * CX; 04062 pRo[1][2] = SZ * SY * CX + CZ * SX; 04063 04064 pRo[2][0] = SY; 04065 pRo[2][1] = -CY * SX; 04066 pRo[2][2] = CY * CX; 04067 04068 return pRo; 04069 } 04070 04071 /* 04072 * KmIJKtoR : IJK Euler Angles To Rotation Matrix 04073 * 04074 * INPUT 04075 * pI: input angle I 04076 * pJ: input angle J 04077 * pK: input angle K 04078 * pOrd: order of rotations Ord 04079 * 04080 * OUTPUT 04081 * pRo: output matrix Ro = R(I, J, K, Ord) 04082 * 04083 * RETURN VALUE 04084 * pRo 04085 * 04086 * REMARKS 04087 * - Rotations are post-multiplied 04088 */ 04089 K_INLINE double (*KmIJKtoR(double (*const pRo)[4], double pI, double pJ, double pK, const int pOrd))[4] 04090 { 04091 double CI, CJ, CK, SI, SJ, SK, CC, CS, SC, SS; 04092 int I, J, K; 04093 04094 if(pOrd & KM_EULER_PARITY_ODD) { 04095 SI = kSinCosd(-pI, &CI); 04096 SJ = kSinCosd(-pJ, &CJ); 04097 SK = kSinCosd(-pK, &CK); 04098 } else { 04099 SI = kSinCosd(pI, &CI); 04100 SJ = kSinCosd(pJ, &CJ); 04101 SK = kSinCosd(pK, &CK); 04102 } 04103 04104 CC = CI * CK; 04105 CS = CI * SK; 04106 SC = SI * CK; 04107 SS = SI * SK; 04108 04109 I = KmEulerAxis[pOrd][0]; 04110 J = KmEulerAxis[pOrd][1]; 04111 K = KmEulerAxis[pOrd][2]; 04112 04113 if(pOrd & KM_EULER_REPEAT_YES) { 04114 04115 pRo[I][I] = CJ; 04116 pRo[J][I] = SJ * SI; 04117 pRo[K][I] = SJ * CI; 04118 pRo[I][J] = SJ * SK; 04119 pRo[J][J] = -CJ * SS + CC; 04120 pRo[K][J] = -CJ * CS - SC; 04121 pRo[I][K] = -SJ * CK; 04122 pRo[J][K] = CJ * SC + CS; 04123 pRo[K][K] = CJ * CC - SS; 04124 04125 } else { 04126 04127 pRo[I][I] = CJ * CK; 04128 pRo[J][I] = SJ * SC - CS; 04129 pRo[K][I] = SJ * CC + SS; 04130 pRo[I][J] = CJ * SK; 04131 pRo[J][J] = SJ * SS + CC; 04132 pRo[K][J] = SJ * CS - SC; 04133 pRo[I][K] = -SJ; 04134 pRo[J][K] = CJ * SI; 04135 pRo[K][K] = CJ * CI; 04136 } 04137 04138 return pRo; 04139 } 04140 04141 /* 04142 * KmVYZXtoR : YZX Euler Vector To Rotation Matrix 04143 * 04144 * INPUT 04145 * pVi: input vector Vi 04146 * 04147 * OUTPUT 04148 * pRo: output matrix Ro = RYZX(Vi) 04149 * 04150 * RETURN VALUE 04151 * pRo 04152 */ 04153 K_INLINE double (*KmVYZXtoR(double (*const pRo)[4], const double *const pVi))[4] 04154 { 04155 return KmYZXtoR(pRo, pVi[0], pVi[1], pVi[2]); 04156 } 04157 04158 /* 04159 * KmVXYZtoR : XYZ Euler Vector To Rotation Matrix 04160 * 04161 * INPUT 04162 * pVi: input vector Vi 04163 * 04164 * OUTPUT 04165 * pRo: output matrix Ro = RXYZ(Vi) 04166 * 04167 * RETURN VALUE 04168 * pRo 04169 * 04170 * REMARKS 04171 * - Rotations are post-multiplied 04172 */ 04173 K_INLINE double (*KmVXYZtoR(double (*const pRo)[4], const double *const pVi))[4] 04174 { 04175 return KmXYZtoR(pRo, pVi[0], pVi[1], pVi[2]); 04176 } 04177 04178 /* 04179 * KmVZXYtoR : ZXY Euler Vector To Rotation Matrix 04180 * 04181 * INPUT 04182 * pVi: input vector Vi 04183 * 04184 * OUTPUT 04185 * pRo: output matrix Ro = RZXY(Vi) 04186 * 04187 * RETURN VALUE 04188 * pRo 04189 * 04190 * REMARKS 04191 * - Rotations are post-multiplied 04192 */ 04193 K_INLINE double (*KmVZXYtoR(double (*const pRo)[4], const double *const pVi))[4] 04194 { 04195 return KmZXYtoR(pRo, pVi[0], pVi[1], pVi[2]); 04196 } 04197 04198 /* 04199 * KmVXZYtoR : XZY Euler Vector To Rotation Matrix 04200 * 04201 * INPUT 04202 * pVi: input vector Vi 04203 * 04204 * OUTPUT 04205 * pRo: output matrix Ro = RXZY(Vi) 04206 * 04207 * RETURN VALUE 04208 * pRo 04209 * 04210 * REMARKS 04211 * - Rotations are post-multiplied 04212 */ 04213 K_INLINE double (*KmVXZYtoR(double (*const pRo)[4], const double *const pVi))[4] 04214 { 04215 return KmXZYtoR(pRo, pVi[0], pVi[1], pVi[2]); 04216 } 04217 04218 /* 04219 * KmVYXZtoR : YXZ Euler Vector To Rotation Matrix 04220 * 04221 * INPUT 04222 * pVi: input vector Vi 04223 * 04224 * OUTPUT 04225 * pRo: output matrix Ro = RYXZ(Vi) 04226 * 04227 * RETURN VALUE 04228 * pRo 04229 * 04230 * REMARKS 04231 * - Rotations are post-multiplied 04232 */ 04233 K_INLINE double (*KmVYXZtoR(double (*const pRo)[4], const double *const pVi))[4] 04234 { 04235 return KmYXZtoR(pRo, pVi[0], pVi[1], pVi[2]); 04236 } 04237 04238 /* 04239 * KmVZYXtoR : ZYX Euler Vector To Rotation Matrix 04240 * 04241 * INPUT 04242 * pVi: input vector Vi 04243 * 04244 * OUTPUT 04245 * pRo: output matrix Ro = RZYX(Vi) 04246 * 04247 * RETURN VALUE 04248 * pRo 04249 * 04250 * REMARKS 04251 * - Rotations are post-multiplied 04252 */ 04253 K_INLINE double (*KmVZYXtoR(double (*const pRo)[4], const double *const pVi))[4] 04254 { 04255 return KmZYXtoR(pRo, pVi[0], pVi[1], pVi[2]); 04256 } 04257 04258 /* 04259 * KmVIJKtoR : IJK Euler Vector To Rotation Matrix 04260 * 04261 * INPUT 04262 * pVi: input vector Vi 04263 * pOrd: order (Ord) of rotations 04264 * 04265 * OUTPUT 04266 * pRo: output matrix Ro = R(Vi, Ord) 04267 * 04268 * RETURN VALUE 04269 * pRo 04270 * 04271 * REMARKS 04272 * - Rotations are post-multiplied 04273 */ 04274 K_INLINE double (*KmVIJKtoR(double (*const pRo)[4], const double *const pVi, const int pOrd))[4] 04275 { 04276 return KmIJKtoR(pRo, pVi[0], pVi[1], pVi[2], pOrd); 04277 } 04278 04279 /* 04280 * KmVtoR : Default Order Euler Vector To Rotation Matrix 04281 * 04282 * INPUT 04283 * pVi: input vector Vi 04284 * 04285 * OUTPUT 04286 * pRo: output matrix Ro = RXYZ(Vi) 04287 * 04288 * RETURN VALUE 04289 * pRo 04290 * 04291 * REMARKS 04292 * - Rotations are post-multiplied 04293 */ 04294 K_INLINE double (*KmVtoR(double (*const pRo)[4], const double *const pVi))[4] 04295 { 04296 return KmVXYZtoR(pRo, pVi); 04297 } 04298 04299 /* 04300 * KmXYZtoQ : XYZ Euler Angles To Quaternion 04301 * 04302 * INPUT 04303 * pX: input angle X 04304 * pY: input angle Y 04305 * pZ: input angle Z 04306 * 04307 * OUTPUT 04308 * pQo: output quaternion Qo = Q(RXYZ(X, Y, Z)) 04309 * 04310 * RETURN VALUE 04311 * pQo 04312 * 04313 * REMARKS 04314 * - Rotations are post-multiplied 04315 */ 04316 K_INLINE double *KmXYZtoQ(double *const pQo, double pX, double pY, double pZ) 04317 { 04318 double R[4][4]; 04319 04320 return KmXYZtoR(R, pX, pY, pZ), KmRtoQ(pQo, R); 04321 } 04322 04323 /* 04324 * KmYZXtoQ : YZX Euler Angles To Quaternion 04325 * 04326 * INPUT 04327 * pY: input angle pY 04328 * pZ: input angle pZ 04329 * pX: input angle pX 04330 * 04331 * OUTPUT 04332 * pQo: output quaternion Qo = Q(RYZX(Y, Z, X)) 04333 * 04334 * RETURN VALUE 04335 * pQo 04336 */ 04337 K_INLINE double *KmYZXtoQ(double *const pQo, double pY, double pZ, double pX) 04338 { 04339 double R[4][4]; 04340 04341 return KmYZXtoR(R, pY, pZ, pX), KmRtoQ(pQo, R); 04342 } 04343 04344 /* 04345 * KmZXYtoQ : ZXY Euler Angles To Quaternion 04346 * 04347 * INPUT 04348 * pZ: input angle Z 04349 * pX: input angle X 04350 * pY: input angle Y 04351 * 04352 * OUTPUT 04353 * pQo: output matrix Qo = Q(ZXY(Z, X, Y)) 04354 * 04355 * RETURN VALUE 04356 * pQo 04357 * 04358 * REMARKS 04359 * - Rotations are post-multiplied 04360 */ 04361 K_INLINE double *KmZXYtoQ(double *const pQo, double pZ, double pX, double pY) 04362 { 04363 double R[4][4]; 04364 04365 return KmZXYtoR(R, pZ, pX, pY), KmRtoQ(pQo, R); 04366 } 04367 04368 /* 04369 * KmXZYtoQ : XZY Euler Angles To Quaternion 04370 * 04371 * INPUT 04372 * pX: input angle X 04373 * pZ: input angle Z 04374 * pY: input angle Y 04375 * 04376 * OUTPUT 04377 * pQo: output quaternion Qo = Q(RXZY(X, Z, Y)) 04378 * 04379 * RETURN VALUE 04380 * pQo 04381 * 04382 * REMARKS 04383 * - Rotations are post-multiplied 04384 */ 04385 K_INLINE double *KmXZYtoQ(double *const pQo, double pX, double pZ, double pY) 04386 { 04387 double R[4][4]; 04388 04389 return KmXZYtoR(R, pX, pZ, pY), KmRtoQ(pQo, R); 04390 } 04391 04392 /* 04393 * KmYXZtoQ : YXZ Euler Angles To Quaternion 04394 * 04395 * INPUT 04396 * pY: input angle Y 04397 * pX: input angle X 04398 * pZ: input angle Z 04399 * 04400 * OUTPUT 04401 * pQo: output matrix Qo = Q(RYXZ(Y, X, Z)) 04402 * 04403 * RETURN VALUE 04404 * pQo 04405 * 04406 * REMARKS 04407 * - Rotations are post-multiplied 04408 */ 04409 K_INLINE double *KmYXZtoQ(double *const pQo, double pY, double pX, double pZ) 04410 { 04411 double R[4][4]; 04412 04413 return KmYXZtoR(R, pY, pX, pZ), KmRtoQ(pQo, R); 04414 } 04415 04416 /* 04417 * KmZYXtoQ : ZYX Euler Angle To Quaternion 04418 * 04419 * INPUT 04420 * pZ: input angle Z 04421 * pY: input angle Y 04422 * pX: input angle X 04423 * 04424 * OUTPUT 04425 * pQo: output quaternion Qo = Q(RZYX(Z, Y, X)) 04426 * 04427 * RETURN VALUE 04428 * pQo 04429 * 04430 * REMARKS 04431 * - Rotations are post-multiplied 04432 */ 04433 K_INLINE double *KmZYXtoQ(double *const pQo, double pZ, double pY, double pX) 04434 { 04435 double R[4][4]; 04436 04437 return KmZYXtoR(R, pZ, pY, pX), KmRtoQ(pQo, R); 04438 } 04439 04440 /* 04441 * KmIJKtoQ : IJK Euler Angles To Quaternion 04442 * 04443 * INPUT 04444 * pI: input angle I 04445 * pJ: input angle J 04446 * pK: input angle K 04447 * pOrd: order of rotations Ord 04448 * 04449 * OUTPUT 04450 * pQo: output quaternion Qo = Q(R(I, J, K, Ord)) 04451 * 04452 * RETURN VALUE 04453 * pQo 04454 * 04455 * REMARKS 04456 * - Rotations are post-multiplied 04457 */ 04458 K_INLINE double *KmIJKtoQ(double *const pQo, double pI, double pJ, double pK, const int pOrd) 04459 { 04460 double R[4][4]; 04461 04462 return KmIJKtoR(R, pI, pJ, pK, pOrd), KmRtoQ(pQo, R); 04463 } 04464 04465 /* 04466 * KmVYZXtoQ : YZX Euler Vector To Quaternion 04467 * 04468 * INPUT 04469 * pVi: input vector Vi 04470 * 04471 * OUTPUT 04472 * pQo: output quaternion Qo = Q(RYZX(Vi)) 04473 * 04474 * RETURN VALUE 04475 * pQo 04476 */ 04477 K_INLINE double *KmVYZXtoQ(double *const pQo, const double *const pVi) 04478 { 04479 return KmYZXtoQ(pQo, pVi[0], pVi[1], pVi[2]); 04480 } 04481 04482 /* 04483 * KmVXYZtoQ : XYZ Euler Vector To Quaternion 04484 * 04485 * INPUT 04486 * pVi: input vector Vi 04487 * 04488 * OUTPUT 04489 * pQo: output quaternion Qo = Q(RXYZ(Vi)) 04490 * 04491 * RETURN VALUE 04492 * pQo 04493 * 04494 * REMARKS 04495 * - Rotations are post-multiplied 04496 */ 04497 K_INLINE double *KmVXYZtoQ(double *const pQo, const double *const pVi) 04498 { 04499 return KmXYZtoQ(pQo, pVi[0], pVi[1], pVi[2]); 04500 } 04501 04502 /* 04503 * KmVZXYtoQ : ZXY Euler Vector To Quaternion 04504 * 04505 * INPUT 04506 * pVi: input vector Vi 04507 * 04508 * OUTPUT 04509 * pQo: output quaternion = Q(RZXY(Vi)) 04510 * 04511 * RETURN VALUE 04512 * pQo 04513 * 04514 * REMARKS 04515 * - Rotations are post-multiplied 04516 */ 04517 K_INLINE double *KmVZXYtoQ(double *const pQo, const double *const pVi) 04518 { 04519 return KmZXYtoQ(pQo, pVi[0], pVi[1], pVi[2]); 04520 } 04521 04522 /* 04523 * KmVXZYtoQ : XZY Euler Vector To Quaternion 04524 * 04525 * INPUT 04526 * pVi: input vector Vi 04527 * 04528 * OUTPUT 04529 * pQo: output quaternion Qo = Q(RXZY(Vi)) 04530 * 04531 * RETURN VALUE 04532 * pQo 04533 * 04534 * REMARKS 04535 * - Rotations are post-multiplied 04536 */ 04537 K_INLINE double *KmVXZYtoQ(double *const pQo, const double *const pVi) 04538 { 04539 return KmXZYtoQ(pQo, pVi[0], pVi[1], pVi[2]); 04540 } 04541 04542 /* 04543 * KmVYXZtoQ : YXZ Euler Vector To Quaternion 04544 * 04545 * INPUT 04546 * pVi: input vector Vi 04547 * 04548 * OUTPUT 04549 * pQo: output quaternion Qo = Q(RYXZ(Vi)) 04550 * 04551 * RETURN VALUE 04552 * pQo 04553 * 04554 * REMARKS 04555 * - Rotations are post-multiplied 04556 */ 04557 K_INLINE double *KmVYXZtoQ(double *const pQo, const double *const pVi) 04558 { 04559 return KmYXZtoQ(pQo, pVi[0], pVi[1], pVi[2]); 04560 } 04561 04562 /* 04563 * KmVZYXtoQ : ZYX Euler Vector To Quaternion 04564 * 04565 * INPUT 04566 * pVi: input vector Vi 04567 * 04568 * OUTPUT 04569 * pQo: output quaternion Qo = Q(RZYX(Vi)) 04570 * 04571 * RETURN VALUE 04572 * pQo 04573 * 04574 * REMARKS 04575 * - Rotations are post-multiplied 04576 */ 04577 K_INLINE double *KmVZYXtoQ(double *const pQo, const double *const pVi) 04578 { 04579 return KmZYXtoQ(pQo, pVi[0], pVi[1], pVi[2]); 04580 } 04581 04582 /* 04583 * KmVIJKtoQ : IJK Euler Vector To Quaternion 04584 * 04585 * INPUT 04586 * pVi: input vector Vi 04587 * pOrd: order (Ord) of rotations 04588 * 04589 * OUTPUT 04590 * pQo: output quaternion Qo = Q(R(Vi, Ord)) 04591 * 04592 * RETURN VALUE 04593 * pQo 04594 * 04595 * REMARKS 04596 * - Rotations are post-multiplied 04597 */ 04598 K_INLINE double *KmVIJKtoQ(double *const pQo, const double *const pVi, const int pOrd) 04599 { 04600 return KmIJKtoQ(pQo, pVi[0], pVi[1], pVi[2], pOrd); 04601 } 04602 04603 /* 04604 * KmVtoQ : Default Euler Vector To Quaternion 04605 * 04606 * INPUT 04607 * pVi: input vector Vi 04608 * 04609 * OUTPUT 04610 * pQo: output quaternion Qo = Q(RXYZ(Vi)) 04611 * 04612 * RETURN VALUE 04613 * pQo 04614 * 04615 * REMARKS 04616 * - Rotations are post-multiplied 04617 */ 04618 K_INLINE double *KmVtoQ(double *const pQo, const double *const pVi) 04619 { 04620 return KmVXYZtoQ(pQo, pVi); 04621 } 04622 04623 /* 04624 * KmQtoR : Quaternion to Rotation Matrix 04625 * 04626 * INPUT 04627 * pQi: input quaternion Qi 04628 * 04629 * OUTPUT 04630 * pRo: output vector Vo = R(Qi) 04631 * 04632 * RETURN VALUE 04633 * pRo 04634 * 04635 * REMARKS 04636 */ 04637 K_INLINE double (*KmQtoR(double (*const pRo)[4], const double *const pQi))[4] 04638 { 04639 double S, XS, YS, ZS, WX, WY, WZ, XX, XY, XZ, YY, YZ, ZZ; 04640 04641 if ((S = pQi[0] * pQi[0] + pQi[1] * pQi[1] + pQi[2] * pQi[2] + pQi[3] * pQi[3])!=0.0) { 04642 S = 2.0 / S; 04643 } 04644 04645 XS = pQi[0] * S; YS = pQi[1] * S; ZS = pQi[2] * S; 04646 WX = pQi[3] * XS; WY = pQi[3] * YS; WZ = pQi[3] * ZS; 04647 XX = pQi[0] * XS; XY = pQi[0] * YS; XZ = pQi[0] * ZS; 04648 YY = pQi[1] * YS; YZ = pQi[1] * ZS; ZZ = pQi[2] * ZS; 04649 04650 pRo[0][0] = 1.0 - YY - ZZ; 04651 pRo[0][1] = XY + WZ; 04652 pRo[0][2] = XZ - WY; 04653 pRo[1][0] = XY - WZ; 04654 pRo[1][1] = 1.0 - XX - ZZ; 04655 pRo[1][2] = YZ + WX; 04656 pRo[2][0] = XZ + WY; 04657 pRo[2][1] = YZ - WX; 04658 pRo[2][2] = 1.0 - XX - YY; 04659 04660 return pRo; 04661 } 04662 04663 /* 04664 * KmQtoVXYZ : Quaternion To XYZ Euler Vector 04665 * 04666 * INPUT 04667 * pQi: input matrix Qi 04668 * 04669 * OUTPUT 04670 * pVo: output vector Vo = RXYZ(Qi) 04671 * 04672 * RETURN VALUE 04673 * pVo 04674 * 04675 * REMARKS 04676 * - Rotations are post-multiplied 04677 */ 04678 K_INLINE double *KmQtoVXYZ(double *const pVo, const double *const pQi) 04679 { 04680 double R[4][4]; 04681 04682 (void) KmQtoR(R, pQi); 04683 04684 return KmRtoVXYZ(pVo, R); 04685 } 04686 04687 /* 04688 * KmQtoVZYX : Quaternion To ZYX Euler Vector 04689 * 04690 * INPUT 04691 * pQi: input quaternion Qi 04692 * 04693 * OUTPUT 04694 * pVo: output vector Vo = RZYX(Qi) 04695 * 04696 * RETURN VALUE 04697 * pVo 04698 * 04699 * REMARKS 04700 * - Rotations are post-multiplied 04701 */ 04702 K_INLINE double *KmQtoVZYX(double *const pVo, const double *const pQi) 04703 { 04704 double R[4][4]; 04705 04706 (void) KmQtoR(R, pQi); 04707 04708 return KmRtoVZYX(pVo, R); 04709 } 04710 04711 /* 04712 * KmQtoVord : Quaternion To Ordered Euler Vector 04713 * 04714 * INPUT 04715 * pQi: input quaternion Qi 04716 * pOrd: order (Ord) of rotations 04717 * 04718 * OUTPUT 04719 * pVo: output vector Vo = R(Qi, Ord) 04720 * 04721 * RETURN VALUE 04722 * pVo 04723 * 04724 * REMARKS 04725 * - Rotations are post-multiplied 04726 */ 04727 K_INLINE double *KmQtoVord(double *const pVo, const double *const pQi, const int pOrd) 04728 { 04729 double R[4][4]; 04730 04731 (void) KmQtoR(R, pQi); 04732 04733 return KmRtoVord(pVo, R, pOrd); 04734 } 04735 04736 /* 04737 * KmQtoV : Default Order Rotation Matrix To Quaternion 04738 * 04739 * INPUT 04740 * pQi: input quaternion Qi 04741 * 04742 * OUTPUT 04743 * pVo: output vector Vo = RXYZ(Qi) 04744 * 04745 * RETURN VALUE 04746 * pVo 04747 * 04748 * REMARKS 04749 * - Rotations are post-multiplied 04750 */ 04751 K_INLINE double *KmQtoV(double *const pVo, const double *const pQi) 04752 { 04753 return KmQtoVXYZ(pVo, pQi); 04754 } 04755 04756 /* 04757 * KmRS : Scaling Vector Times Rotation Matrix 04758 * 04759 * INPUT 04760 * pAi: input rotation matrix Ri 04761 * pSi: input scaling vector Si 04762 * 04763 * OUTPUT 04764 * pAo: output matrix Ao = Ri * S(Si) 04765 * 04766 * RETURN VALUE 04767 * pAo 04768 */ 04769 K_INLINE double (*KmRS( 04770 double (*const pAo)[4], 04771 const double (*const pRi)[4], const double *const pSi))[4] 04772 { 04773 double T; 04774 04775 T = pSi[0]; 04776 04777 pAo[0][0] = pRi[0][0] * T; 04778 pAo[0][1] = pRi[0][1] * T; 04779 pAo[0][2] = pRi[0][2] * T; 04780 04781 T = pSi[1]; 04782 04783 pAo[1][0] = pRi[1][0] * T; 04784 pAo[1][1] = pRi[1][1] * T; 04785 pAo[1][2] = pRi[1][2] * T; 04786 04787 T = pSi[2]; 04788 04789 pAo[2][0] = pRi[2][0] * T; 04790 pAo[2][1] = pRi[2][1] * T; 04791 pAo[2][2] = pRi[2][2] * T; 04792 04793 return pAo; 04794 } 04795 04796 /* 04797 * KmVtoA : Vectors To Affine Matrix 04798 * 04799 * INPUT 04800 * pTi: input translation vector Ti 04801 * pRi: input XYZ Euler rotation vector Ri 04802 * pSi: input scaling vector Si 04803 * 04804 * OUTPUT 04805 * pAo: output matrix Ao = T(Ti) * RXYZ(Ri) * S(Si) 04806 * 04807 * RETURN VALUE 04808 * pAo 04809 */ 04810 K_INLINE double (*KmVtoA( 04811 double (*const pAo)[4], 04812 const double *const pTi, const double *const pRi, const double *const pSi))[4] 04813 { 04814 (void) KmVtoR(pAo, pRi); 04815 (void) KmRS(pAo, pAo, pSi); 04816 04817 pAo[3][0] = pTi[0]; 04818 pAo[3][1] = pTi[1]; 04819 pAo[3][2] = pTi[2]; 04820 04821 return pAo; 04822 } 04823 04824 /* 04825 * KmVtoAord : Vectors To Affine Matrix 04826 * 04827 * INPUT 04828 * pTi: input translation vector Ti 04829 * pRi: input XYZ Euler rotation vector Ri 04830 * pSi: input scaling vector Si 04831 * pOrd: order of rotation Ord 04832 * 04833 * OUTPUT 04834 * pAo: output matrix Ao = T(Ti) * RIJK(Ri, Ord) * S(Si) 04835 * 04836 * RETURN VALUE 04837 * pAo 04838 */ 04839 K_INLINE double (*KmVtoAord( 04840 double (*const pAo)[4], 04841 const double *const pTi, const double *const pRi, const double *const pSi, const int pOrd))[4] 04842 { 04843 double T; 04844 04845 (void) KmIJKtoR(pAo, pRi[0], pRi[1], pRi[2], pOrd); 04846 04847 T = pSi[0]; 04848 04849 pAo[0][0] *= T; 04850 pAo[0][1] *= T; 04851 pAo[0][2] *= T; 04852 04853 T = pSi[1]; 04854 04855 pAo[1][0] *= T; 04856 pAo[1][1] *= T; 04857 pAo[1][2] *= T; 04858 04859 T = pSi[2]; 04860 04861 pAo[2][0] *= T; 04862 pAo[2][1] *= T; 04863 pAo[2][2] *= T; 04864 04865 pAo[3][0] = pTi[0]; 04866 pAo[3][1] = pTi[1]; 04867 pAo[3][2] = pTi[2]; 04868 04869 return pAo; 04870 } 04871 04872 /* 04873 * KmNtoS : Isotropic Scaling Matrix 04874 * 04875 * INPUT 04876 * pNi: input value Ni 04877 * 04878 * OUTPUT 04879 * pSo: output matrix So = S([Ni Ni Ni]) 04880 * 04881 * RETURN VALUE 04882 * pSo 04883 */ 04884 K_INLINE double (*KmNtoS(double (*const pSo)[4], const double pNi))[4] 04885 { 04886 KM_ASSERT(pNi, "Zero scaling factor used in KmNtoS()"); 04887 04888 pSo[0][0] = pSo[1][1] = pSo[2][2] = pNi; 04889 04890 return pSo; 04891 } 04892 04893 /* 04894 * KmVtoS : Vector To Scaling Matrix 04895 * 04896 * INPUT 04897 * pVi: input vector Vi 04898 * 04899 * OUTPUT 04900 * pSo: output matrix So = S(Vi) 04901 * 04902 * RETURN VALUE 04903 * pSo 04904 */ 04905 K_INLINE double (*KmVtoS(double (*const pSo)[4], const double *const pVi))[4] 04906 { 04907 KM_ASSERT(pVi[0] * pVi[1] * pVi[2] != 0.0, "Zero scaling factor used in KmVtoS()"); 04908 04909 pSo[0][0] = pVi[0]; 04910 pSo[1][1] = pVi[1]; 04911 pSo[2][2] = pVi[2]; 04912 04913 return pSo; 04914 } 04915 04916 /* 04917 * KmStoV : Scaling Matrix To Vector 04918 * 04919 * INPUT 04920 * pSi: input matrix Si 04921 * 04922 * OUTPUT 04923 * pVo: output vector Vo = S(Si) 04924 * 04925 * RETURN VALUE 04926 * pVo 04927 */ 04928 K_INLINE double *KmStoV(double *const pVo, const double (*const pSi)[4]) 04929 { 04930 pVo[0] = pSi[0][0]; 04931 pVo[1] = pSi[1][1]; 04932 pVo[2] = pSi[2][2]; 04933 04934 return pVo; 04935 } 04936 04937 /* 04938 * KmAtoS : Affine Matrix to Scaling Vector 04939 * 04940 * INPUT 04941 * pAi: input matrix Ai 04942 * 04943 * OUTPUT 04944 * pSo: output matrix So = S(Ai) 04945 * 04946 * RETURN VALUE 04947 * pSo 04948 */ 04949 K_INLINE double *KmAtoS(double *const pSo, const double (*const pAi)[4]) 04950 { 04951 double S, T; 04952 04953 T = pAi[0][0]; 04954 S = T * T; 04955 T = pAi[0][1]; 04956 S += T * T; 04957 T = pAi[0][2]; 04958 pSo[0] = kSqrt(S + T * T); 04959 04960 T = pAi[1][0]; 04961 S = T * T; 04962 T = pAi[1][1]; 04963 S += T * T; 04964 T = pAi[1][2]; 04965 pSo[1] = kSqrt(S + T * T); 04966 04967 T = pAi[2][0]; 04968 S = T * T; 04969 T = pAi[2][1]; 04970 S += T * T; 04971 T = pAi[2][2]; 04972 pSo[2] = kSqrt(S + T * T); 04973 04974 return pSo; 04975 } 04976 04977 K_INLINE double KmAdist(const double (*const pAiA)[4], const double (*const pAiB)[4]) 04978 { 04979 return kSqrt( 04980 KmVdist2(pAiA[0], pAiB[0]) + 04981 KmVdist2(pAiA[1], pAiB[1]) + 04982 KmVdist2(pAiA[2], pAiB[2]) + 04983 KmVdist2(pAiA[3], pAiB[3])); 04984 } 04985 04986 K_INLINE double KmRdist(const double (*const pRiA)[4], const double (*const pRiB)[4]) 04987 { 04988 return kSqrt( 04989 KmVdist2(pRiA[0], pRiB[0]) + 04990 KmVdist2(pRiA[1], pRiB[1]) + 04991 KmVdist2(pRiA[2], pRiB[2])); 04992 } 04993 04994 K_INLINE double KmTdist(const double (*const pTiA)[4], const double (*const pTiB)[4]) 04995 { 04996 return KmVdist(pTiA[3], pTiB[3]); 04997 } 04998 04999 #include <fbxfilesdk/fbxfilesdk_nsend.h> 05000 05001 #endif // FBXFILESDK_COMPONENTS_KBASELIB_KMATH_KMATH_H 05002