 |
TAPs 0.7.7.3
|
|
TAPs 0.7.7.3
|
00001 /****************************************************************************** 00002 TAPsCGMath.hpp 00003 00004 SUKITTI PUNAK (07/27/2005) 00005 UPDATE (10/26/2010) 00006 ******************************************************************************/ 00007 00034 #ifndef TAPs_CG_MATH_HPP 00035 #define TAPs_CG_MATH_HPP 00036 00037 #include "TAPsGenMath.hpp" 00038 00039 #include "TAPsVector2.hpp" 00040 #include "TAPsVector3.hpp" 00041 #include "TAPsVector4.hpp" 00042 #include "TAPsMatrix2x2.hpp" 00043 #include "TAPsMatrix3x3.hpp" 00044 #include "TAPsMatrix4x4.hpp" 00045 #include "TAPsColMatrix2x2.hpp" 00046 #include "TAPsColMatrix3x3.hpp" 00047 #include "TAPsColMatrix4x4.hpp" 00048 #include "TAPsQuaternion.hpp" 00049 00050 //#include <GL/glut.h> // DEBUG 00051 00052 BEGIN_NAMESPACE_TAPs 00053 //============================================================================= 00054 template <typename T> 00055 class CGMath { 00056 public: 00057 //============================================================================= 00058 //========================================================================= 00059 // Constant Values 00060 //========================================================================= 00061 static const Vector3<T> ZeroVector; 00062 static const Vector3<T> VectorX; 00063 static const Vector3<T> VectorY; 00064 static const Vector3<T> VectorZ; 00065 static const Matrix3x3<T> IdentityMatrix3x3; 00066 static const Matrix4x4<T> IdentityMatrix4x4; 00067 // 00068 static Vector3<T> TempVector3; 00069 static Vector4<T> TempVector4; 00070 static Matrix3x3<T> TempMatrix3x3; 00071 static Matrix4x4<T> TempMatrix4x4; 00072 //----------------------------------------------------------------------------- 00080 //------------------------------------------------------------------------- 00081 #ifndef _WINDOWS_ 00082 #endif 00083 //------------------------------------------------------------------------- 00084 00085 //========================================================================= 00086 // Conversion and Creation Functions 00087 //------------------------------------------------------------------------- 00089 inline static Matrix4x4<T> MatrixTranslation ( Vector3<T> const & translation ); 00090 00092 inline static Matrix4x4<T> MatrixRotation ( Quaternion<T> const & orientation ); 00093 00095 inline static Matrix4x4<T> MatrixScale ( Vector3<T> const & scale ); 00096 //------------------------------------------------------------------------- 00097 // Conversion and Creation Functions 00098 //========================================================================= 00099 00100 00101 00102 //========================================================================= 00103 // Normal Calculation Fns 00104 //========================================================================= 00105 00113 inline static void NormalOfTriangle ( 00114 Vector3<T> const & v1, 00115 Vector3<T> const & v2, 00116 Vector3<T> const & v3, 00117 Vector3<T> & vNormal 00118 ) 00119 { vNormal = (v3-v2)^(v1-v2); } 00120 00130 inline static Vector3<T> NormalOfTriangle ( 00131 Vector3<T> const & v1, // I/P: vertex# 1 00132 Vector3<T> const & v2, // I/P: vertex# 2 00133 Vector3<T> const & v3 // I/P: vertex# 3 00134 ) 00135 { return (v3-v2)^(v1-v2); } 00136 00137 //========================================================================= 00138 00139 00140 00141 00142 //========================================================================= 00143 // Transformation Fns 00144 //========================================================================= 00145 00169 static Matrix3x3<T> CreateRotationMatrix3x3 ( 00170 T x, T y, T z, // I/P: Rotation Axis 00171 T rotationAngle // I/P: Rotation Angle (in degs) 00172 ) { return CreateRotationMatrix3x3( Vector3<T>(x,y,z), rotationAngle ); } 00173 static Matrix3x3<T> CreateRotationMatrix3x3 ( 00174 Vector3<T> const & rotationAxis, // I/P: Rotation Axis 00175 T rotationAngle // I/P: Rotation Angle (in degs) 00176 ); 00177 00202 static Matrix4x4<T> CreateRotationMatrix4x4 ( 00203 T x, T y, T z, // I/P: Rotation Axis 00204 T rotationAngle // I/P: Rotation Angle (in degs) 00205 ) { return CreateRotationMatrix4x4( Vector3<T>(x,y,z), rotationAngle ); } 00206 static Matrix4x4<T> CreateRotationMatrix4x4 ( 00207 Vector3<T> const & rotationAxis, // I/P: Rotation Axis 00208 T rotationAngle // I/P: Rotation Angle (in degs) 00209 ); 00210 00220 static inline Matrix3x3<T> CreateRotationMatrix3x3FromVectorAtoVectorB ( 00221 Vector3<T> const & startingPt, // I/P: starting point 00222 Vector3<T> const & endingPt // I/P: ending point 00223 ); 00233 static inline Matrix4x4<T> CreateRotationMatrix4x4FromVectorAtoVectorB ( 00234 Vector3<T> const & startingPt, // I/P: starting point 00235 Vector3<T> const & endingPt // I/P: ending point 00236 ); 00237 00238 //========================================================================= 00239 00240 00241 00242 00243 //========================================================================= 00244 // Matrix Fns 00245 //========================================================================= 00246 00259 static inline void EigValsVecsOf3x3SymMatrix ( 00260 Matrix3x3<T> & iM, // I/P: a 3x3 symmetric matrix 00261 T & l1, // O/P: first eigen value 00262 T & l2, // O/P: second eigen value 00263 T & l3, // O/P: third eigen value 00264 Vector3<T> & v1, // O/P: first eigen vector 00265 Vector3<T> & v2, // O/P: second eigen vector 00266 Vector3<T> & v3 // O/P: third eigen vector 00267 ); 00268 00269 //========================================================================= 00270 00271 00272 00273 00274 //========================================================================= 00275 // Distance/Intersection/Penetration Finding 00276 //========================================================================= 00277 00289 static inline void FindRatioOfPointOnLineThatIntersectPlane ( 00290 Vector3<T> const & l1, Vector3<T> const & l2, // I/P: a line 00291 Vector3<T> const & a, // I/P: a point on the plane 00292 Vector3<T> const & n, // I/P: the plane (unit) normal 00293 T & ratio, // O/P: the ratio 00294 Vector3<T> & projPt, // O/P: the projected point 00295 T tolerance = Math<T>::ThresholdZero 00296 ); 00297 00322 static void FindClosestDistanceBetweenTwoLineSegments ( 00323 Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line 00324 Vector3<T> const & q1, Vector3<T> const & q2, // I/P: a line 00325 T & distance, 00326 Vector3<T> & Ncontact, 00327 T tolerance = Math<T>::ThresholdZero 00328 ); 00329 00354 static void FindClosestDistanceBetweenTwoLineSegments ( 00355 Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line 00356 Vector3<T> const & q1, Vector3<T> const & q2, // I/P: a line 00357 T & distance, 00358 Vector3<T> & Ncontact, 00359 Vector3<T> & projPtOnLine_1, 00360 Vector3<T> & projPtOnLine_2, 00361 T tolerance = Math<T>::ThresholdZero 00362 ); 00363 00372 static inline void FindProjectedPointOnAPlane ( 00373 Vector3<T> const & p, 00374 Vector3<T> const & a, 00375 Vector3<T> const & n, 00376 Vector3<T> & projPt, 00377 T & distance 00378 ); 00379 00387 static inline void FindProjectedPointOnAPlane ( 00388 Vector3<T> const & p, 00389 Vector3<T> const & a, 00390 Vector3<T> const & n, 00391 Vector3<T> & projPt 00392 ); 00393 00401 static inline void FindProjectedPointOnALine ( 00402 Vector3<T> const & p, 00403 Vector3<T> const & p1, 00404 Vector3<T> const & p2, 00405 T & ratio, 00406 Vector3<T> & projPt, 00407 T & distance 00408 ); 00409 00417 static inline void FindProjectedPointOnALine ( 00418 Vector3<T> const & p, 00419 Vector3<T> const & p1, 00420 Vector3<T> const & p2, 00421 T & ratio, 00422 Vector3<T> & projPt 00423 ); 00424 00432 static inline void FindProjectedPointOnALine ( 00433 Vector3<T> const & p, 00434 Vector3<T> const & p1, 00435 Vector3<T> const & p2, 00436 T & ratio 00437 ); 00438 00452 static bool FindIntersectionLineSegmentLineSegment ( 00453 Vector3<T> const & p1, // first line segment 00454 Vector3<T> const & q1, 00455 Vector3<T> const & p2, // second line segment 00456 Vector3<T> const & q2, 00457 T & t1, // parameter for first line segment 00458 // where intersection_pt = p1 + t1(q1-p1) 00459 T & t2, // parameter for second line segment 00460 // where intersection_pt = p2 + t2(q2-p2) 00461 bool & touching, // Touching instead of Intersecting 00462 T tolerance = Math<T>::ThresholdZero 00463 ); 00464 00465 00466 //------------------------------------------------------------------------- 00485 static bool FindIntersectionCircleTriangle ( 00486 // I/P: 00487 Vector3<T> const & center, 00488 T radius, 00489 Vector3<T> const & normal, 00490 Vector3<T> const & p, 00491 Vector3<T> const & q, 00492 Vector3<T> const & r, 00493 // O/P: 00494 Vector3<T> & outVec_1, 00495 Vector3<T> & outVec_2, 00496 Vector3<T> & outVec_3, 00497 // O/P: (for translating to force feedback) 00498 Vector3<T> * contactPt = NULL, 00499 // Threshold for zero value 00500 T tolerance = Math<T>::ThresholdZero 00501 ); 00502 //------------------------------------------------------------------------- 00503 00504 00505 //------------------------------------------------------------------------- 00518 static bool FindIntersectionSphereTriangle ( 00519 // I/P: 00520 Vector3<T> const & center, 00521 T radius, 00522 Vector3<T> const & p, 00523 Vector3<T> const & q, 00524 Vector3<T> const & r, 00525 T tolerance = Math<T>::ThresholdZero 00526 ); 00527 00546 static bool FindIntersectionSphereTriangle ( 00547 // I/P: 00548 Vector3<T> const & center, 00549 T radius, 00550 Vector3<T> const & p, 00551 Vector3<T> const & q, 00552 Vector3<T> const & r, 00553 // O/P: 00554 Vector3<T> & outVec_1, 00555 Vector3<T> & outVec_2, 00556 Vector3<T> & outVec_3, 00557 // O/P: (for translating to force feedback) 00558 Vector3<T> * contactPt = NULL, 00559 // Threshold for zero value 00560 T tolerance = Math<T>::ThresholdZero 00561 ); 00562 //------------------------------------------------------------------------- 00563 00564 00565 //------------------------------------------------------------------------- 00580 static bool FindIntersectionCylinderCylinder ( 00581 // I/P: 00582 Vector3<T> const & cA1, 00583 Vector3<T> const & cA2, 00584 T cAradius, 00585 Vector3<T> const & cB1, 00586 Vector3<T> const & cB2, 00587 T cBradius, 00588 // O/P: 00589 Vector3<T> & distVector, 00590 // Threshold for zero value 00591 T tolerance = Math<T>::ThresholdZero 00592 ); 00593 00594 //------------------------------------------------------------------------- 00607 static bool FindIntersectionCylinderTriangle ( 00608 // I/P: 00609 Vector3<T> const & c1, 00610 Vector3<T> const & c2, 00611 T radius, 00612 Vector3<T> const & p, 00613 Vector3<T> const & q, 00614 Vector3<T> const & r, 00615 // Threshold for zero value 00616 T tolerance = Math<T>::ThresholdZero 00617 ); 00618 00637 static bool FindIntersectionCylinderTriangle ( 00638 // I/P: 00639 Vector3<T> const & c1, 00640 Vector3<T> const & c2, 00641 T radius, 00642 Vector3<T> const & p, 00643 Vector3<T> const & q, 00644 Vector3<T> const & r, 00645 // O/P: 00646 Vector3<T> & outVec_1, 00647 Vector3<T> & outVec_2, 00648 Vector3<T> & outVec_3, 00649 // O/P: (for translating to force feedback) 00650 Vector3<T> * contactPt = NULL, 00651 // Threshold for zero value 00652 T tolerance = Math<T>::ThresholdZero 00653 ); 00654 //------------------------------------------------------------------------- 00655 00665 static bool FindIntersectionTriangleTriangle ( 00666 Vector3<T> const & p1, // first triangle 00667 Vector3<T> const & q1, 00668 Vector3<T> const & r1, 00669 Vector3<T> const & p2, // second triangle 00670 Vector3<T> const & q2, 00671 Vector3<T> const & r2, 00672 // int & noOfIntersectPts, 00673 // Vector3<T> & IntersectPt1, 00674 // Vector3<T> & IntersectPt2, 00675 // Vector3<T> & IntersectPt3, 00676 T tolerance = Math<T>::ThresholdZero 00677 ); 00678 00695 static int FindIfACircleIntersectsATriangleOnTheSamePlane ( 00696 Vector3<T> const & circleCenter, 00697 T const & circleRadius, 00698 Vector3<T> const & p, 00699 Vector3<T> const & q, 00700 Vector3<T> const & r, 00701 T tolerance = Math<T>::ThresholdZero 00702 ); 00703 00719 static bool FindIfAPointIsInATriangleOnTheSamePlane ( 00720 Vector3<T> const & pt, 00721 Vector3<T> const & p, 00722 Vector3<T> const & q, 00723 Vector3<T> const & r, 00724 T tolerance = Math<T>::ThresholdZero 00725 ); 00726 00735 static Vector3<T> CentroidOfTriangle ( 00736 Vector3<T> const & p, 00737 Vector3<T> const & q, 00738 Vector3<T> const & r 00739 ); 00740 00755 static bool FindIfAPointIsInATriangle ( 00756 Vector3<T> const & pt, 00757 Vector3<T> const & p, 00758 Vector3<T> const & q, 00759 Vector3<T> const & r, 00760 T tolerance = Math<T>::ThresholdZero 00761 ); 00762 00777 static bool FindIntersectionLineSegmentTriangle ( 00778 Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line segment 00779 Vector3<T> const & t1, // I/P: the triangle 00780 Vector3<T> const & t2, 00781 Vector3<T> const & t3, 00782 T & ratio, // O/P: the ratio 00783 Vector3<T> & projPt, // O/P: the projected point 00784 T tolerance = Math<T>::ThresholdZero 00785 ); 00786 00803 static bool FindIntersectionLineSegmentTriangle ( 00804 Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line segment 00805 Vector3<T> const & t1, // I/P: the triangle 00806 Vector3<T> const & t2, 00807 Vector3<T> const & t3, 00808 T & ratio, // O/P: the ratio 00809 T & intersectionAngle, // O/P: the intersection angle (in degrees) of the line with the plane's normal 00810 Vector3<T> & projPt, // O/P: the projected point 00811 T tolerance = Math<T>::ThresholdZero 00812 ); 00813 00814 //========================================================================= 00815 00816 00817 00818 00819 //========================================================================= 00820 // Spline Fns 00821 //========================================================================= 00822 00826 static Vector3<T> CatmullRomSplinePt ( 00827 // I/P: 4 points p0, p1, p2, p3 and t in ranges [0,1] 00828 // O/P: Catmull-Rom Spline Curve Pt 00829 Vector3<T> const & p0, Vector3<T> const & p1, 00830 Vector3<T> const & p2, Vector3<T> const & p3, 00831 T t 00832 ); 00833 00834 00835 /* 00836 //------------------------------------------------------------------------- 00837 // Bezier Curve Pt Fn 00838 static Vector3<T> BezierSplinePt ( 00839 // I/P: ctrl pts P[] of size degree+1, and t in ranges [0,1] 00840 // O/P: Bezier Spline Curve Pt 00841 Vector3<T> const & P[], int degree, T t 00842 ); 00843 //*/ 00844 00845 //========================================================================= 00846 00847 00848 00849 00850 //========================================================================= 00851 // Subdivision Fns 00852 //========================================================================= 00853 00900 static void SubdivideUniPolyInBBForm ( 00901 Vector3<T> const P[], // I/P: points on curve 00902 int degree, // I/P: degree 00903 T t, // I/P: parameter value 00904 Vector3<T> L[], // O/P: subdivision points 00905 Vector3<T> R[] // O/P: subdivision points 00906 ); 00907 00908 //========================================================================= 00909 00910 00911 00912 00913 //========================================================================= 00914 // Constraint Fns 00915 //========================================================================= 00916 00928 static T SolveAForConstCubicBezierCurveLength ( 00929 const Vector3<T> p0, // I/P: ctrl pt #1 00930 const Vector3<T> p3, // I/P: ctrl pt #4 00931 const Vector3<T> t0, // I/P: tangent at pt #1 00932 const Vector3<T> t3, // I/P: tangent at pt #4 00933 int iNumSubdivision, // I/P: #subdivision constraint 00934 T tLength, // I/P: length constraint 00935 T tolerance = 0.0000001, // I/P: tolerance 00936 Vector3<T> * pPtOnCurve = NULL // O/P: ptr to points on curve 00937 ); 00938 //========================================================================= 00939 00940 00941 00942 00943 //========================================================================= 00944 // Transformation Fns 00945 //========================================================================= 00946 00955 static Vector3<T> TransformALinkToWorldXAxis ( 00956 const Vector3<T> V, // I/P: V 00957 T & Cy, // O/P: cosine(theta) of y-rotation 00958 T & Sy, // O/P: sine(theta) of y-rotation 00959 T & Cz, // O/P: cosine(phi) of z-rotation 00960 T & Sz // O/P: sine(phi) of z-rotation 00961 // return Rz * Ry * V; 00962 // where Rz = [ Cz -Sz 0 ] 00963 // [ Sz Cz 0 ] 00964 // [ 0 0 1 ] 00965 // and Ry = [ Cy 0 Sy ] 00966 // [ 0 1 0 ] 00967 // [ -Sy 0 Cy ] 00968 ); 00969 00970 //========================================================================= 00971 00972 00973 00974 00975 //========================================================================= 00976 // Helper Function for Collision Detection and Response 00977 //------------------------------------------------------------------------- 00990 static inline bool CD_SphereAtOrigin_vs_Point ( 00991 T sphere_radius, 00992 Vector3<T> const & point, 00993 Vector3<T> & shortestDistanceOut 00994 ); 00995 01009 static inline bool CD_CylinderAtOrigin_vs_Point ( 01010 T cylinder_radius, 01011 T cylinder_height, 01012 Vector3<T> const & point, 01013 Vector3<T> & shortestDistanceOut 01014 ); 01015 01027 static inline bool CD_CylinderAtOrigin_vs_Sphere ( 01028 T cylinder_radius, 01029 T cylinder_height, 01030 Vector3<T> const & sphereCenter, 01031 T sphereRadius 01032 ); 01033 01047 static inline bool CD_CylinderAtOrigin_vs_Sphere ( 01048 T cylinder_radius, 01049 T cylinder_height, 01050 Vector3<T> const & sphereCenter, 01051 T sphereRadius, 01052 Vector3<T> & shortestDistanceOut 01053 ); 01054 //------------------------------------------------------------------------- 01055 // Helper Function for Collision Detection and Response 01056 //========================================================================= 01057 01058 01059 //========================================================================= 01060 // Sphere-Sphere Intersection Test 01061 //------------------------------------------------------------------------- 01073 static inline T IntersectionDistance_Sphere_vs_Sphere ( 01074 Vector3<T> const & centerA, 01075 T radiusA, 01076 Vector3<T> const & centerB, 01077 T radiusB, 01078 Vector3<T> & shortestDistanceOut 01079 ); 01080 static inline T IntersectionDistance_Sphere_vs_Sphere ( 01081 Vector3<T> const & centerA, 01082 T radiusA, 01083 Vector3<T> const & centerB, 01084 T radiusB, 01085 Matrix4x4<T> const & transformB, 01086 Vector3<T> & shortestDistanceOut 01087 ); 01088 static inline T IntersectionDistance_Sphere_vs_Sphere ( 01089 Vector3<T> const & centerA, 01090 T radiusA, 01091 Matrix4x4<T> const & transformA, 01092 Vector3<T> const & centerB, 01093 T radiusB, 01094 Vector3<T> & shortestDistanceOut 01095 ); 01096 static inline T IntersectionDistance_Sphere_vs_Sphere ( 01097 Vector3<T> const & centerA, 01098 T radiusA, 01099 Matrix4x4<T> const & transformA, 01100 Vector3<T> const & centerB, 01101 T radiusB, 01102 Matrix4x4<T> const & transformB, 01103 Vector3<T> & shortestDistanceOut 01104 ); 01105 //------------------------------------------------------------------------- 01106 // Sphere-Sphere Intersection Test 01107 //========================================================================= 01108 01109 //========================================================================= 01110 // Interpolation Functions 01111 //------------------------------------------------------------------------- 01119 static inline T LinearInterpolation ( 01120 T val1, 01121 T val2, 01122 T percent 01123 ); 01124 static inline Vector2<T> LinearInterpolation ( 01125 Vector2<T> val1, 01126 Vector2<T> val2, 01127 T percent 01128 ); 01129 static inline Vector3<T> LinearInterpolation ( 01130 Vector3<T> val1, 01131 Vector3<T> val2, 01132 T percent 01133 ); 01134 static inline Vector4<T> LinearInterpolation ( 01135 Vector4<T> val1, 01136 Vector4<T> val2, 01137 T percent 01138 ); 01139 //------------------------------------------------------------------------- 01140 // Interpolation Functions 01141 //========================================================================= 01142 01143 01144 }; // END CLASS CGMath 01145 //============================================================================= 01146 END_NAMESPACE_TAPs 01147 //----------------------------------------------------------------------------- 01148 // Include definition if TAPs_USE_EXPORT is not defined 01149 //#if !defined( TAPs_USE_EXPORT ) 01150 #include "TAPsCGMath.cpp" 01151 //#endif 01152 //----------------------------------------------------------------------------- 01153 #endif 01154 //34567890123456789012345678901234567890123456789012345678901234567890123456789 01155 //--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----
1.7.4 
