Open Dynamics Engine

collision_util.h

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