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/odemath.h>
  35 #include <ode/rotation.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 inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
  46         dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
  47         return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
  48 }
  49 #endif
  50
  51
  52 // if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
  53 // return 1, else return 0.
  54
  55 int dCollideSpheres (dVector3 p1, dReal r1,
  56                      dVector3 p2, dReal r2, dContactGeom *c);
  57
  58
  59 // given two lines
  60 //    qa = pa + alpha* ua
  61 //    qb = pb + beta * ub
  62 // where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
  63 // beta go from [-inf,inf], return alpha and beta such that qa and qb are
  64 // as close as possible
  65
  66 void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
  67                            const dVector3 pb, const dVector3 ub,
  68                            dReal *alpha, dReal *beta);
  69
  70
  71 // given a line segment p1-p2 and a box (center 'c', rotation 'R', side length
  72 // vector 'side'), compute the points of closest approach between the box
  73 // and the line. return these points in 'lret' (the point on the line) and
  74 // 'bret' (the point on the box). if the line actually penetrates the box
  75 // then the solution is not unique, but only one solution will be returned.
  76 // in this case the solution points will coincide.
  77
  78 void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
  79                             const dVector3 c, const dMatrix3 R,
  80                             const dVector3 side,
  81                             dVector3 lret, dVector3 bret);
  82
  83 // 20 Apr 2004
  84 // Start code by Nguyen Binh
  85 int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);
  86 // clip polygon with plane and generate new polygon points
  87 void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane );
  88
  89 void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);
  90
  91 // Some vector math
  92 inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
  93 {
  94         c[0] = a[0] - b[0];
  95         c[1] = a[1] - b[1];
  96         c[2] = a[2] - b[2];
  97 }
  98
  99 // Some vector math
 100 inline void dVector3Scale(dVector3& a,dReal nScale)
 101 {
 102         a[0] *= nScale ;
 103         a[1] *= nScale ;
 104         a[2] *= nScale ;
 105 }
 106
 107 inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
 108 {
 109         c[0] = a[0] + b[0];
 110         c[1] = a[1] + b[1];
 111         c[2] = a[2] + b[2];
 112 }
 113
 114 inline void dVector3Copy(const dVector3& a,dVector3& c)
 115 {
 116         c[0] = a[0];
 117         c[1] = a[1];
 118         c[2] = a[2];
 119 }
 120
 121 inline void dVector4Copy(const dVector4& a,dVector4& c)
 122 {
 123         c[0] = a[0];
 124         c[1] = a[1];
 125         c[2] = a[2];
 126         c[3] = a[3];
 127 }
 128
 129 inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
 130 {
 131         dCROSS(c,=,a,b);
 132 }
 133
 134 inline dReal dVector3Length(const dVector3& a)
 135 {
 136         return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
 137 }
 138
 139 inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
 140 {
 141         return dDOT(a,b);
 142 }
 143
 144 inline void dVector3Inv(dVector3& a)
 145 {
 146         a[0] = -a[0];
 147         a[1] = -a[1];
 148         a[2] = -a[2];
 149 }
 150
 151 inline dReal dVector3Length2(const dVector3& a)
 152 {
 153         return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
 154 }
 155
 156 inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
 157 {
 158         v[0] = m[col + 0];
 159         v[1] = m[col + 4];
 160         v[2] = m[col + 8];
 161 }
 162
 163 inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
 164 {
 165         r[0] =  v[1] * m[2*4 + col] - v[2] * m[1*4 + col];
 166         r[1] =  v[2] * m[0*4 + col] - v[0] * m[2*4 + col];
 167         r[2] =  v[0] * m[1*4 + col] - v[1] * m[0*4 + col];
 168 }
 169
 170 inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
 171 {
 172         r[0] =   v[2] * m[1*4 + col] - v[1] * m[2*4 + col];
 173         r[1] =   v[0] * m[2*4 + col] - v[2] * m[0*4 + col];
 174         r[2] =   v[1] * m[0*4 + col] - v[0] * m[1*4 + col];
 175 }
 176
 177 inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
 178 {
 179         dMULTIPLY0_331(r,m,v);
 180 }
 181
 182 inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
 183 {
 184         return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
 185 }
 186
 187 inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
 188 {
 189         plane[0] = normal[0];
 190         plane[1] = normal[1];
 191         plane[2] = normal[2];
 192         plane[3] = distance;
 193 }
 194
 195 inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)
 196 {
 197         dest[0] =       source[0];
 198         dest[1] =       source[1];
 199         dest[2] =       source[2];
 200
 201         dest[4] =       source[4];
 202         dest[5] =       source[5];
 203         dest[6] =       source[6];
 204
 205         dest[8] =       source[8];
 206         dest[9] =       source[9];
 207         dest[10]=       source[10];
 208 }
 209
 210 inline dReal dMatrix3Det( const dMatrix3& mat )
 211 {
 212         dReal det;
 213
 214         det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
 215                 - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
 216                 + mat[2] * ( mat[4]*mat[9]  - mat[8]*mat[5] );
 217
 218         return( det );
 219 }
 220
 221
 222 inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
 223 {
 224         dReal det = dMatrix3Det( ma );
 225
 226         if ( dFabs( det ) < REAL(0.0005) )
 227         {
 228                 dRSetIdentity( dst );
 229                 return;
 230         }
 231
 232         dst[0] =    ma[5]*ma[10] - ma[6]*ma[9]   / det;
 233         dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det;
 234         dst[2] =    ma[1]*ma[6]  - ma[5]*ma[2]   / det;
 235
 236         dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det;
 237         dst[5] =    ma[0]*ma[10] - ma[8]*ma[2]   / det;
 238         dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det;
 239
 240         dst[8] =    ma[4]*ma[9] - ma[8]*ma[5]   / det;
 241         dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det;
 242         dst[10] =    ma[0]*ma[5] - ma[1]*ma[4]   / det;
 243 }
 244
 245 inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
 246 {
 247
 248         // Nguyen Binh : this code seem to be the fastest.
 249         dReal x0 =      source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
 250         dReal x1 =      source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
 251         dReal x2 =      source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
 252         dReal x3 =      source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];
 253
 254         dest[0]  =      quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
 255         dest[1]  =      quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
 256         dest[2]  =      quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;
 257
 258         /*
 259         // nVidia SDK implementation
 260         dVector3 uv, uuv;
 261         dVector3 qvec;
 262         qvec[0] = quat[1];
 263         qvec[1] = quat[2];
 264         qvec[2] = quat[3];
 265
 266         dVector3Cross(qvec,source,uv);
 267         dVector3Cross(qvec,uv,uuv);
 268
 269         dVector3Scale(uv,REAL(2.0)*quat[0]);
 270         dVector3Scale(uuv,REAL(2.0));
 271
 272         dest[0] = source[0] + uv[0] + uuv[0];
 273         dest[1] = source[1] + uv[1] + uuv[1];
 274         dest[2] = source[2] + uv[2] + uuv[2];
 275         */
 276 }
 277
 278 inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
 279 {
 280
 281         dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
 282
 283         if (norm > REAL(0.0))
 284         {
 285                 dQuaternion invQuat;
 286                 invQuat[0] =  quat[0] / norm;
 287                 invQuat[1] = -quat[1] / norm;
 288                 invQuat[2] = -quat[2] / norm;
 289                 invQuat[3] = -quat[3] / norm;
 290
 291                 dQuatTransform(invQuat,source,dest);
 292
 293         }
 294         else
 295         {
 296                 // Singular -> return identity
 297                 dVector3Copy(source,dest);
 298         }
 299 }
 300
 301 inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
 302 {
 303         rY = asin(mRot[0 * 4 + 2]);
 304         if (rY < M_PI /2)
 305         {
 306                 if (rY > -M_PI /2)
 307                 {
 308                         rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
 309                         rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
 310                 }
 311                 else
 312                 {
 313                         // not unique
 314                         rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
 315                         rZ = REAL(0.0);
 316                 }
 317         }
 318         else
 319         {
 320                 // not unique
 321                 rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
 322                 rZ = REAL(0.0);
 323         }
 324 }
 325
 326 inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)
 327 {
 328         dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
 329
 330         if (norm > 0.0f)
 331         {
 332                 dest[0] = source[0] / norm;
 333                 dest[1] = -source[1] / norm;
 334                 dest[2] = -source[2] / norm;
 335                 dest[3] = -source[3] / norm;
 336         }
 337         else
 338         {
 339                 // Singular -> return identity
 340                 dest[0] = REAL(1.0);
 341                 dest[1] = REAL(0.0);
 342                 dest[2] = REAL(0.0);
 343                 dest[3] = REAL(0.0);
 344         }
 345 }
 346
 347
 348 #endif