aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_util.h
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_util.h')
-rw-r--r--libraries/ode-0.9/ode/src/collision_util.h340
1 files changed, 0 insertions, 340 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_util.h b/libraries/ode-0.9/ode/src/collision_util.h
deleted file mode 100644
index 4c479e3..0000000
--- a/libraries/ode-0.9/ode/src/collision_util.h
+++ /dev/null
@@ -1,340 +0,0 @@
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
25some 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
45inline 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
55int 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
66void 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
78void 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
85int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);
86// clip polygon with plane and generate new polygon points
87void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane );
88
89void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);
90
91// Some vector math
92inline 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
100inline void dVector3Scale(dVector3& a,dReal nScale)
101{
102 a[0] *= nScale ;
103 a[1] *= nScale ;
104 a[2] *= nScale ;
105}
106
107inline 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
114inline 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
121inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
122{
123 dCROSS(c,=,a,b);
124}
125
126inline dReal dVector3Length(const dVector3& a)
127{
128 return dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
129}
130
131inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
132{
133 return dDOT(a,b);
134}
135
136inline void dVector3Inv(dVector3& a)
137{
138 a[0] = -a[0];
139 a[1] = -a[1];
140 a[2] = -a[2];
141}
142
143inline dReal dVector3Length2(const dVector3& a)
144{
145 return (a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
146}
147
148inline 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
155inline 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
162inline 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
169inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
170{
171 dMULTIPLY0_331(r,m,v);
172}
173
174inline 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
179inline 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
187inline 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
202inline 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
214inline 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
237inline 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
270inline 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
293inline 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
318inline 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
generated by cgit v1.1 at 2024-12-28 07:09:12 +0000