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 dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c) |
---|
122 | { |
---|
123 | dCROSS(c,=,a,b); |
---|
124 | } |
---|
125 | |
---|
126 | inline dReal dVector3Length(const dVector3& a) |
---|
127 | { |
---|
128 | return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]); |
---|
129 | } |
---|
130 | |
---|
131 | inline dReal dVector3Dot(const dVector3& a,const dVector3& b) |
---|
132 | { |
---|
133 | return dDOT(a,b); |
---|
134 | } |
---|
135 | |
---|
136 | inline void dVector3Inv(dVector3& a) |
---|
137 | { |
---|
138 | a[0] = -a[0]; |
---|
139 | a[1] = -a[1]; |
---|
140 | a[2] = -a[2]; |
---|
141 | } |
---|
142 | |
---|
143 | inline dReal dVector3Length2(const dVector3& a) |
---|
144 | { |
---|
145 | return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]); |
---|
146 | } |
---|
147 | |
---|
148 | inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v) |
---|
149 | { |
---|
150 | v[0] = m[col + 0]; |
---|
151 | v[1] = m[col + 4]; |
---|
152 | v[2] = m[col + 8]; |
---|
153 | } |
---|
154 | |
---|
155 | inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) |
---|
156 | { |
---|
157 | r[0] = v[1] * m[2*4 + col] - v[2] * m[1*4 + col]; |
---|
158 | r[1] = v[2] * m[0*4 + col] - v[0] * m[2*4 + col]; |
---|
159 | r[2] = v[0] * m[1*4 + col] - v[1] * m[0*4 + col]; |
---|
160 | } |
---|
161 | |
---|
162 | inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) |
---|
163 | { |
---|
164 | r[0] = v[2] * m[1*4 + col] - v[1] * m[2*4 + col]; |
---|
165 | r[1] = v[0] * m[2*4 + col] - v[2] * m[0*4 + col]; |
---|
166 | r[2] = v[1] * m[0*4 + col] - v[0] * m[1*4 + col]; |
---|
167 | } |
---|
168 | |
---|
169 | inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r) |
---|
170 | { |
---|
171 | dMULTIPLY0_331(r,m,v); |
---|
172 | } |
---|
173 | |
---|
174 | inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane) |
---|
175 | { |
---|
176 | return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]); |
---|
177 | } |
---|
178 | |
---|
179 | inline void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane) |
---|
180 | { |
---|
181 | plane[0] = normal[0]; |
---|
182 | plane[1] = normal[1]; |
---|
183 | plane[2] = normal[2]; |
---|
184 | plane[3] = distance; |
---|
185 | } |
---|
186 | |
---|
187 | inline void dMatrix3Copy(const dReal* source,dMatrix3& dest) |
---|
188 | { |
---|
189 | dest[0] = source[0]; |
---|
190 | dest[1] = source[1]; |
---|
191 | dest[2] = source[2]; |
---|
192 | |
---|
193 | dest[4] = source[4]; |
---|
194 | dest[5] = source[5]; |
---|
195 | dest[6] = source[6]; |
---|
196 | |
---|
197 | dest[8] = source[8]; |
---|
198 | dest[9] = source[9]; |
---|
199 | dest[10]= source[10]; |
---|
200 | } |
---|
201 | |
---|
202 | inline dReal dMatrix3Det( const dMatrix3& mat ) |
---|
203 | { |
---|
204 | dReal det; |
---|
205 | |
---|
206 | det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) |
---|
207 | - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) |
---|
208 | + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); |
---|
209 | |
---|
210 | return( det ); |
---|
211 | } |
---|
212 | |
---|
213 | |
---|
214 | inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst ) |
---|
215 | { |
---|
216 | dReal det = dMatrix3Det( ma ); |
---|
217 | |
---|
218 | if ( dFabs( det ) < REAL(0.0005) ) |
---|
219 | { |
---|
220 | dRSetIdentity( dst ); |
---|
221 | return; |
---|
222 | } |
---|
223 | |
---|
224 | dst[0] = ma[5]*ma[10] - ma[6]*ma[9] / det; |
---|
225 | dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det; |
---|
226 | dst[2] = ma[1]*ma[6] - ma[5]*ma[2] / det; |
---|
227 | |
---|
228 | dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det; |
---|
229 | dst[5] = ma[0]*ma[10] - ma[8]*ma[2] / det; |
---|
230 | dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det; |
---|
231 | |
---|
232 | dst[8] = ma[4]*ma[9] - ma[8]*ma[5] / det; |
---|
233 | dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det; |
---|
234 | dst[10] = ma[0]*ma[5] - ma[1]*ma[4] / det; |
---|
235 | } |
---|
236 | |
---|
237 | inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) |
---|
238 | { |
---|
239 | |
---|
240 | // Nguyen Binh : this code seem to be the fastest. |
---|
241 | dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3]; |
---|
242 | dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1]; |
---|
243 | dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2]; |
---|
244 | dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3]; |
---|
245 | |
---|
246 | dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1; |
---|
247 | dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2; |
---|
248 | dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0; |
---|
249 | |
---|
250 | /* |
---|
251 | // nVidia SDK implementation |
---|
252 | dVector3 uv, uuv; |
---|
253 | dVector3 qvec; |
---|
254 | qvec[0] = quat[1]; |
---|
255 | qvec[1] = quat[2]; |
---|
256 | qvec[2] = quat[3]; |
---|
257 | |
---|
258 | dVector3Cross(qvec,source,uv); |
---|
259 | dVector3Cross(qvec,uv,uuv); |
---|
260 | |
---|
261 | dVector3Scale(uv,REAL(2.0)*quat[0]); |
---|
262 | dVector3Scale(uuv,REAL(2.0)); |
---|
263 | |
---|
264 | dest[0] = source[0] + uv[0] + uuv[0]; |
---|
265 | dest[1] = source[1] + uv[1] + uuv[1]; |
---|
266 | dest[2] = source[2] + uv[2] + uuv[2]; |
---|
267 | */ |
---|
268 | } |
---|
269 | |
---|
270 | inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest) |
---|
271 | { |
---|
272 | |
---|
273 | dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]; |
---|
274 | |
---|
275 | if (norm > REAL(0.0)) |
---|
276 | { |
---|
277 | dQuaternion invQuat; |
---|
278 | invQuat[0] = quat[0] / norm; |
---|
279 | invQuat[1] = -quat[1] / norm; |
---|
280 | invQuat[2] = -quat[2] / norm; |
---|
281 | invQuat[3] = -quat[3] / norm; |
---|
282 | |
---|
283 | dQuatTransform(invQuat,source,dest); |
---|
284 | |
---|
285 | } |
---|
286 | else |
---|
287 | { |
---|
288 | // Singular -> return identity |
---|
289 | dVector3Copy(source,dest); |
---|
290 | } |
---|
291 | } |
---|
292 | |
---|
293 | inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ) |
---|
294 | { |
---|
295 | rY = asin(mRot[0 * 4 + 2]); |
---|
296 | if (rY < M_PI /2) |
---|
297 | { |
---|
298 | if (rY > -M_PI /2) |
---|
299 | { |
---|
300 | rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]); |
---|
301 | rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]); |
---|
302 | } |
---|
303 | else |
---|
304 | { |
---|
305 | // not unique |
---|
306 | rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]); |
---|
307 | rZ = REAL(0.0); |
---|
308 | } |
---|
309 | } |
---|
310 | else |
---|
311 | { |
---|
312 | // not unique |
---|
313 | rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]); |
---|
314 | rZ = REAL(0.0); |
---|
315 | } |
---|
316 | } |
---|
317 | |
---|
318 | inline void dQuatInv(const dQuaternion& source, dQuaternion& dest) |
---|
319 | { |
---|
320 | dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3]; |
---|
321 | |
---|
322 | if (norm > 0.0f) |
---|
323 | { |
---|
324 | dest[0] = source[0] / norm; |
---|
325 | dest[1] = -source[1] / norm; |
---|
326 | dest[2] = -source[2] / norm; |
---|
327 | dest[3] = -source[3] / norm; |
---|
328 | } |
---|
329 | else |
---|
330 | { |
---|
331 | // Singular -> return identity |
---|
332 | dest[0] = REAL(1.0); |
---|
333 | dest[1] = REAL(0.0); |
---|
334 | dest[2] = REAL(0.0); |
---|
335 | dest[3] = REAL(0.0); |
---|
336 | } |
---|
337 | } |
---|
338 | |
---|
339 | |
---|
340 | #endif |
---|