diff options
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_util.h')
-rw-r--r-- | libraries/ode-0.9/ode/src/collision_util.h | 340 |
1 files changed, 340 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_util.h b/libraries/ode-0.9/ode/src/collision_util.h new file mode 100644 index 0000000..4c479e3 --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_util.h | |||
@@ -0,0 +1,340 @@ | |||
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 | ||