Open Dynamics Engine
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 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 static inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
93 {
94  dSubtractVectors3(c, a, b);
95 }
96 
97 static inline void dVector3Scale(dVector3& a,dReal nScale)
98 {
99  dScaleVector3(a, nScale);
100 }
101 
102 static inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
103 {
104  dAddVectors3(c, a, b);
105 }
106 
107 static inline void dVector3Copy(const dVector3& a,dVector3& c)
108 {
109  dCopyVector3(c, a);
110 }
111 
112 static inline void dVector4Copy(const dVector4& a,dVector4& c)
113 {
114  dCopyVector4(c, a);
115 }
116 
117 static inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
118 {
119  dCalcVectorCross3(c, a, b);
120 }
121 
122 static inline dReal dVector3Length(const dVector3& a)
123 {
124  return dCalcVectorLength3(a);
125 }
126 
127 static inline dReal dVector3LengthSquare(const dVector3& a)
128 {
129  return dCalcVectorLengthSquare3(a);
130 }
131 
132 static inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
133 {
134  return dCalcVectorDot3(a, b);
135 }
136 
137 static inline void dVector3Inv(dVector3& a)
138 {
139  dNegateVector3(a);
140 }
141 
142 static inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
143 {
144  dGetMatrixColumn3(v, m, col);
145 }
146 
147 static inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
148 {
149  dCalcVectorCross3_114(r, v, m + col);
150 }
151 
152 static inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
153 {
154  dCalcVectorCross3_141(r, m + col, v);
155 }
156 
157 static inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
158 {
159  dMultiply0_331(r, m, v);
160 }
161 
162 static inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
163 {
164  return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
165 }
166 
167 static inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
168 {
169  plane[0] = normal[0];
170  plane[1] = normal[1];
171  plane[2] = normal[2];
172  plane[3] = distance;
173 }
174 
175 static inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)
176 {
177  dCopyMatrix4x3(dest, source);
178 }
179 
180 static inline dReal dMatrix3Det( const dMatrix3& mat )
181 {
182  dReal det;
183 
184  det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
185  - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
186  + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] );
187 
188  return( det );
189 }
190 
191 
192 inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
193 {
194  dReal det = dMatrix3Det( ma );
195 
196  if ( dFabs( det ) < REAL(0.0005) )
197  {
198  dRSetIdentity( dst );
199  return;
200  }
201 
202  double detRecip = REAL(1.0) / det;
203 
204  dst[0] = ( ma[5]*ma[10] - ma[6]*ma[9] ) * detRecip;
205  dst[1] = ( ma[9]*ma[2] - ma[1]*ma[10] ) * detRecip;
206  dst[2] = ( ma[1]*ma[6] - ma[5]*ma[2] ) * detRecip;
207 
208  dst[4] = ( ma[6]*ma[8] - ma[4]*ma[10] ) * detRecip;
209  dst[5] = ( ma[0]*ma[10] - ma[8]*ma[2] ) * detRecip;
210  dst[6] = ( ma[4]*ma[2] - ma[0]*ma[6] ) * detRecip;
211 
212  dst[8] = ( ma[4]*ma[9] - ma[8]*ma[5] ) * detRecip;
213  dst[9] = ( ma[8]*ma[1] - ma[0]*ma[9] ) * detRecip;
214  dst[10] = ( ma[0]*ma[5] - ma[1]*ma[4] ) * detRecip;
215 }
216 
217 inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
218 {
219 
220  // Nguyen Binh : this code seem to be the fastest.
221  dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
222  dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
223  dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
224  dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];
225 
226  dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
227  dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
228  dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;
229 
230  /*
231  // nVidia SDK implementation
232  dVector3 uv, uuv;
233  dVector3 qvec;
234  qvec[0] = quat[1];
235  qvec[1] = quat[2];
236  qvec[2] = quat[3];
237 
238  dVector3Cross(qvec,source,uv);
239  dVector3Cross(qvec,uv,uuv);
240 
241  dVector3Scale(uv,REAL(2.0)*quat[0]);
242  dVector3Scale(uuv,REAL(2.0));
243 
244  dest[0] = source[0] + uv[0] + uuv[0];
245  dest[1] = source[1] + uv[1] + uuv[1];
246  dest[2] = source[2] + uv[2] + uuv[2];
247  */
248 }
249 
250 inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
251 {
252 
253  dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
254 
255  if (norm > REAL(0.0))
256  {
257  dQuaternion invQuat;
258  invQuat[0] = quat[0] / norm;
259  invQuat[1] = -quat[1] / norm;
260  invQuat[2] = -quat[2] / norm;
261  invQuat[3] = -quat[3] / norm;
262 
263  dQuatTransform(invQuat,source,dest);
264 
265  }
266  else
267  {
268  // Singular -> return identity
269  dVector3Copy(source,dest);
270  }
271 }
272 
273 inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
274 {
275  rY = asin(mRot[0 * 4 + 2]);
276  if (rY < M_PI /2)
277  {
278  if (rY > -M_PI /2)
279  {
280  rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
281  rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
282  }
283  else
284  {
285  // not unique
286  rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
287  rZ = REAL(0.0);
288  }
289  }
290  else
291  {
292  // not unique
293  rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
294  rZ = REAL(0.0);
295  }
296 }
297 
298 inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)
299 {
300  dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
301 
302  if (norm > 0.0f)
303  {
304  dest[0] = source[0] / norm;
305  dest[1] = -source[1] / norm;
306  dest[2] = -source[2] / norm;
307  dest[3] = -source[3] / norm;
308  }
309  else
310  {
311  // Singular -> return identity
312  dest[0] = REAL(1.0);
313  dest[1] = REAL(0.0);
314  dest[2] = REAL(0.0);
315  dest[3] = REAL(0.0);
316  }
317 }
318 
319 
320 #endif
Describe the contact point between two geoms.
Definition: contact.h:88