ode/src/collision_util.h

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  *************************************************************************/
  22
  23 /*
  24
  25 some useful collision utility stuff.
  26
  27 */
  28
  29 #ifndef _ODE_COLLISION_UTIL_H_
  30 #define _ODE_COLLISION_UTIL_H_
  31
  32 #include <ode/common.h>
  33 #include <ode/contact.h>
  34 #include <ode/rotation.h>
  35 #include "odemath.h"
  36
  37
  38 // given a pointer `p' to a dContactGeom, return the dContactGeom at
  39 // p + skip bytes.
  40 #define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
  41
  42 #if 1
  43 #include "collision_kernel.h"
  44 // Fetches a contact
  45 static inline
  46 dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
  47     dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
  48     return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
  49 }
  50 #endif
  51
  52
  53 // if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
  54 // return 1, else return 0.
  55
  56 int dCollideSpheres (dVector3 p1, dReal r1,
  57                      dVector3 p2, dReal r2, dContactGeom *c);
  58
  59
  60 // given two lines
  61 //    qa = pa + alpha* ua
  62 //    qb = pb + beta * ub
  63 // where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
  64 // beta go from [-inf,inf], return alpha and beta such that qa and qb are
  65 // as close as possible
  66
  67 void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
  68                            const dVector3 pb, const dVector3 ub,
  69                            dReal *alpha, dReal *beta);
  70
  71
  72 // given a line segment p1-p2 and a box (center 'c', rotation 'R', side length
  73 // vector 'side'), compute the points of closest approach between the box
  74 // and the line. return these points in 'lret' (the point on the line) and
  75 // 'bret' (the point on the box). if the line actually penetrates the box
  76 // then the solution is not unique, but only one solution will be returned.
  77 // in this case the solution points will coincide.
  78
  79 void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
  80                             const dVector3 c, const dMatrix3 R,
  81                             const dVector3 side,
  82                             dVector3 lret, dVector3 bret);
  83
  84 // 20 Apr 2004
  85 // Start code by Nguyen Binh
  86 int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);
  87 // clip polygon with plane and generate new polygon points
  88 void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane );
  89
  90 void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);
  91
  92 // Some vector math
  93 static inline
  94 void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
  95 {
  96     dSubtractVectors3(c, a, b);
  97 }
  98
  99 static inline
 100 void dVector3Scale(dVector3& a,dReal nScale)
 101 {
 102     dScaleVector3(a, nScale);
 103 }
 104
 105 static inline
 106 void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
 107 {
 108     dAddVectors3(c, a, b);
 109 }
 110
 111 static inline
 112 void dVector3Copy(const dVector3& a,dVector3& c)
 113 {
 114     dCopyVector3(c, a);
 115 }
 116
 117 static inline
 118 void dVector4Copy(const dVector4& a,dVector4& c)
 119 {
 120     dCopyVector4(c, a);
 121 }
 122
 123 static inline
 124 void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
 125 {
 126     dCalcVectorCross3(c, a, b);
 127 }
 128
 129 static inline
 130 dReal dVector3Length(const dVector3& a)
 131 {
 132     return dCalcVectorLength3(a);
 133 }
 134
 135 static inline
 136 dReal dVector3LengthSquare(const dVector3& a)
 137 {
 138     return dCalcVectorLengthSquare3(a);
 139 }
 140
 141 static inline
 142 dReal dVector3Dot(const dVector3& a,const dVector3& b)
 143 {
 144     return dCalcVectorDot3(a, b);
 145 }
 146
 147 static inline
 148 void dVector3Inv(dVector3& a)
 149 {
 150     dNegateVector3(a);
 151 }
 152
 153 static inline
 154 void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
 155 {
 156     dGetMatrixColumn3(v, m, col);
 157 }
 158
 159 static inline
 160 void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
 161 {
 162     dCalcVectorCross3_114(r, v, m + col);
 163 }
 164
 165 static inline
 166 void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
 167 {
 168     dCalcVectorCross3_141(r, m + col, v);
 169 }
 170
 171 static inline
 172 void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
 173 {
 174     dMultiply0_331(r, m, v);
 175 }
 176
 177 static inline
 178 dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
 179 {
 180     return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
 181 }
 182
 183 static inline
 184 void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
 185 {
 186     plane[0] = normal[0];
 187     plane[1] = normal[1];
 188     plane[2] = normal[2];
 189     plane[3] = distance;
 190 }
 191
 192 static inline
 193 void dMatrix3Copy(const dReal* source,dMatrix3& dest)
 194 {
 195     dCopyMatrix4x3(dest, source);
 196 }
 197
 198 static inline
 199 dReal dMatrix3Det( const dMatrix3& mat )
 200 {
 201     dReal det;
 202
 203     det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
 204         - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
 205         + mat[2] * ( mat[4]*mat[9]  - mat[8]*mat[5] );
 206
 207     return( det );
 208 }
 209
 210
 211 static inline
 212 void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
 213 {
 214     dReal det = dMatrix3Det( ma );
 215
 216     if ( dFabs( det ) < REAL(0.0005) )
 217     {
 218         dRSetIdentity( dst );
 219         return;
 220     }
 221
 222     double detRecip = REAL(1.0) / det;
 223
 224     dst[0] =  (dReal)(( ma[5]*ma[10] - ma[6]*ma[9]  ) * detRecip);
 225     dst[1] =  (dReal)(( ma[9]*ma[2]  - ma[1]*ma[10] ) * detRecip);
 226     dst[2] =  (dReal)(( ma[1]*ma[6]  - ma[5]*ma[2]  ) * detRecip);
 227
 228     dst[4] =  (dReal)(( ma[6]*ma[8]  - ma[4]*ma[10] ) * detRecip);
 229     dst[5] =  (dReal)(( ma[0]*ma[10] - ma[8]*ma[2]  ) * detRecip);
 230     dst[6] =  (dReal)(( ma[4]*ma[2]  - ma[0]*ma[6]  ) * detRecip);
 231
 232     dst[8] =  (dReal)(( ma[4]*ma[9]  - ma[8]*ma[5]  ) * detRecip);
 233     dst[9] =  (dReal)(( ma[8]*ma[1]  - ma[0]*ma[9]  ) * detRecip);
 234     dst[10] = (dReal)(( ma[0]*ma[5]  - ma[1]*ma[4]  ) * detRecip);
 235 }
 236
 237 static inline
 238 void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
 239 {
 240
 241     // Nguyen Binh : this code seem to be the fastest.
 242     dReal x0 =  source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
 243     dReal x1 =  source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
 244     dReal x2 =  source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
 245     dReal x3 =  source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];
 246
 247     dest[0]  =  quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
 248     dest[1]  =  quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
 249     dest[2]  =  quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;
 250
 251     /*
 252     // nVidia SDK implementation
 253     dVector3 uv, uuv;
 254     dVector3 qvec;
 255     qvec[0] = quat[1];
 256     qvec[1] = quat[2];
 257     qvec[2] = quat[3];
 258
 259     dVector3Cross(qvec,source,uv);
 260     dVector3Cross(qvec,uv,uuv);
 261
 262     dVector3Scale(uv,REAL(2.0)*quat[0]);
 263     dVector3Scale(uuv,REAL(2.0));
 264
 265     dest[0] = source[0] + uv[0] + uuv[0];
 266     dest[1] = source[1] + uv[1] + uuv[1];
 267     dest[2] = source[2] + uv[2] + uuv[2];
 268     */
 269 }
 270
 271 static inline
 272 void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
 273 {
 274
 275     dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
 276
 277     if (norm > REAL(0.0))
 278     {
 279         dQuaternion invQuat;
 280         invQuat[0] =  quat[0] / norm;
 281         invQuat[1] = -quat[1] / norm;
 282         invQuat[2] = -quat[2] / norm;
 283         invQuat[3] = -quat[3] / norm;
 284
 285         dQuatTransform(invQuat,source,dest);
 286
 287     }
 288     else
 289     {
 290         // Singular -> return identity
 291         dVector3Copy(source,dest);
 292     }
 293 }
 294
 295 static inline
 296 void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
 297 {
 298     rY = asin(mRot[0 * 4 + 2]);
 299     if (rY < M_PI /2)
 300     {
 301         if (rY > -M_PI /2)
 302         {
 303             rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
 304             rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
 305         }
 306         else
 307         {
 308             // not unique
 309             rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
 310             rZ = REAL(0.0);
 311         }
 312     }
 313     else
 314     {
 315         // not unique
 316         rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
 317         rZ = REAL(0.0);
 318     }
 319 }
 320
 321 static inline
 322 void dQuatInv(const dQuaternion& source, dQuaternion& dest)
 323 {
 324     dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
 325
 326     if (norm > 0.0f)
 327     {
 328         dReal neg_norm_recip = -REAL(1.0) / norm;
 329         dest[0] = -source[0] * neg_norm_recip;
 330         dest[1] = source[1] * neg_norm_recip;
 331         dest[2] = source[2] * neg_norm_recip;
 332         dest[3] = source[3] * neg_norm_recip;
 333     }
 334     else
 335     {
 336         // Singular -> return identity
 337         dest[0] = REAL(1.0);
 338         dest[1] = REAL(0.0);
 339         dest[2] = REAL(0.0);
 340         dest[3] = REAL(0.0);
 341     }
 342 }
 343
 344 // Finds barycentric
 345 static inline
 346 void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
 347     dReal w = REAL(1.0) - u - v;
 348
 349     Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
 350     Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
 351     Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
 352     Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
 353 }
 354
 355
 356
 357
 358 #endif