Open Dynamics Engine
|
00001 /************************************************************************* 00002 * * 00003 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * 00004 * All rights reserved. Email: russ@q12.org Web: www.q12.org * 00005 * * 00006 * This library is free software; you can redistribute it and/or * 00007 * modify it under the terms of EITHER: * 00008 * (1) The GNU Lesser General Public License as published by the Free * 00009 * Software Foundation; either version 2.1 of the License, or (at * 00010 * your option) any later version. The text of the GNU Lesser * 00011 * General Public License is included with this library in the * 00012 * file LICENSE.TXT. * 00013 * (2) The BSD-style license that is included with this library in * 00014 * the file LICENSE-BSD.TXT. * 00015 * * 00016 * This library is distributed in the hope that it will be useful, * 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * 00019 * LICENSE.TXT and LICENSE-BSD.TXT for more details. * 00020 * * 00021 *************************************************************************/ 00022 00023 /* 00024 00025 some useful collision utility stuff. 00026 00027 */ 00028 00029 #ifndef _ODE_COLLISION_UTIL_H_ 00030 #define _ODE_COLLISION_UTIL_H_ 00031 00032 #include <ode/common.h> 00033 #include <ode/contact.h> 00034 #include <ode/odemath.h> 00035 #include <ode/rotation.h> 00036 00037 00038 // given a pointer `p' to a dContactGeom, return the dContactGeom at 00039 // p + skip bytes. 00040 #define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) 00041 00042 #if 1 00043 #include "collision_kernel.h" 00044 // Fetches a contact 00045 static inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ 00046 dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK)); 00047 return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); 00048 } 00049 #endif 00050 00051 00052 // if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and 00053 // return 1, else return 0. 00054 00055 int dCollideSpheres (dVector3 p1, dReal r1, 00056 dVector3 p2, dReal r2, dContactGeom *c); 00057 00058 00059 // given two lines 00060 // qa = pa + alpha* ua 00061 // qb = pb + beta * ub 00062 // where pa,pb are two points, ua,ub are two unit length vectors, and alpha, 00063 // beta go from [-inf,inf], return alpha and beta such that qa and qb are 00064 // as close as possible 00065 00066 void dLineClosestApproach (const dVector3 pa, const dVector3 ua, 00067 const dVector3 pb, const dVector3 ub, 00068 dReal *alpha, dReal *beta); 00069 00070 00071 // given a line segment p1-p2 and a box (center 'c', rotation 'R', side length 00072 // vector 'side'), compute the points of closest approach between the box 00073 // and the line. return these points in 'lret' (the point on the line) and 00074 // 'bret' (the point on the box). if the line actually penetrates the box 00075 // then the solution is not unique, but only one solution will be returned. 00076 // in this case the solution points will coincide. 00077 00078 void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, 00079 const dVector3 c, const dMatrix3 R, 00080 const dVector3 side, 00081 dVector3 lret, dVector3 bret); 00082 00083 // 20 Apr 2004 00084 // Start code by Nguyen Binh 00085 int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane); 00086 // clip polygon with plane and generate new polygon points 00087 void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ); 00088 00089 void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius); 00090 00091 // Some vector math 00092 static inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c) 00093 { 00094 dSubtractVectors3(c, a, b); 00095 } 00096 00097 static inline void dVector3Scale(dVector3& a,dReal nScale) 00098 { 00099 dScaleVector3(a, nScale); 00100 } 00101 00102 static inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c) 00103 { 00104 dAddVectors3(c, a, b); 00105 } 00106 00107 static inline void dVector3Copy(const dVector3& a,dVector3& c) 00108 { 00109 dCopyVector3(c, a); 00110 } 00111 00112 static inline void dVector4Copy(const dVector4& a,dVector4& c) 00113 { 00114 dCopyVector4(c, a); 00115 } 00116 00117 static inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c) 00118 { 00119 dCalcVectorCross3(c, a, b); 00120 } 00121 00122 static inline dReal dVector3Length(const dVector3& a) 00123 { 00124 return dCalcVectorLength3(a); 00125 } 00126 00127 static inline dReal dVector3LengthSquare(const dVector3& a) 00128 { 00129 return dCalcVectorLengthSquare3(a); 00130 } 00131 00132 static inline dReal dVector3Dot(const dVector3& a,const dVector3& b) 00133 { 00134 return dCalcVectorDot3(a, b); 00135 } 00136 00137 static inline void dVector3Inv(dVector3& a) 00138 { 00139 dNegateVector3(a); 00140 } 00141 00142 static inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v) 00143 { 00144 dGetMatrixColumn3(v, m, col); 00145 } 00146 00147 static inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) 00148 { 00149 dCalcVectorCross3_114(r, v, m + col); 00150 } 00151 00152 static inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) 00153 { 00154 dCalcVectorCross3_141(r, m + col, v); 00155 } 00156 00157 static inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r) 00158 { 00159 dMultiply0_331(r, m, v); 00160 } 00161 00162 static inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane) 00163 { 00164 return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]); 00165 } 00166 00167 static inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane) 00168 { 00169 plane[0] = normal[0]; 00170 plane[1] = normal[1]; 00171 plane[2] = normal[2]; 00172 plane[3] = distance; 00173 } 00174 00175 static inline void dMatrix3Copy(const dReal* source,dMatrix3& dest) 00176 { 00177 dCopyMatrix4x3(dest, source); 00178 } 00179 00180 static inline dReal dMatrix3Det( const dMatrix3& mat ) 00181 { 00182 dReal det; 00183 00184 det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) 00185 - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) 00186 + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); 00187 00188 return( det ); 00189 } 00190 00191 00192 inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst ) 00193 { 00194 dReal det = dMatrix3Det( ma ); 00195 00196 if ( dFabs( det ) < REAL(0.0005) ) 00197 { 00198 dRSetIdentity( dst ); 00199 return; 00200 } 00201 00202 dst[0] = ma[5]*ma[10] - ma[6]*ma[9] / det; 00203 dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det; 00204 dst[2] = ma[1]*ma[6] - ma[5]*ma[2] / det; 00205 00206 dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det; 00207 dst[5] = ma[0]*ma[10] - ma[8]*ma[2] / det; 00208 dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det; 00209 00210 dst[8] = ma[4]*ma[9] - ma[8]*ma[5] / det; 00211 dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det; 00212 dst[10] = ma[0]*ma[5] - ma[1]*ma[4] / det; 00213 } 00214 00215 inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) 00216 { 00217 00218 // Nguyen Binh : this code seem to be the fastest. 00219 dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3]; 00220 dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1]; 00221 dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2]; 00222 dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3]; 00223 00224 dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1; 00225 dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2; 00226 dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0; 00227 00228 /* 00229 // nVidia SDK implementation 00230 dVector3 uv, uuv; 00231 dVector3 qvec; 00232 qvec[0] = quat[1]; 00233 qvec[1] = quat[2]; 00234 qvec[2] = quat[3]; 00235 00236 dVector3Cross(qvec,source,uv); 00237 dVector3Cross(qvec,uv,uuv); 00238 00239 dVector3Scale(uv,REAL(2.0)*quat[0]); 00240 dVector3Scale(uuv,REAL(2.0)); 00241 00242 dest[0] = source[0] + uv[0] + uuv[0]; 00243 dest[1] = source[1] + uv[1] + uuv[1]; 00244 dest[2] = source[2] + uv[2] + uuv[2]; 00245 */ 00246 } 00247 00248 inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) 00249 { 00250 00251 dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]; 00252 00253 if (norm > REAL(0.0)) 00254 { 00255 dQuaternion invQuat; 00256 invQuat[0] = quat[0] / norm; 00257 invQuat[1] = -quat[1] / norm; 00258 invQuat[2] = -quat[2] / norm; 00259 invQuat[3] = -quat[3] / norm; 00260 00261 dQuatTransform(invQuat,source,dest); 00262 00263 } 00264 else 00265 { 00266 // Singular -> return identity 00267 dVector3Copy(source,dest); 00268 } 00269 } 00270 00271 inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ) 00272 { 00273 rY = asin(mRot[0 * 4 + 2]); 00274 if (rY < M_PI /2) 00275 { 00276 if (rY > -M_PI /2) 00277 { 00278 rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]); 00279 rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]); 00280 } 00281 else 00282 { 00283 // not unique 00284 rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]); 00285 rZ = REAL(0.0); 00286 } 00287 } 00288 else 00289 { 00290 // not unique 00291 rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]); 00292 rZ = REAL(0.0); 00293 } 00294 } 00295 00296 inline void dQuatInv(const dQuaternion& source, dQuaternion& dest) 00297 { 00298 dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3]; 00299 00300 if (norm > 0.0f) 00301 { 00302 dest[0] = source[0] / norm; 00303 dest[1] = -source[1] / norm; 00304 dest[2] = -source[2] / norm; 00305 dest[3] = -source[3] / norm; 00306 } 00307 else 00308 { 00309 // Singular -> return identity 00310 dest[0] = REAL(1.0); 00311 dest[1] = REAL(0.0); 00312 dest[2] = REAL(0.0); 00313 dest[3] = REAL(0.0); 00314 } 00315 } 00316 00317 00318 #endif