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