aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/TerrainAndCone
diff options
context:
space:
mode:
authordan miller2007-10-19 05:20:48 +0000
committerdan miller2007-10-19 05:20:48 +0000
commitd48ea5bb797037069d641da41da0f195f0124491 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/contrib/TerrainAndCone
parentdont ask (diff)
downloadopensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.zip
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.gz
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.bz2
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.xz
one more for the gipper
Diffstat (limited to '')
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h100
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp504
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp662
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp659
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/readme.txt322
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp1375
6 files changed, 3622 insertions, 0 deletions
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h b/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h
new file mode 100644
index 0000000..b445353
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h
@@ -0,0 +1,100 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2#ifndef _ODE_COLLISION_STD_INTERNAL_H_
3#define _ODE_COLLISION_STD_INTERNAL_H_
4
5#include <ode/common.h>
6#include "collision_kernel.h"
7
8struct dxSphere : public dxGeom {
9 dReal radius; // sphere radius
10 dxSphere (dSpaceID space, dReal _radius);
11 void computeAABB();
12};
13
14
15struct dxBox : public dxGeom {
16 dVector3 side; // side lengths (x,y,z)
17 dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
18 void computeAABB();
19};
20
21
22struct dxCCylinder : public dxGeom {
23 dReal radius,lz; // radius, length along z axis
24 dxCCylinder (dSpaceID space, dReal _radius, dReal _length);
25 void computeAABB();
26};
27
28
29struct dxPlane : public dxGeom {
30 dReal p[4];
31 dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
32 void computeAABB();
33};
34
35struct dxCylinder : public dxGeom {
36 dReal radius,lz; // radius, length along z axis
37 dxCylinder (dSpaceID space, dReal _radius, dReal _length);
38 void computeAABB();
39};
40
41struct dxCone : public dxGeom {
42 dReal radius,lz;
43 dxCone(dSpaceID space, dReal _radius,dReal _length);
44 ~dxCone();
45 void computeAABB();
46};
47
48struct dxRay : public dxGeom {
49 dReal length;
50 dxRay (dSpaceID space, dReal _length);
51 void computeAABB();
52};
53
54struct dxTerrainY : public dxGeom {
55 dReal m_vLength;
56 dReal *m_pHeights;
57 dReal m_vMinHeight;
58 dReal m_vMaxHeight;
59 dReal m_vNodeLength;
60 int m_nNumNodesPerSide;
61 int m_nNumNodesPerSideShift;
62 int m_nNumNodesPerSideMask;
63 int m_bFinite;
64 dxTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
65 ~dxTerrainY();
66 void computeAABB();
67 dReal GetHeight(dReal x,dReal z);
68 dReal GetHeight(int x,int z);
69 int dCollideTerrainUnit(int x,int z,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip);
70 bool IsOnTerrain(int nx,int nz,int w,dReal *pos);
71};
72
73struct dxTerrainZ : public dxGeom {
74 dReal m_vLength;
75 dReal *m_pHeights;
76 dReal m_vMinHeight;
77 dReal m_vMaxHeight;
78 dReal m_vNodeLength;
79 int m_nNumNodesPerSide;
80 int m_nNumNodesPerSideShift;
81 int m_nNumNodesPerSideMask;
82 int m_bFinite;
83 dxTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
84 ~dxTerrainZ();
85 void computeAABB();
86 dReal GetHeight(dReal x,dReal y);
87 dReal GetHeight(int x,int y);
88 int dCollideTerrainUnit(int x,int y,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip);
89 bool IsOnTerrain(int nx,int ny,int w,dReal *pos);
90};
91
92#ifndef MIN
93#define MIN(a,b) ((a<b)?a:b)
94#endif
95
96#ifndef MAX
97#define MAX(a,b) ((a>b)?a:b)
98#endif
99
100#endif //_ODE_COLLISION_STD_INTERNAL_H_ \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
new file mode 100644
index 0000000..8c5c3be
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
@@ -0,0 +1,504 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2//some code inspired by Magic Software
3#include <ode/common.h>
4#include <ode/collision.h>
5#include <ode/matrix.h>
6#include <ode/rotation.h>
7#include <ode/odemath.h>
8#include "collision_kernel.h"
9#include "collision_std.h"
10#include "collision_std_internal.h"
11#include "collision_util.h"
12#include <drawstuff/drawstuff.h>
13#include "windows.h"
14#include "ode\ode.h"
15
16#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
17const dReal fEPSILON = 1e-9f;
18
19dxCone::dxCone (dSpaceID space, dReal _radius,dReal _length) :
20dxGeom (space,1)
21{
22 dAASSERT(_radius > 0.f);
23 dAASSERT(_length > 0.f);
24 type = dConeClass;
25 radius = _radius;
26 lz = _length;
27}
28
29dxCone::~dxCone()
30{
31}
32
33void dxCone::computeAABB()
34{
35 const dMatrix3& R = final_posr->R;
36 const dVector3& pos = final_posr->pos;
37
38 dReal xrange = dFabs(R[2] * lz) + radius;
39 dReal yrange = dFabs(R[6] * lz) + radius;
40 dReal zrange = dFabs(R[10] * lz) + radius;
41 aabb[0] = pos[0] - xrange;
42 aabb[1] = pos[0] + xrange;
43 aabb[2] = pos[1] - yrange;
44 aabb[3] = pos[1] + yrange;
45 aabb[4] = pos[2] - zrange;
46 aabb[5] = pos[2] + zrange;
47}
48
49dGeomID dCreateCone(dSpaceID space, dReal _radius,dReal _length)
50{
51 return new dxCone(space,_radius,_length);
52}
53
54void dGeomConeSetParams (dGeomID g, dReal _radius, dReal _length)
55{
56 dUASSERT (g && g->type == dConeClass,"argument not a cone");
57 dAASSERT (_radius > 0.f);
58 dAASSERT (_length > 0.f);
59 g->recomputePosr();
60 dxCone *c = (dxCone*) g;
61 c->radius = _radius;
62 c->lz = _length;
63 dGeomMoved (g);
64}
65
66
67void dGeomConeGetParams (dGeomID g, dReal *_radius, dReal *_length)
68{
69 dUASSERT (g && g->type == dConeClass,"argument not a cone");
70 g->recomputePosr();
71 dxCone *c = (dxCone*) g;
72 *_radius = c->radius;
73 *_length = c->lz;
74}
75
76//positive inside
77dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z)
78{
79 dUASSERT (g && g->type == dConeClass,"argument not a cone");
80
81 g->recomputePosr();
82 dxCone *cone = (dxCone*) g;
83
84 dVector3 tmp,q;
85 tmp[0] = x - cone->final_posr->pos[0];
86 tmp[1] = y - cone->final_posr->pos[1];
87 tmp[2] = z - cone->final_posr->pos[2];
88 dMULTIPLY1_331 (q,cone->final_posr->R,tmp);
89
90 dReal r = cone->radius;
91 dReal h = cone->lz;
92
93 dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]);
94 dReal d1 = q[2];
95 dReal d2 = h-q[2];
96
97 if (d0 < d1) {
98 if (d0 < d2) return d0; else return d2;
99 }
100 else {
101 if (d1 < d2) return d1; else return d2;
102 }
103}
104
105//plane plane
106bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
107 dVector3 LinePos,dVector3 LineDir)
108{
109 // If Cross(N0,N1) is zero, then either planes are parallel and separated
110 // or the same plane. In both cases, 'false' is returned. Otherwise,
111 // the intersection line is
112 //
113 // L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
114 //
115 // for some coefficients c0 and c1 and for t any real number (the line
116 // parameter). Taking dot products with the normals,
117 //
118 // d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
119 // d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
120 //
121 // which are two equations in two unknowns. The solution is
122 //
123 // c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
124 // c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
125 //
126 // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
127/*
128 Real fN00 = rkPlane0.Normal().SquaredLength();
129 Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
130 Real fN11 = rkPlane1.Normal().SquaredLength();
131 Real fDet = fN00*fN11 - fN01*fN01;
132
133 if ( Math::FAbs(fDet) < gs_fEpsilon )
134 return false;
135
136 Real fInvDet = 1.0f/fDet;
137 Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
138 Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;
139
140 rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
141 rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
142 return true;
143*/
144 dReal fN00 = dLENGTHSQUARED(Plane0);
145 dReal fN01 = dDOT(Plane0,Plane1);
146 dReal fN11 = dLENGTHSQUARED(Plane1);
147 dReal fDet = fN00*fN11 - fN01*fN01;
148
149 if ( fabs(fDet) < fEPSILON)
150 return false;
151
152 dReal fInvDet = 1.0f/fDet;
153 dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
154 dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;
155
156 dCROSS(LineDir,=,Plane0,Plane1);
157 dNormalize3(LineDir);
158
159 dVector3 Temp0,Temp1;
160 dOPC(Temp0,*,Plane0,fC0);
161 dOPC(Temp1,*,Plane1,fC1);
162 dOP(LinePos,+,Temp0,Temp1);
163
164 return true;
165}
166
167//plane ray
168bool FindIntersectionPlaneRay(const dReal Plane[4],
169 const dVector3 &LinePos,const dVector3 &LineDir,
170 dReal &u,dVector3 &Pos)
171{
172/*
173 u = (A*X1 + B*Y1 + C*Z1 + D) / (A*(X1-X2) + B*(Y1-Y2)+C*(Z1-Z2))
174*/
175 dReal fDet = -dDot(Plane,LineDir,3);
176
177 if ( fabs(fDet) < fEPSILON)
178 return false;
179
180 u = (dDot(Plane,LinePos,3) - Plane[3]) / fDet;
181 dOPC(Pos,*,LineDir,u);
182 dOPE(Pos,+=,LinePos);
183
184 return true;
185}
186
187int SolveQuadraticPolynomial(dReal a,dReal b,dReal c,dReal &x0,dReal &x1)
188{
189 dReal d = b*b - 4*a*c;
190 int NumRoots = 0;
191 dReal dr;
192
193 if (d < 0.f)
194 return NumRoots;
195
196 if (d == 0.f)
197 {
198 NumRoots = 1;
199 dr = 0.f;
200 }
201 else
202 {
203 NumRoots = 2;
204 dr = sqrtf(d);
205 }
206
207 x0 = (-b -dr) / (2.f * a);
208 x1 = (-b +dr) / (2.f * a);
209
210 return NumRoots;
211}
212/*
213const int VALID_INTERSECTION = 1<<0;
214const int POS_TEST_FAILEDT0 = 1<<0;
215const int POS_TEST_FAILEDT1 = 1<<1;
216*/
217int ProcessConeRayIntersectionPoint( dReal r,dReal h,
218 const dVector3 &q,const dVector3 &v,dReal t,
219 dVector3 &p,
220 dVector3 &n,
221 int &f)
222{
223 dOPC(p,*,v,t);
224 dOPE(p,+=,q);
225 n[0] = 2*p[0];
226 n[1] = 2*p[1];
227 n[2] = -2*p[2]*r*r/(h*h);
228
229 f = 0;
230 if (p[2] > h) return 0;
231 if (p[2] < 0) return 0;
232 if (t > 1) return 0;
233 if (t < 0) return 0;
234
235 return 1;
236}
237
238//cone ray
239//line in cone space (position,direction)
240//distance from line position (direction normalized)(if any)
241//return the number of intersection
242int FindIntersectionConeRay(dReal r,dReal h,
243 const dVector3 &q,const dVector3 &v,dContactGeom *pContact)
244{
245 dVector3 qp,vp;
246 dOPE(qp,=,q);
247 dOPE(vp,=,v);
248 qp[2] = h-q[2];
249 vp[2] = -v[2];
250 dReal ts = (r/h);
251 ts *= ts;
252 dReal a = vp[0]*vp[0] + vp[1]*vp[1] - ts*vp[2]*vp[2];
253 dReal b = 2.f*qp[0]*vp[0] + 2.f*qp[1]*vp[1] - 2.f*ts*qp[2]*vp[2];
254 dReal c = qp[0]*qp[0] + qp[1]*qp[1] - ts*qp[2]*qp[2];
255
256/*
257 dReal a = v[0]*v[0] + v[1]*v[1] - (v[2]*v[2]*r*r) / (h*h);
258 dReal b = 2.f*q[0]*v[0] + 2.f*q[1]*v[1] + 2.f*r*r*v[2]/h - 2*r*r*q[0]*v[0]/(h*h);
259 dReal c = q[0]*q[0] + q[1]*q[1] + 2*r*r*q[2]/h - r*r*q[2]/(h*h) - r*r;
260*/
261 int nNumRoots=SolveQuadraticPolynomial(a,b,c,pContact[0].depth,pContact[1].depth);
262 int flag = 0;
263
264 dContactGeom ValidContact[2];
265
266 int nNumValidContacts = 0;
267 for (int i=0;i<nNumRoots;i++)
268 {
269 if (ProcessConeRayIntersectionPoint(r,h,q,v,pContact[i].depth,pContact[i].pos,
270 pContact[i].normal,flag))
271 {
272 ValidContact[nNumValidContacts] = pContact[i];
273 nNumValidContacts++;
274 }
275 }
276
277 dOP(qp,+,q,v);
278
279 if ((nNumValidContacts < 2) && (v[2] != 0.f))
280 {
281 dReal d = (0.f-q[2]) / (v[2]);
282 if ((d>=0) && (d<=1))
283 {
284 dOPC(vp,*,v,d);
285 dOP(qp,+,q,vp);
286
287 if (qp[0]*qp[0]+qp[1]*qp[1] < r*r)
288 {
289 dOPE(ValidContact[nNumValidContacts].pos,=,qp);
290 ValidContact[nNumValidContacts].normal[0] = 0.f;
291 ValidContact[nNumValidContacts].normal[1] = 0.f;
292 ValidContact[nNumValidContacts].normal[2] = -1.f;
293 ValidContact[nNumValidContacts].depth = d;
294 nNumValidContacts++;
295 }
296 }
297 }
298
299 if (nNumValidContacts == 2)
300 {
301 if (ValidContact[0].depth > ValidContact[1].depth)
302 {
303 pContact[0] = ValidContact[1];
304 pContact[1] = ValidContact[0];
305 }
306 else
307 {
308 pContact[0] = ValidContact[0];
309 pContact[1] = ValidContact[1];
310 }
311 }
312 else if (nNumValidContacts == 1)
313 {
314 pContact[0] = ValidContact[0];
315 }
316
317 return nNumValidContacts;
318}
319
320int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags,
321 dContactGeom *contact, int skip)
322{
323 dIASSERT (skip >= (int)sizeof(dContactGeom));
324 dIASSERT (o1->type == dConeClass);
325 dIASSERT (o2->type == dPlaneClass);
326 dxCone *cone = (dxCone*) o1;
327 dxPlane *plane = (dxPlane*) o2;
328
329 contact->g1 = o1;
330 contact->g2 = o2;
331
332 dVector3 p0,p1,pp0,pp1;
333 dOPE(p0,=,cone->final_posr->pos);
334 p1[0] = cone->final_posr->R[0*4+2] * cone->lz + p0[0];
335 p1[1] = cone->final_posr->R[1*4+2] * cone->lz + p0[1];
336 p1[2] = cone->final_posr->R[2*4+2] * cone->lz + p0[2];
337
338 dReal u;
339 FindIntersectionPlaneRay(plane->p,p0,plane->p,u,pp0);
340 FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);
341
342 if (dDISTANCE(pp0,pp1) < fEPSILON)
343 {
344 p1[0] = cone->final_posr->R[0*4+0] * cone->lz + p0[0];
345 p1[1] = cone->final_posr->R[1*4+0] * cone->lz + p0[1];
346 p1[2] = cone->final_posr->R[2*4+0] * cone->lz + p0[2];
347 FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);
348 dIASSERT(dDISTANCE(pp0,pp1) >= fEPSILON);
349 }
350 dVector3 h,r0,r1;
351 h[0] = cone->final_posr->R[0*4+2];
352 h[1] = cone->final_posr->R[1*4+2];
353 h[2] = cone->final_posr->R[2*4+2];
354
355 dOP(r0,-,pp0,pp1);
356 dCROSS(r1,=,h,r0);
357 dCROSS(r0,=,r1,h);
358 dNormalize3(r0);
359 dOPEC(h,*=,cone->lz);
360 dOPEC(r0,*=,cone->radius);
361
362 dVector3 p[3];
363 dOP(p[0],+,cone->final_posr->pos,h);
364 dOP(p[1],+,cone->final_posr->pos,r0);
365 dOP(p[2],-,cone->final_posr->pos,r0);
366
367 int numMaxContacts = flags & 0xffff;
368 if (numMaxContacts == 0)
369 numMaxContacts = 1;
370
371 int n=0;
372 for (int i=0;i<3;i++)
373 {
374 dReal d = dGeomPlanePointDepth(o2, p[i][0], p[i][1], p[i][2]);
375
376 if (d>0.f)
377 {
378 CONTACT(contact,n*skip)->g1 = o1;
379 CONTACT(contact,n*skip)->g2 = o2;
380 dOPE(CONTACT(contact,n*skip)->normal,=,plane->p);
381 dOPE(CONTACT(contact,n*skip)->pos,=,p[i]);
382 CONTACT(contact,n*skip)->depth = d;
383 n++;
384
385 if (n == numMaxContacts)
386 return n;
387 }
388 }
389
390 return n;
391}
392
393int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags,
394 dContactGeom *contact, int skip)
395{
396 dIASSERT (skip >= (int)sizeof(dContactGeom));
397 dIASSERT (o1->type == dRayClass);
398 dIASSERT (o2->type == dConeClass);
399 dxRay *ray = (dxRay*) o1;
400 dxCone *cone = (dxCone*) o2;
401
402 contact->g1 = o1;
403 contact->g2 = o2;
404
405 dVector3 tmp,q,v;
406 tmp[0] = ray->final_posr->pos[0] - cone->final_posr->pos[0];
407 tmp[1] = ray->final_posr->pos[1] - cone->final_posr->pos[1];
408 tmp[2] = ray->final_posr->pos[2] - cone->final_posr->pos[2];
409 dMULTIPLY1_331 (q,cone->final_posr->R,tmp);
410 tmp[0] = ray->final_posr->R[0*4+2] * ray->length;
411 tmp[1] = ray->final_posr->R[1*4+2] * ray->length;
412 tmp[2] = ray->final_posr->R[2*4+2] * ray->length;
413 dMULTIPLY1_331 (v,cone->final_posr->R,tmp);
414
415 dReal r = cone->radius;
416 dReal h = cone->lz;
417
418 dContactGeom Contact[2];
419
420 if (FindIntersectionConeRay(r,h,q,v,Contact))
421 {
422 dMULTIPLY0_331(contact->normal,cone->final_posr->R,Contact[0].normal);
423 dMULTIPLY0_331(contact->pos,cone->final_posr->R,Contact[0].pos);
424 dOPE(contact->pos,+=,cone->final_posr->pos);
425 contact->depth = Contact[0].depth * dLENGTH(v);
426/*
427 dMatrix3 RI;
428 dRSetIdentity (RI);
429 dVector3 ss;
430 ss[0] = 0.01f;
431 ss[1] = 0.01f;
432 ss[2] = 0.01f;
433
434 dsSetColorAlpha (1,0,0,0.8f);
435 dsDrawBox(contact->pos,RI,ss);
436*/
437 return 1;
438 }
439
440 return 0;
441}
442
443int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
444{
445 dIASSERT (skip >= (int)sizeof(dContactGeom));
446 dIASSERT (o1->type == dConeClass);
447 dIASSERT (o2->type == dSphereClass);
448 dxCone *cone = (dxCone*) o1;
449
450 dxSphere ASphere(0,cone->radius);
451 dGeomSetRotation(&ASphere,cone->final_posr->R);
452 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
453
454 return dCollideSphereSphere(&ASphere, o2, flags, contact, skip);
455}
456
457int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
458{
459 dIASSERT (skip >= (int)sizeof(dContactGeom));
460 dIASSERT (o1->type == dConeClass);
461 dIASSERT (o2->type == dBoxClass);
462 dxCone *cone = (dxCone*) o1;
463
464 dxSphere ASphere(0,cone->radius);
465 dGeomSetRotation(&ASphere,cone->final_posr->R);
466 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
467
468 return dCollideSphereBox(&ASphere, o2, flags, contact, skip);
469}
470
471int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
472{
473 dIASSERT (skip >= (int)sizeof(dContactGeom));
474 dIASSERT (o1->type == dCCylinderClass);
475 dIASSERT (o2->type == dConeClass);
476 dxCone *cone = (dxCone*) o2;
477
478 dxSphere ASphere(0,cone->radius);
479 dGeomSetRotation(&ASphere,cone->final_posr->R);
480 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
481
482 return dCollideCCylinderSphere(o1, &ASphere, flags, contact, skip);
483}
484
485extern int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
486
487int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
488{
489 dIASSERT (skip >= (int)sizeof(dContactGeom));
490 dIASSERT (o1->type == dTriMeshClass);
491 dIASSERT (o2->type == dConeClass);
492 dxCone *cone = (dxCone*) o2;
493
494 dxSphere ASphere(0,cone->radius);
495 dGeomSetRotation(&ASphere,cone->final_posr->R);
496 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
497
498 return dCollideSTL(o1, &ASphere, flags, contact, skip);
499}
500
501
502
503
504
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
new file mode 100644
index 0000000..ff779ac
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
@@ -0,0 +1,662 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2//some code inspired by Magic Software
3#include <ode/common.h>
4#include <ode/collision.h>
5#include <ode/matrix.h>
6#include <ode/rotation.h>
7#include <ode/odemath.h>
8#include "collision_kernel.h"
9#include "collision_std.h"
10#include "collision_std_internal.h"
11#include "collision_util.h"
12//#include <drawstuff/drawstuff.h>
13#include "windows.h"
14#include "ode\ode.h"
15
16#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
17#define MAXCONTACT 10
18#define TERRAINTOL 0.0f
19
20static bool IsAPowerOfTwo(int f)
21{
22 dAASSERT(f!=0);
23 while ((f&1) != 1)
24 f >>= 1;
25
26 return (f == 1);
27}
28
29static int GetPowerOfTwo(int f)
30{
31 dAASSERT(f!=0);
32 int n = 0;
33 while ((f&1) != 1)
34 {
35 n++;
36 f >>= 1;
37 }
38
39 return n;
40}
41
42dxTerrainY::dxTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
43dxGeom (space,bPlaceable)
44{
45 dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
46 dIASSERT(pHeights);
47 dIASSERT(vLength > 0.f);
48 dIASSERT(nNumNodesPerSide > 0);
49 type = dTerrainYClass;
50 m_vLength = vLength;
51 m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
52 dIASSERT(m_pHeights);
53 m_nNumNodesPerSide = nNumNodesPerSide;
54 m_vNodeLength = m_vLength / m_nNumNodesPerSide;
55 m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
56 m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1;
57 m_vMinHeight = dInfinity;
58 m_vMaxHeight = -dInfinity;
59 m_bFinite = bFinite;
60
61 for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
62 {
63 m_pHeights[i] = pHeights[i];
64 if (m_pHeights[i] < m_vMinHeight) m_vMinHeight = m_pHeights[i];
65 if (m_pHeights[i] > m_vMaxHeight) m_vMaxHeight = m_pHeights[i];
66 }
67}
68
69dxTerrainY::~dxTerrainY()
70{
71 dIASSERT(m_pHeights);
72 delete [] m_pHeights;
73}
74
75void dxTerrainY::computeAABB()
76{
77 if (m_bFinite)
78 {
79 if (gflags & GEOM_PLACEABLE)
80 {
81 dReal dx[6],dy[6],dz[6];
82 dx[0] = 0;
83 dx[1] = final_posr->R[0] * m_vLength;
84 dx[2] = final_posr->R[1] * m_vMinHeight;
85 dx[3] = final_posr->R[1] * m_vMaxHeight;
86 dx[4] = 0;
87 dx[5] = final_posr->R[2] * m_vLength;
88
89 dy[0] = 0;
90 dy[1] = final_posr->R[4] * m_vLength;
91 dy[2] = final_posr->R[5] * m_vMinHeight;
92 dy[3] = final_posr->R[5] * m_vMaxHeight;
93 dy[4] = 0;
94 dy[5] = final_posr->R[6] * m_vLength;
95
96 dz[0] = 0;
97 dz[1] = final_posr->R[8] * m_vLength;
98 dz[2] = final_posr->R[9] * m_vMinHeight;
99 dz[3] = final_posr->R[9] * m_vMaxHeight;
100 dz[4] = 0;
101 dz[5] = final_posr->R[10] * m_vLength;
102
103 aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
104 aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
105 aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
106 aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
107 aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
108 aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
109 }
110 else
111 {
112 aabb[0] = 0;
113 aabb[1] = m_vLength;
114 aabb[2] = m_vMinHeight;
115 aabb[3] = m_vMaxHeight;
116 aabb[4] = 0;
117 aabb[5] = m_vLength;
118 }
119 }
120 else
121 {
122 if (gflags & GEOM_PLACEABLE)
123 {
124 aabb[0] = -dInfinity;
125 aabb[1] = dInfinity;
126 aabb[2] = -dInfinity;
127 aabb[3] = dInfinity;
128 aabb[4] = -dInfinity;
129 aabb[5] = dInfinity;
130 }
131 else
132 {
133 aabb[0] = -dInfinity;
134 aabb[1] = dInfinity;
135 aabb[2] = m_vMinHeight;
136 aabb[3] = m_vMaxHeight;
137 aabb[4] = -dInfinity;
138 aabb[5] = dInfinity;
139 }
140 }
141}
142
143dReal dxTerrainY::GetHeight(int x,int z)
144{
145 return m_pHeights[ (((unsigned int)(z) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
146 + ((unsigned int)(x) & m_nNumNodesPerSideMask)];
147}
148
149dReal dxTerrainY::GetHeight(dReal x,dReal z)
150{
151 int nX = int(floor(x / m_vNodeLength));
152 int nZ = int(floor(z / m_vNodeLength));
153 dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
154 dReal dz = (z - (dReal(nZ) * m_vNodeLength)) / m_vNodeLength;
155 dIASSERT((dx >= 0.f) && (dx <= 1.f));
156 dIASSERT((dz >= 0.f) && (dz <= 1.f));
157
158 dReal y,y0;
159
160 if (dx + dz < 1.f)
161 {
162 y0 = GetHeight(nX,nZ);
163 y = y0
164 + (GetHeight(nX+1,nZ) - y0) * dx
165 + (GetHeight(nX,nZ+1) - y0) * dz;
166 }
167 else
168 {
169 y0 = GetHeight(nX+1,nZ+1);
170 y = y0
171 + (GetHeight(nX+1,nZ) - y0) * (1.f - dz)
172 + (GetHeight(nX,nZ+1) - y0) * (1.f - dx);
173 }
174
175 return y;
176}
177
178bool dxTerrainY::IsOnTerrain(int nx,int nz,int w,dReal *pos)
179{
180 dVector3 Min,Max;
181 Min[0] = nx * m_vNodeLength;
182 Min[2] = nz * m_vNodeLength;
183 Max[0] = (nx+1) * m_vNodeLength;
184 Max[2] = (nz+1) * m_vNodeLength;
185 dReal Tol = m_vNodeLength * TERRAINTOL;
186
187 if ((pos[0]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
188 return false;
189
190 if ((pos[2]<Min[2]-Tol) || (pos[2]>Max[2]+Tol))
191 return false;
192
193 dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
194 dReal dz = (pos[2] - (dReal(nz) * m_vNodeLength)) / m_vNodeLength;
195
196 if ((w == 0) && (dx + dz > 1.f+TERRAINTOL))
197 return false;
198
199 if ((w == 1) && (dx + dz < 1.f-TERRAINTOL))
200 return false;
201
202 return true;
203}
204
205dGeomID dCreateTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
206{
207 return new dxTerrainY(space, pHeights,vLength,nNumNodesPerSide,bFinite,bPlaceable);
208}
209
210dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z)
211{
212 dUASSERT (g && g->type == dTerrainYClass,"argument not a terrain");
213 g->recomputePosr();
214 dxTerrainY *t = (dxTerrainY*) g;
215 return t->GetHeight(x,z) - y;
216}
217
218typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z);
219#define RECOMPUTE_RAYNORMAL
220//#define DO_RAYDEPTH
221
222#define DMESS(A) \
223 dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
224 x,z,A, \
225 pContact->depth, \
226 dGeomSphereGetRadius(o2), \
227 pContact->pos[0], \
228 pContact->pos[1], \
229 pContact->pos[2], \
230 pContact->normal[0], \
231 pContact->normal[1], \
232 pContact->normal[2]);
233/*
234(y is up)
235
236A-B-E.x
237|/|
238C-D
239|
240F
241.
242z
243*/
244int dxTerrainY::dCollideTerrainUnit(
245 int x,int z,dxGeom *o2,int numMaxContacts,
246 int flags,dContactGeom *contact, int skip)
247{
248 dColliderFn *CollideRayN;
249 dColliderFn *CollideNPlane;
250 dGetDepthFn *GetDepth;
251 int numContacts = 0;
252 int numPlaneContacts = 0;
253 int i;
254
255 if (numContacts == numMaxContacts)
256 return numContacts;
257
258 dContactGeom PlaneContact[MAXCONTACT];
259 flags = (flags & 0xffff0000) | MAXCONTACT;
260
261 switch (o2->type)
262 {
263 case dSphereClass:
264 CollideRayN = dCollideRaySphere;
265 CollideNPlane = dCollideSpherePlane;
266 GetDepth = dGeomSpherePointDepth;
267 break;
268 case dBoxClass:
269 CollideRayN = dCollideRayBox;
270 CollideNPlane = dCollideBoxPlane;
271 GetDepth = dGeomBoxPointDepth;
272 break;
273 case dCCylinderClass:
274 CollideRayN = dCollideRayCCylinder;
275 CollideNPlane = dCollideCCylinderPlane;
276 GetDepth = dGeomCCylinderPointDepth;
277 break;
278 case dRayClass:
279 CollideRayN = NULL;
280 CollideNPlane = dCollideRayPlane;
281 GetDepth = NULL;
282 break;
283 case dConeClass:
284 CollideRayN = dCollideRayCone;
285 CollideNPlane = dCollideConePlane;
286 GetDepth = dGeomConePointDepth;
287 break;
288 default:
289 dIASSERT(0);
290 }
291
292 dReal Plane[4],lBD,lCD,lBC;
293 dVector3 A,B,C,D,BD,CD,BC,AB,AC;
294 A[0] = x * m_vNodeLength;
295 A[2] = z* m_vNodeLength;
296 A[1] = GetHeight(x,z);
297 B[0] = (x+1) * m_vNodeLength;
298 B[2] = z * m_vNodeLength;
299 B[1] = GetHeight(x+1,z);
300 C[0] = x * m_vNodeLength;
301 C[2] = (z+1) * m_vNodeLength;
302 C[1] = GetHeight(x,z+1);
303 D[0] = (x+1) * m_vNodeLength;
304 D[2] = (z+1) * m_vNodeLength;
305 D[1] = GetHeight(x+1,z+1);
306
307 dOP(BC,-,C,B);
308 lBC = dLENGTH(BC);
309 dOPEC(BC,/=,lBC);
310
311 dOP(BD,-,D,B);
312 lBD = dLENGTH(BD);
313 dOPEC(BD,/=,lBD);
314
315 dOP(CD,-,D,C);
316 lCD = dLENGTH(CD);
317 dOPEC(CD,/=,lCD);
318
319 dOP(AB,-,B,A);
320 dNormalize3(AB);
321
322 dOP(AC,-,C,A);
323 dNormalize3(AC);
324
325 if (CollideRayN)
326 {
327#ifdef RECOMPUTE_RAYNORMAL
328 dVector3 E,F;
329 dVector3 CE,FB,AD;
330 dVector3 Normal[3];
331 E[0] = (x+2) * m_vNodeLength;
332 E[2] = z * m_vNodeLength;
333 E[1] = GetHeight(x+2,z);
334 F[0] = x * m_vNodeLength;
335 F[2] = (z+2) * m_vNodeLength;
336 F[1] = GetHeight(x,z+2);
337 dOP(AD,-,D,A);
338 dNormalize3(AD);
339 dOP(CE,-,E,C);
340 dNormalize3(CE);
341 dOP(FB,-,B,F);
342 dNormalize3(FB);
343
344 //BC
345 dCROSS(Normal[0],=,BC,AD);
346 dNormalize3(Normal[0]);
347
348 //BD
349 dCROSS(Normal[1],=,BD,CE);
350 dNormalize3(Normal[1]);
351
352 //CD
353 dCROSS(Normal[2],=,CD,FB);
354 dNormalize3(Normal[2]);
355#endif
356 int nA[3],nB[3];
357 dContactGeom ContactA[3],ContactB[3];
358 dxRay rayBC(0,lBC);
359 dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
360 nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom));
361 dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
362 nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom));
363
364 dxRay rayBD(0,lBD);
365 dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
366 nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom));
367 dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
368 nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom));
369
370 dxRay rayCD(0,lCD);
371 dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
372 nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom));
373 dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
374 nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom));
375
376 for (i=0;i<3;i++)
377 {
378 if (nA[i] & nB[i])
379 {
380 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
381 pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2;
382 pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2;
383 pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2;
384#ifdef RECOMPUTE_RAYNORMAL
385 pContact->normal[0] = -Normal[i][0];
386 pContact->normal[1] = -Normal[i][1];
387 pContact->normal[2] = -Normal[i][2];
388#else
389 pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f;
390 pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f;
391 pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f;
392 dNormalize3(pContact->normal);
393#endif
394#ifdef DO_RAYDEPTH
395 dxRay rayV(0,1000.f);
396 dGeomRaySet(&rayV, pContact->pos[0],
397 pContact->pos[1],
398 pContact->pos[2],
399 -pContact->normal[0],
400 -pContact->normal[1],
401 -pContact->normal[2]);
402
403 dContactGeom ContactV;
404 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
405 {
406 pContact->depth = ContactV.depth;
407 numContacts++;
408 }
409#else
410
411 if (GetDepth == NULL)
412 {
413 dxRay rayV(0,1000.f);
414 dGeomRaySet(&rayV, pContact->pos[0],
415 pContact->pos[1],
416 pContact->pos[2],
417 -pContact->normal[0],
418 -pContact->normal[1],
419 -pContact->normal[2]);
420
421 dContactGeom ContactV;
422
423 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
424 {
425 pContact->depth = ContactV.depth;
426 numContacts++;
427 }
428 }
429 else
430 {
431 pContact->depth = GetDepth(o2,
432 pContact->pos[0],
433 pContact->pos[1],
434 pContact->pos[2]);
435 numContacts++;
436 }
437
438#endif
439 if (numContacts == numMaxContacts)
440 return numContacts;
441
442 }
443 }
444 }
445
446 dCROSS(Plane,=,AC,AB);
447 dNormalize3(Plane);
448 Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
449 dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
450 numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
451
452 for (i=0;i<numPlaneContacts;i++)
453 {
454 if (IsOnTerrain(x,z,0,PlaneContact[i].pos))
455 {
456 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
457 pContact->pos[0] = PlaneContact[i].pos[0];
458 pContact->pos[1] = PlaneContact[i].pos[1];
459 pContact->pos[2] = PlaneContact[i].pos[2];
460 pContact->normal[0] = -PlaneContact[i].normal[0];
461 pContact->normal[1] = -PlaneContact[i].normal[1];
462 pContact->normal[2] = -PlaneContact[i].normal[2];
463 pContact->depth = PlaneContact[i].depth;
464
465 //DMESS(0);
466 numContacts++;
467
468 if (numContacts == numMaxContacts)
469 return numContacts;
470 }
471 }
472
473 dCROSS(Plane,=,BD,CD);
474 dNormalize3(Plane);
475 Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
476 dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
477 numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
478
479 for (i=0;i<numPlaneContacts;i++)
480 {
481 if (IsOnTerrain(x,z,1,PlaneContact[i].pos))
482 {
483 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
484 pContact->pos[0] = PlaneContact[i].pos[0];
485 pContact->pos[1] = PlaneContact[i].pos[1];
486 pContact->pos[2] = PlaneContact[i].pos[2];
487 pContact->normal[0] = -PlaneContact[i].normal[0];
488 pContact->normal[1] = -PlaneContact[i].normal[1];
489 pContact->normal[2] = -PlaneContact[i].normal[2];
490 pContact->depth = PlaneContact[i].depth;
491 //DMESS(1);
492 numContacts++;
493
494 if (numContacts == numMaxContacts)
495 return numContacts;
496 }
497 }
498
499 return numContacts;
500}
501
502int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip)
503{
504 dIASSERT (skip >= (int)sizeof(dContactGeom));
505 dIASSERT (o1->type == dTerrainYClass);
506 int i,j;
507
508 if ((flags & 0xffff) == 0)
509 flags = (flags & 0xffff0000) | 1;
510
511 int numMaxTerrainContacts = (flags & 0xffff);
512 dxTerrainY *terrain = (dxTerrainY*) o1;
513
514 dReal aabbbak[6];
515 int gflagsbak;
516
517 dVector3 pos0;
518 int numTerrainContacts = 0;
519
520 dxPosR *bak;
521 dxPosR X1;
522
523 if (terrain->gflags & GEOM_PLACEABLE)
524 {
525 dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
526 dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
527 dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
528 bak = o2->final_posr;
529 o2->final_posr = &X1;
530 memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
531 gflagsbak = o2->gflags;
532 o2->computeAABB();
533 }
534
535 int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
536 int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
537 int nMinZ = int(floor(o2->aabb[4] / terrain->m_vNodeLength));
538 int nMaxZ = int(floor(o2->aabb[5] / terrain->m_vNodeLength)) + 1;
539
540 if (terrain->m_bFinite)
541 {
542 nMinX = MAX(nMinX,0);
543 nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
544 nMinZ = MAX(nMinZ,0);
545 nMaxZ = MIN(nMaxZ,terrain->m_nNumNodesPerSide);
546
547 if ((nMinX >= nMaxX) || (nMinZ >= nMaxZ))
548 goto dCollideTerrainYExit;
549 }
550
551 dVector3 AabbTop;
552 AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
553 AabbTop[2] = (o2->aabb[4]+o2->aabb[5]) / 2;
554 AabbTop[1] = o2->aabb[3];
555 if (o2->type != dRayClass)
556 {
557 dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[2]) - AabbTop[1];
558 if (AabbTopDepth > 0.f)
559 {
560 contact->depth = AabbTopDepth;
561 dReal MaxDepth = (o2->aabb[3]-o2->aabb[2]) / 2;
562 if (contact->depth > MaxDepth)
563 contact->depth = MaxDepth;
564 contact->g1 = o1;
565 contact->g2 = o2;
566 dOPE(contact->pos,=,AabbTop);
567 contact->normal[0] = 0.f;
568 contact->normal[1] = -1.f;
569 contact->normal[2] = 0.f;
570
571 numTerrainContacts = 1;
572 goto dCollideTerrainYExit;
573 }
574 }
575
576 for (i=nMinX;i<nMaxX;i++)
577 {
578 for (j=nMinZ;j<nMaxZ;j++)
579 {
580 numTerrainContacts += terrain->dCollideTerrainUnit(
581 i,j,o2,numMaxTerrainContacts - numTerrainContacts,
582 flags,CONTACT(contact,numTerrainContacts*skip),skip );
583 }
584 }
585
586 dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
587
588 for (i=0; i<numTerrainContacts; i++)
589 {
590 CONTACT(contact,i*skip)->g1 = o1;
591 CONTACT(contact,i*skip)->g2 = o2;
592 }
593
594dCollideTerrainYExit:
595
596 if (terrain->gflags & GEOM_PLACEABLE)
597 {
598 o2->final_posr = bak;
599 memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
600 o2->gflags = gflagsbak;
601
602 for (i=0; i<numTerrainContacts; i++)
603 {
604 dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
605 dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
606 dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos);
607
608 dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
609 dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
610 }
611 }
612
613 return numTerrainContacts;
614}
615/*
616void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
617{
618 float A[3],B[3],C[3],D[3];
619 float R[12];
620 float pos[3];
621 if (pR)
622 memcpy(R,pR,sizeof(R));
623 else
624 {
625 memset(R,0,sizeof(R));
626 R[0] = 1.f;
627 R[5] = 1.f;
628 R[10] = 1.f;
629 }
630
631 if (ppos)
632 memcpy(pos,ppos,sizeof(pos));
633 else
634 memset(pos,0,sizeof(pos));
635
636 float vx,vz;
637 vx = vLength * x;
638 vz = vLength * z;
639
640 int i;
641 for (i=0;i<nNumNodesPerSide;i++)
642 {
643 for (int j=0;j<nNumNodesPerSide;j++)
644 {
645 A[0] = i * vNodeLength + vx;
646 A[2] = j * vNodeLength + vz;
647 A[1] = GetHeight(i,j,nNumNodesPerSide,pHeights);
648 B[0] = (i+1) * vNodeLength + vx;
649 B[2] = j * vNodeLength + vz;
650 B[1] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
651 C[0] = i * vNodeLength + vx;
652 C[2] = (j+1) * vNodeLength + vz;
653 C[1] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
654 D[0] = (i+1) * vNodeLength + vx;
655 D[2] = (j+1) * vNodeLength + vz;
656 D[1] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
657 dsDrawTriangle(pos,R,C,B,A,1);
658 dsDrawTriangle(pos,R,D,B,C,1);
659 }
660 }
661}
662*/
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
new file mode 100644
index 0000000..d47c402
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
@@ -0,0 +1,659 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2//some code inspired by Magic Software
3#include <ode/common.h>
4#include <ode/collision.h>
5#include <ode/matrix.h>
6#include <ode/rotation.h>
7#include <ode/odemath.h>
8#include "collision_kernel.h"
9#include "collision_std.h"
10#include "collision_std_internal.h"
11#include "collision_util.h"
12//#include <drawstuff/drawstuff.h>
13#include "windows.h"
14#include "ode\ode.h"
15
16#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
17#define MAXCONTACT 10
18#define TERRAINTOL 0.0f
19
20static bool IsAPowerOfTwo(int f)
21{
22 dAASSERT(f!=0);
23 while ((f&1) != 1)
24 f >>= 1;
25
26 return (f == 1);
27}
28
29static int GetPowerOfTwo(int f)
30{
31 dAASSERT(f!=0);
32 int n = 0;
33 while ((f&1) != 1)
34 {
35 n++;
36 f >>= 1;
37 }
38
39 return n;
40}
41
42dxTerrainZ::dxTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
43dxGeom (space,bPlaceable)
44{
45 dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
46 dIASSERT(pHeights);
47 dIASSERT(vLength > 0.f);
48 dIASSERT(nNumNodesPerSide > 0);
49 type = dTerrainZClass;
50 m_vLength = vLength;
51 m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
52 dIASSERT(m_pHeights);
53 m_nNumNodesPerSide = nNumNodesPerSide;
54 m_vNodeLength = m_vLength / m_nNumNodesPerSide;
55 m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
56 m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1;
57 m_vMinHeight = dInfinity;
58 m_vMaxHeight = -dInfinity;
59 m_bFinite = bFinite;
60
61 for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
62 {
63 m_pHeights[i] = pHeights[i];
64 if (m_pHeights[i] < m_vMinHeight) m_vMinHeight = m_pHeights[i];
65 if (m_pHeights[i] > m_vMaxHeight) m_vMaxHeight = m_pHeights[i];
66 }
67}
68
69dxTerrainZ::~dxTerrainZ()
70{
71 dIASSERT(m_pHeights);
72 delete [] m_pHeights;
73}
74
75void dxTerrainZ::computeAABB()
76{
77 if (m_bFinite)
78 {
79 if (gflags & GEOM_PLACEABLE)
80 {
81 dReal dx[6],dy[6],dz[6];
82 dx[0] = 0;
83 dx[1] = final_posr->R[0] * m_vLength;
84 dx[2] = 0;
85 dx[3] = final_posr->R[1] * m_vLength;
86 dx[4] = final_posr->R[2] * m_vMinHeight;
87 dx[5] = final_posr->R[2] * m_vMaxHeight;
88
89 dy[0] = 0;
90 dy[1] = final_posr->R[4] * m_vLength;
91 dy[2] = 0;
92 dy[3] = final_posr->R[5] * m_vLength;
93 dy[4] = final_posr->R[6] * m_vMinHeight;
94 dy[5] = final_posr->R[6] * m_vMaxHeight;
95
96 dz[0] = 0;
97 dz[1] = final_posr->R[8] * m_vLength;
98 dz[2] = 0;
99 dz[3] = final_posr->R[9] * m_vLength;
100 dz[4] = final_posr->R[10] * m_vMinHeight;
101 dz[5] = final_posr->R[10] * m_vMaxHeight;
102
103 aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
104 aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
105 aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
106 aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
107 aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
108 aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
109 }
110 else
111 {
112 aabb[0] = 0;
113 aabb[1] = m_vLength;
114 aabb[2] = 0;
115 aabb[3] = m_vLength;
116 aabb[4] = m_vMinHeight;
117 aabb[5] = m_vMaxHeight;
118 }
119 }
120 else
121 {
122 if (gflags & GEOM_PLACEABLE)
123 {
124 aabb[0] = -dInfinity;
125 aabb[1] = dInfinity;
126 aabb[2] = -dInfinity;
127 aabb[3] = dInfinity;
128 aabb[4] = -dInfinity;
129 aabb[5] = dInfinity;
130 }
131 else
132 {
133 aabb[0] = -dInfinity;
134 aabb[1] = dInfinity;
135 aabb[2] = -dInfinity;
136 aabb[3] = dInfinity;
137 aabb[4] = m_vMinHeight;
138 aabb[5] = m_vMaxHeight;
139 }
140 }
141}
142
143dReal dxTerrainZ::GetHeight(int x,int y)
144{
145 return m_pHeights[ (((unsigned int)(y) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
146 + ((unsigned int)(x) & m_nNumNodesPerSideMask)];
147}
148
149dReal dxTerrainZ::GetHeight(dReal x,dReal y)
150{
151 int nX = int(floor(x / m_vNodeLength));
152 int nY = int(floor(y / m_vNodeLength));
153 dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
154 dReal dy = (y - (dReal(nY) * m_vNodeLength)) / m_vNodeLength;
155 dIASSERT((dx >= 0.f) && (dx <= 1.f));
156 dIASSERT((dy >= 0.f) && (dy <= 1.f));
157
158 dReal z,z0;
159
160 if (dx + dy < 1.f)
161 {
162 z0 = GetHeight(nX,nY);
163 z = z0
164 + (GetHeight(nX+1,nY) - z0) * dx
165 + (GetHeight(nX,nY+1) - z0) * dy;
166 }
167 else
168 {
169 z0 = GetHeight(nX+1,nY+1);
170 z = z0
171 + (GetHeight(nX+1,nY) - z0) * (1.f - dy)
172 + (GetHeight(nX,nY+1) - z0) * (1.f - dx);
173 }
174
175 return z;
176}
177
178bool dxTerrainZ::IsOnTerrain(int nx,int ny,int w,dReal *pos)
179{
180 dVector3 Min,Max;
181 Min[0] = nx * m_vNodeLength;
182 Min[1] = ny * m_vNodeLength;
183 Max[0] = (nx+1) * m_vNodeLength;
184 Max[1] = (ny+1) * m_vNodeLength;
185 dReal Tol = m_vNodeLength * TERRAINTOL;
186
187 if ((pos[0]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
188 return false;
189
190 if ((pos[1]<Min[1]-Tol) || (pos[1]>Max[1]+Tol))
191 return false;
192
193 dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
194 dReal dy = (pos[1] - (dReal(ny) * m_vNodeLength)) / m_vNodeLength;
195
196 if ((w == 0) && (dx + dy > 1.f+TERRAINTOL))
197 return false;
198
199 if ((w == 1) && (dx + dy < 1.f-TERRAINTOL))
200 return false;
201
202 return true;
203}
204
205dGeomID dCreateTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
206{
207 return new dxTerrainZ(space, pHeights,vLength,nNumNodesPerSide, bFinite, bPlaceable);
208}
209
210dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z)
211{
212 dUASSERT (g && g->type == dTerrainZClass,"argument not a terrain");
213 g->recomputePosr();
214 dxTerrainZ *t = (dxTerrainZ*) g;
215 return t->GetHeight(x,y) - z;
216}
217
218typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z);
219#define RECOMPUTE_RAYNORMAL
220//#define DO_RAYDEPTH
221
222#define DMESS(A) \
223 dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
224 x,y,A, \
225 pContact->depth, \
226 dGeomSphereGetRadius(o2), \
227 pContact->pos[0], \
228 pContact->pos[1], \
229 pContact->pos[2], \
230 pContact->normal[0], \
231 pContact->normal[1], \
232 pContact->normal[2]);
233/*
234(z is up)
235
236y
237.
238F
239|
240C-D
241|\|
242A-B-E.x
243*/
244int dxTerrainZ::dCollideTerrainUnit(
245 int x,int y,dxGeom *o2,int numMaxContacts,
246 int flags,dContactGeom *contact, int skip)
247{
248 dColliderFn *CollideRayN;
249 dColliderFn *CollideNPlane;
250 dGetDepthFn *GetDepth;
251 int numContacts = 0;
252 int numPlaneContacts = 0;
253 int i;
254
255 if (numContacts == numMaxContacts)
256 return numContacts;
257
258 dContactGeom PlaneContact[MAXCONTACT];
259 flags = (flags & 0xffff0000) | MAXCONTACT;
260
261 switch (o2->type)
262 {
263 case dSphereClass:
264 CollideRayN = dCollideRaySphere;
265 CollideNPlane = dCollideSpherePlane;
266 GetDepth = dGeomSpherePointDepth;
267 break;
268 case dBoxClass:
269 CollideRayN = dCollideRayBox;
270 CollideNPlane = dCollideBoxPlane;
271 GetDepth = dGeomBoxPointDepth;
272 break;
273 case dCCylinderClass:
274 CollideRayN = dCollideRayCCylinder;
275 CollideNPlane = dCollideCCylinderPlane;
276 GetDepth = dGeomCCylinderPointDepth;
277 break;
278 case dRayClass:
279 CollideRayN = NULL;
280 CollideNPlane = dCollideRayPlane;
281 GetDepth = NULL;
282 break;
283 case dConeClass:
284 CollideRayN = dCollideRayCone;
285 CollideNPlane = dCollideConePlane;
286 GetDepth = dGeomConePointDepth;
287 break;
288 default:
289 dIASSERT(0);
290 }
291
292 dReal Plane[4],lBD,lCD,lBC;
293 dVector3 A,B,C,D,BD,CD,BC,AB,AC;
294 A[0] = x * m_vNodeLength;
295 A[1] = y * m_vNodeLength;
296 A[2] = GetHeight(x,y);
297 B[0] = (x+1) * m_vNodeLength;
298 B[1] = y * m_vNodeLength;
299 B[2] = GetHeight(x+1,y);
300 C[0] = x * m_vNodeLength;
301 C[1] = (y+1) * m_vNodeLength;
302 C[2] = GetHeight(x,y+1);
303 D[0] = (x+1) * m_vNodeLength;
304 D[1] = (y+1) * m_vNodeLength;
305 D[2] = GetHeight(x+1,y+1);
306
307 dOP(BC,-,C,B);
308 lBC = dLENGTH(BC);
309 dOPEC(BC,/=,lBC);
310
311 dOP(BD,-,D,B);
312 lBD = dLENGTH(BD);
313 dOPEC(BD,/=,lBD);
314
315 dOP(CD,-,D,C);
316 lCD = dLENGTH(CD);
317 dOPEC(CD,/=,lCD);
318
319 dOP(AB,-,B,A);
320 dNormalize3(AB);
321
322 dOP(AC,-,C,A);
323 dNormalize3(AC);
324
325 if (CollideRayN)
326 {
327#ifdef RECOMPUTE_RAYNORMAL
328 dVector3 E,F;
329 dVector3 CE,FB,AD;
330 dVector3 Normal[3];
331 E[0] = (x+2) * m_vNodeLength;
332 E[1] = y * m_vNodeLength;
333 E[2] = GetHeight(x+2,y);
334 F[0] = x * m_vNodeLength;
335 F[1] = (y+2) * m_vNodeLength;
336 F[2] = GetHeight(x,y+2);
337 dOP(AD,-,D,A);
338 dNormalize3(AD);
339 dOP(CE,-,E,C);
340 dNormalize3(CE);
341 dOP(FB,-,B,F);
342 dNormalize3(FB);
343
344 //BC
345 dCROSS(Normal[0],=,AD,BC);
346 dNormalize3(Normal[0]);
347
348 //BD
349 dCROSS(Normal[1],=,CE,BD);
350 dNormalize3(Normal[1]);
351
352 //CD
353 dCROSS(Normal[2],=,FB,CD);
354 dNormalize3(Normal[2]);
355#endif
356 int nA[3],nB[3];
357 dContactGeom ContactA[3],ContactB[3];
358 dxRay rayBC(0,lBC);
359 dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
360 nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom));
361 dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
362 nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom));
363
364 dxRay rayBD(0,lBD);
365 dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
366 nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom));
367 dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
368 nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom));
369
370 dxRay rayCD(0,lCD);
371 dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
372 nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom));
373 dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
374 nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom));
375
376 for (i=0;i<3;i++)
377 {
378 if (nA[i] & nB[i])
379 {
380 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
381 pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2;
382 pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2;
383 pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2;
384#ifdef RECOMPUTE_RAYNORMAL
385 pContact->normal[0] = -Normal[i][0];
386 pContact->normal[1] = -Normal[i][1];
387 pContact->normal[2] = -Normal[i][2];
388#else
389 pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f;
390 pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f;
391 pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f;
392 dNormalize3(pContact->normal);
393#endif
394#ifdef DO_RAYDEPTH
395 dxRay rayV(0,1000.f);
396 dGeomRaySet(&rayV, pContact->pos[0],
397 pContact->pos[1],
398 pContact->pos[2],
399 -pContact->normal[0],
400 -pContact->normal[1],
401 -pContact->normal[2]);
402
403 dContactGeom ContactV;
404 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
405 {
406 pContact->depth = ContactV.depth;
407 numContacts++;
408 }
409#else
410 if (GetDepth == NULL)
411 {
412 dxRay rayV(0,1000.f);
413 dGeomRaySet(&rayV, pContact->pos[0],
414 pContact->pos[1],
415 pContact->pos[2],
416 -pContact->normal[0],
417 -pContact->normal[1],
418 -pContact->normal[2]);
419
420 dContactGeom ContactV;
421 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
422 {
423 pContact->depth = ContactV.depth;
424 numContacts++;
425 }
426 }
427 else
428 {
429 pContact->depth = GetDepth(o2,
430 pContact->pos[0],
431 pContact->pos[1],
432 pContact->pos[2]);
433 numContacts++;
434 }
435#endif
436 if (numContacts == numMaxContacts)
437 return numContacts;
438
439 }
440 }
441 }
442
443 dCROSS(Plane,=,AB,AC);
444 dNormalize3(Plane);
445 Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
446 dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
447 numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
448
449 for (i=0;i<numPlaneContacts;i++)
450 {
451 if (IsOnTerrain(x,y,0,PlaneContact[i].pos))
452 {
453 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
454 pContact->pos[0] = PlaneContact[i].pos[0];
455 pContact->pos[1] = PlaneContact[i].pos[1];
456 pContact->pos[2] = PlaneContact[i].pos[2];
457 pContact->normal[0] = -PlaneContact[i].normal[0];
458 pContact->normal[1] = -PlaneContact[i].normal[1];
459 pContact->normal[2] = -PlaneContact[i].normal[2];
460 pContact->depth = PlaneContact[i].depth;
461
462 //DMESS(0);
463 numContacts++;
464
465 if (numContacts == numMaxContacts)
466 return numContacts;
467 }
468 }
469
470 dCROSS(Plane,=,CD,BD);
471 dNormalize3(Plane);
472 Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
473 dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
474 numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
475
476 for (i=0;i<numPlaneContacts;i++)
477 {
478 if (IsOnTerrain(x,y,1,PlaneContact[i].pos))
479 {
480 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
481 pContact->pos[0] = PlaneContact[i].pos[0];
482 pContact->pos[1] = PlaneContact[i].pos[1];
483 pContact->pos[2] = PlaneContact[i].pos[2];
484 pContact->normal[0] = -PlaneContact[i].normal[0];
485 pContact->normal[1] = -PlaneContact[i].normal[1];
486 pContact->normal[2] = -PlaneContact[i].normal[2];
487 pContact->depth = PlaneContact[i].depth;
488 //DMESS(1);
489 numContacts++;
490
491 if (numContacts == numMaxContacts)
492 return numContacts;
493 }
494 }
495
496 return numContacts;
497}
498
499int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip)
500{
501 dIASSERT (skip >= (int)sizeof(dContactGeom));
502 dIASSERT (o1->type == dTerrainZClass);
503 int i,j;
504
505 if ((flags & 0xffff) == 0)
506 flags = (flags & 0xffff0000) | 1;
507
508 int numMaxTerrainContacts = (flags & 0xffff);
509 dxTerrainZ *terrain = (dxTerrainZ*) o1;
510
511 dReal aabbbak[6];
512 int gflagsbak;
513
514 dVector3 pos0;
515 int numTerrainContacts = 0;
516
517 dxPosR *bak;
518 dxPosR X1;
519
520 if (terrain->gflags & GEOM_PLACEABLE)
521 {
522 dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
523 dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
524 dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
525 bak = o2->final_posr;
526 o2->final_posr = &X1;
527 memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
528 gflagsbak = o2->gflags;
529 o2->computeAABB();
530 }
531
532 int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
533 int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
534 int nMinY = int(floor(o2->aabb[2] / terrain->m_vNodeLength));
535 int nMaxY = int(floor(o2->aabb[3] / terrain->m_vNodeLength)) + 1;
536
537 if (terrain->m_bFinite)
538 {
539 nMinX = MAX(nMinX,0);
540 nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
541 nMinY = MAX(nMinY,0);
542 nMaxY = MIN(nMaxY,terrain->m_nNumNodesPerSide);
543
544 if ((nMinX >= nMaxX) || (nMinY >= nMaxY))
545 goto dCollideTerrainZExit;
546 }
547
548 dVector3 AabbTop;
549 AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
550 AabbTop[1] = (o2->aabb[2]+o2->aabb[3]) / 2;
551 AabbTop[2] = o2->aabb[5];
552 if (o2->type != dRayClass)
553 {
554 dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[1]) - AabbTop[2];
555 if (AabbTopDepth > 0.f)
556 {
557 contact->depth = AabbTopDepth;
558 dReal MaxDepth = (o2->aabb[5]-o2->aabb[4]) / 2;
559 if (contact->depth > MaxDepth)
560 contact->depth = MaxDepth;
561 contact->g1 = o1;
562 contact->g2 = o2;
563 dOPE(contact->pos,=,AabbTop);
564 contact->normal[0] = 0.f;
565 contact->normal[1] = 0.f;
566 contact->normal[2] = -1.f;
567
568 numTerrainContacts = 1;
569 goto dCollideTerrainZExit;
570 }
571 }
572
573 for (i=nMinX;i<nMaxX;i++)
574 {
575 for (j=nMinY;j<nMaxY;j++)
576 {
577 numTerrainContacts += terrain->dCollideTerrainUnit(
578 i,j,o2,numMaxTerrainContacts - numTerrainContacts,
579 flags,CONTACT(contact,numTerrainContacts*skip),skip );
580 }
581 }
582
583 dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
584
585 for (i=0; i<numTerrainContacts; i++)
586 {
587 CONTACT(contact,i*skip)->g1 = o1;
588 CONTACT(contact,i*skip)->g2 = o2;
589 }
590
591dCollideTerrainZExit:
592
593 if (terrain->gflags & GEOM_PLACEABLE)
594 {
595 o2->final_posr = bak;
596 memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
597 o2->gflags = gflagsbak;
598
599 for (i=0; i<numTerrainContacts; i++)
600 {
601 dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
602 dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
603 dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos);
604
605 dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
606 dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
607 }
608 }
609
610 return numTerrainContacts;
611}
612/*
613void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
614{
615 float A[3],B[3],C[3],D[3];
616 float R[12];
617 float pos[3];
618 if (pR)
619 memcpy(R,pR,sizeof(R));
620 else
621 {
622 memset(R,0,sizeof(R));
623 R[0] = 1.f;
624 R[5] = 1.f;
625 R[10] = 1.f;
626 }
627
628 if (ppos)
629 memcpy(pos,ppos,sizeof(pos));
630 else
631 memset(pos,0,sizeof(pos));
632
633 float vx,vz;
634 vx = vLength * x;
635 vz = vLength * z;
636
637 int i;
638 for (i=0;i<nNumNodesPerSide;i++)
639 {
640 for (int j=0;j<nNumNodesPerSide;j++)
641 {
642 A[0] = i * vNodeLength + vx;
643 A[1] = j * vNodeLength + vz;
644 A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
645 B[0] = (i+1) * vNodeLength + vx;
646 B[1] = j * vNodeLength + vz;
647 B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
648 C[0] = i * vNodeLength + vx;
649 C[1] = (j+1) * vNodeLength + vz;
650 C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
651 D[0] = (i+1) * vNodeLength + vx;
652 D[1] = (j+1) * vNodeLength + vz;
653 D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
654 dsDrawTriangle(pos,R,C,A,B,1);
655 dsDrawTriangle(pos,R,D,C,B,1);
656 }
657 }
658}
659*/
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt b/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt
new file mode 100644
index 0000000..0428846
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt
@@ -0,0 +1,322 @@
1Benoit CHAPEROT 2003-2004 www.jstarlab.com
2Support for terrain and cones, collision and drawing.
3
4Terrains can be with z up (dTerrainZ) or y up (dTerrainY).
5Terrains are defined by a height field.
6Terrains are now placeable.
7Terrains can now be finite or infinite (repeat themselve in the x and (y or z) directions).
8
9Terrains can potentially collide with everything that collides with planes and rays;
10see the switch statement.
11
12Cones currently collides only with terrain and planes and rays.
13Cones, with high radius to height ratios are perfect to simulate vehicle wheels on terrains.
14
15
16
17There was an error in the depths returned by dCollideTerrain.
18Plus now contacts are not sorted according to their depths.
19Contact sorting is now supposed to be done externally.
20Not all dCollide functions seem to sort contacts according to depth.
21Requesting a high number of contacts, sorting them and then considering only the most significant contacts
22is a good way I think to improve stability.
23* Cones Collisions with spheres, boxes, ccylinder and trimesh are now roughly approximated using sphere collisions.
24
25You will need to complete the following operations (with ODE 0.039):
26
27*** add to folder ode\src:
28
29dCone.cpp
30dTerrainY.cpp
31dTerrainZ.cpp
32collision_std_internal.h
33
34On linux => edit each .cpp file and comment out #include "windows.h" & #include "ode\ode.h"
35
36
37*** add to drawstuff\src\drawstuff.cpp:
38
39static void drawCone(float l, float r)
40{
41 int i;
42 float tmp,ny,nz,a,ca,sa;
43 const int n = 24; // number of sides to the cone (divisible by 4)
44
45 a = float(M_PI*2.0)/float(n);
46 sa = (float) sin(a);
47 ca = (float) cos(a);
48
49 // draw top
50 glShadeModel (GL_FLAT);
51 ny=1; nz=0; // normal vector = (0,ny,nz)
52 glBegin (GL_TRIANGLE_FAN);
53 glNormal3d (0,0,1);
54 glVertex3d (0,0,l);
55 for (i=0; i<=n; i++) {
56 if (i==1 || i==n/2+1)
57 setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
58 glNormal3d (ny*r,nz*r,0);
59 glVertex3d (ny*r,nz*r,0);
60 if (i==1 || i==n/2+1)
61 setColor (color[0],color[1],color[2],color[3]);
62
63 // rotate ny,nz
64 tmp = ca*ny - sa*nz;
65 nz = sa*ny + ca*nz;
66 ny = tmp;
67 }
68 glEnd();
69
70 // draw bottom
71 ny=1; nz=0; // normal vector = (0,ny,nz)
72 glBegin (GL_TRIANGLE_FAN);
73 glNormal3d (0,0,-1);
74 glVertex3d (0,0,0);
75 for (i=0; i<=n; i++) {
76 if (i==1 || i==n/2+1)
77 setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
78 glNormal3d (0,0,-1);
79 glVertex3d (ny*r,nz*r,0);
80 if (i==1 || i==n/2+1)
81 setColor (color[0],color[1],color[2],color[3]);
82
83 // rotate ny,nz
84 tmp = ca*ny + sa*nz;
85 nz = -sa*ny + ca*nz;
86 ny = tmp;
87 }
88 glEnd();
89}
90
91void dsDrawCone (const float pos[3], const float R[12], float length, float radius)
92{
93 if (current_state != 2) dsError ("drawing function called outside simulation loop");
94 setupDrawingMode();
95 glShadeModel (GL_SMOOTH);
96 setTransform (pos,R);
97 drawCone (length,radius);
98 glPopMatrix();
99
100 if (use_shadows) {
101 setShadowDrawingMode();
102 setShadowTransform();
103 setTransform (pos,R);
104 drawCone (length,radius);
105 glPopMatrix();
106 glPopMatrix();
107 glDepthRange (0,1);
108 }
109}
110
111void dsDrawConeD (const double pos[3], const double R[12], float length, float radius)
112{
113 int i;
114 float pos2[3],R2[12];
115 for (i=0; i<3; i++) pos2[i]=(float)pos[i];
116 for (i=0; i<12; i++) R2[i]=(float)R[i];
117 dsDrawCone(pos2,R2,length,radius);
118}
119
120static float GetHeight(int x,int y,int nNumNodesPerSide,float *pHeights)
121{
122 int nNumNodesPerSideMask = nNumNodesPerSide - 1;
123 return pHeights[ (((unsigned int)(y) & nNumNodesPerSideMask) * nNumNodesPerSide)
124 + ((unsigned int)(x) & nNumNodesPerSideMask)];
125}
126
127void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
128{
129 float A[3],B[3],C[3],D[3];
130 float R[12];
131 float pos[3];
132 if (pR)
133 memcpy(R,pR,sizeof(R));
134 else
135 {
136 memset(R,0,sizeof(R));
137 R[0] = 1.f;
138 R[5] = 1.f;
139 R[10] = 1.f;
140 }
141
142 if (ppos)
143 memcpy(pos,ppos,sizeof(pos));
144 else
145 memset(pos,0,sizeof(pos));
146
147 float vx,vz;
148 vx = vLength * x;
149 vz = vLength * z;
150
151 int i;
152 for (i=0;i<nNumNodesPerSide;i++)
153 {
154 for (int j=0;j<nNumNodesPerSide;j++)
155 {
156 A[0] = i * vNodeLength + vx;
157 A[2] = j * vNodeLength + vz;
158 A[1] = GetHeight(i,j,nNumNodesPerSide,pHeights);
159 B[0] = (i+1) * vNodeLength + vx;
160 B[2] = j * vNodeLength + vz;
161 B[1] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
162 C[0] = i * vNodeLength + vx;
163 C[2] = (j+1) * vNodeLength + vz;
164 C[1] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
165 D[0] = (i+1) * vNodeLength + vx;
166 D[2] = (j+1) * vNodeLength + vz;
167 D[1] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
168 dsDrawTriangle(pos,R,C,B,A,1);
169 dsDrawTriangle(pos,R,D,B,C,1);
170 }
171 }
172}
173
174void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
175{
176 float A[3],B[3],C[3],D[3];
177 float R[12];
178 float pos[3];
179 if (pR)
180 memcpy(R,pR,sizeof(R));
181 else
182 {
183 memset(R,0,sizeof(R));
184 R[0] = 1.f;
185 R[5] = 1.f;
186 R[10] = 1.f;
187 }
188
189 if (ppos)
190 memcpy(pos,ppos,sizeof(pos));
191 else
192 memset(pos,0,sizeof(pos));
193
194 float vx,vz;
195 vx = vLength * x;
196 vz = vLength * z;
197
198 int i;
199 for (i=0;i<nNumNodesPerSide;i++)
200 {
201 for (int j=0;j<nNumNodesPerSide;j++)
202 {
203 A[0] = i * vNodeLength + vx;
204 A[1] = j * vNodeLength + vz;
205 A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
206 B[0] = (i+1) * vNodeLength + vx;
207 B[1] = j * vNodeLength + vz;
208 B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
209 C[0] = i * vNodeLength + vx;
210 C[1] = (j+1) * vNodeLength + vz;
211 C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
212 D[0] = (i+1) * vNodeLength + vx;
213 D[1] = (j+1) * vNodeLength + vz;
214 D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
215 dsDrawTriangle(pos,R,C,A,B,1);
216 dsDrawTriangle(pos,R,D,C,B,1);
217 }
218 }
219}
220
221*** add to include\drawstuff\drawstuff.h:
222void dsDrawCone (const float pos[3], const float R[12], float length, float radius);
223void dsDrawConeD (const double pos[3], const double R[12], float length, float radius);
224void dsDrawTerrainY(int x,int y,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos);
225void dsDrawTerrainZ(int x,int y,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos);
226
227*** add in include\ode\collision.h line 77:
228/* class numbers - each geometry object needs a unique number */
229enum {
230 dSphereClass = 0,
231 dBoxClass,
232 dCCylinderClass,
233 dCylinderClass,
234 dPlaneClass,
235 dRayClass,
236 dGeomTransformClass,
237 dTriMeshClass,
238
239 dTerrainYClass, //here
240 dTerrainZClass, //here
241 dConeClass, //here
242
243 dFirstSpaceClass,
244 dSimpleSpaceClass = dFirstSpaceClass,
245 dHashSpaceClass,
246 dQuadTreeSpaceClass,
247
248 dLastSpaceClass = dQuadTreeSpaceClass,
249
250 dFirstUserClass,
251 dLastUserClass = dFirstUserClass + dMaxUserClasses - 1,
252 dGeomNumClasses
253};
254
255dGeomID dCreateTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
256dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z);
257dGeomID dCreateTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
258dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z);
259
260dGeomID dCreateCone(dSpaceID space, dReal radius, dReal length);
261void dGeomConeSetParams (dGeomID cone, dReal radius, dReal length);
262void dGeomConeGetParams (dGeomID cone, dReal *radius, dReal *length);
263dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z);
264
265*** add in include\ode\odemath.h:
266#define dOP(a,op,b,c) \
267 (a)[0] = ((b)[0]) op ((c)[0]); \
268 (a)[1] = ((b)[1]) op ((c)[1]); \
269 (a)[2] = ((b)[2]) op ((c)[2]);
270#define dOPC(a,op,b,c) \
271 (a)[0] = ((b)[0]) op (c); \
272 (a)[1] = ((b)[1]) op (c); \
273 (a)[2] = ((b)[2]) op (c);
274#define dOPE(a,op,b) \
275 (a)[0] op ((b)[0]); \
276 (a)[1] op ((b)[1]); \
277 (a)[2] op ((b)[2]);
278#define dOPEC(a,op,c) \
279 (a)[0] op (c); \
280 (a)[1] op (c); \
281 (a)[2] op (c);
282#define dLENGTH(a) \
283 (dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) ));
284#define dLENGTHSQUARED(a) \
285 (((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]));
286
287*** add in ode\src\collision_kernel.cpp function 'static void initColliders()' next to other 'setCollider' calls:
288 setCollider (dTerrainYClass,dSphereClass,&dCollideTerrainY);
289 setCollider (dTerrainYClass,dBoxClass,&dCollideTerrainY);
290 setCollider (dTerrainYClass,dCCylinderClass,&dCollideTerrainY);
291 setCollider (dTerrainYClass,dRayClass,&dCollideTerrainY);
292 setCollider (dTerrainYClass,dConeClass,&dCollideTerrainY);
293
294 setCollider (dTerrainZClass,dSphereClass,&dCollideTerrainZ);
295 setCollider (dTerrainZClass,dBoxClass,&dCollideTerrainZ);
296 setCollider (dTerrainZClass,dCCylinderClass,&dCollideTerrainZ);
297 setCollider (dTerrainZClass,dRayClass,&dCollideTerrainZ);
298 setCollider (dTerrainZClass,dConeClass,&dCollideTerrainZ);
299
300 setCollider (dRayClass,dConeClass,&dCollideRayCone);
301 setCollider (dConeClass,dPlaneClass,&dCollideConePlane);
302 setCollider (dConeClass,dSphereClass,&dCollideConeSphere);
303 setCollider (dConeClass,dBoxClass,&dCollideConeBox);
304 setCollider (dCCylinderClass,dConeClass,&dCollideCCylinderCone);
305 setCollider (dTriMeshClass,dConeClass,&dCollideTriMeshCone);
306
307*** add in ode\src\collision_std.h:
308int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
309int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
310
311int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
312int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
313int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
314int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
315int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
316int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
317
318*** add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the ODE_SRC variable in the makefile
319On Linux => add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the libode_a_SOURCES variable in the ode/src/Makefile.am file.
320
321*** now you can now test using file test_boxstackb.cpp (to add in folder ode\test).
322
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp
new file mode 100644
index 0000000..f1fa592
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp
@@ -0,0 +1,1375 @@
1/*************************************************************************
2
3
4* *
5
6
7* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
8
9
10* All rights reserved. Email: russ@q12.org Web: www.q12.org *
11
12
13* *
14
15
16* This library is free software; you can redistribute it and/or *
17
18
19* modify it under the terms of EITHER: *
20
21
22* (1) The GNU Lesser General Public License as published by the Free *
23
24
25* Software Foundation; either version 2.1 of the License, or (at *
26
27
28* your option) any later version. The text of the GNU Lesser *
29
30
31* General Public License is included with this library in the *
32
33
34* file LICENSE.TXT. *
35
36
37* (2) The BSD-style license that is included with this library in *
38
39
40* the file LICENSE-BSD.TXT. *
41
42
43* *
44
45
46* This library is distributed in the hope that it will be useful, *
47
48
49* but WITHOUT ANY WARRANTY; without even the implied warranty of *
50
51
52* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
53
54
55* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
56
57
58* *
59
60
61*************************************************************************/
62
63
64
65
66
67#include <ode/ode.h>
68
69
70#include <drawstuff/drawstuff.h>
71
72
73
74
75
76#ifdef _MSC_VER
77
78
79#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
80
81
82#endif
83
84
85
86
87
88// select correct drawing functions
89
90
91
92
93
94#ifdef dDOUBLE
95
96
97#define dsDrawBox dsDrawBoxD
98
99
100#define dsDrawSphere dsDrawSphereD
101
102
103#define dsDrawCylinder dsDrawCylinderD
104
105
106#define dsDrawCappedCylinder dsDrawCappedCylinderD
107
108
109#endif
110
111
112
113
114
115
116
117
118// some constants
119
120
121
122
123
124const dReal vTerrainLength = 4.f;
125
126
127const dReal vTerrainHeight = 0.5f;
128
129
130const int TERRAINNODES = 4;
131
132
133dReal pTerrainHeights[TERRAINNODES*TERRAINNODES];
134
135
136
137
138
139dGeomID terrainZ = NULL;
140
141
142dGeomID terrainY = NULL;
143
144
145
146
147
148#define NUM 20 // max number of objects
149
150
151#define DENSITY (5.0) // density of all objects
152
153
154#define GPB 3 // maximum number of geometries per body
155
156
157#define MAX_CONTACTS 4 // maximum number of contact points per body
158
159
160
161
162
163
164
165
166// dynamics and collision objects
167
168
169
170
171
172struct MyObject {
173
174
175 dBodyID body; // the body
176
177
178 dGeomID geom[GPB]; // geometries representing this body
179
180
181};
182
183
184
185
186
187static int num=0; // number of objects in simulation
188
189
190static int nextobj=0; // next object to recycle if num==NUM
191
192
193static dWorldID world;
194
195
196static dSpaceID space;
197
198
199static MyObject obj[NUM];
200
201
202static dJointGroupID contactgroup;
203
204
205static int selected = -1; // selected object
206
207
208static int show_aabb = 0; // show geom AABBs?
209
210
211static int show_contacts = 0; // show contact points?
212
213
214static int random_pos = 1; // drop objects from random position?
215
216
217
218
219
220
221
222
223// this is called by dSpaceCollide when two objects in space are
224
225
226// potentially colliding.
227
228
229
230
231
232static void nearCallback (void *data, dGeomID o1, dGeomID o2)
233
234
235{
236
237
238 int i;
239
240
241 // if (o1->body && o2->body) return;
242
243
244
245
246
247 // exit without doing anything if the two bodies are connected by a joint
248
249
250 dBodyID b1 = dGeomGetBody(o1);
251
252
253 dBodyID b2 = dGeomGetBody(o2);
254
255
256 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
257
258
259
260
261
262 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
263
264
265 for (i=0; i<MAX_CONTACTS; i++) {
266
267
268 contact[i].surface.mode = dContactBounce | dContactApprox1; //dContactSoftCFM;
269
270
271 contact[i].surface.mu = dInfinity;
272
273
274 contact[i].surface.mu2 = 0;
275
276
277 contact[i].surface.bounce = 0.1;
278
279
280 contact[i].surface.bounce_vel = 0.1;
281
282
283 contact[i].surface.soft_cfm = 0.01;
284
285
286 }
287
288
289 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
290
291
292 sizeof(dContact))) {
293
294
295 dMatrix3 RI;
296
297
298 dRSetIdentity (RI);
299
300
301 const dReal ss[3] = {0.02,0.02,0.02};
302
303
304 for (i=0; i<numc; i++) {
305
306
307 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
308
309
310 dJointAttach (c,b1,b2);
311
312
313 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
314
315
316 }
317
318
319 }
320
321
322}
323
324
325
326
327
328
329
330
331// start simulation - set viewpoint
332
333
334
335
336
337static void start()
338
339
340{
341
342
343 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
344
345
346 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
347
348
349 dsSetViewpoint (xyz,hpr);
350
351
352 printf ("To drop another object, press:\n");
353
354
355 printf (" b for box.\n");
356
357
358 printf (" s for sphere.\n");
359
360
361 printf (" c for cylinder.\n");
362
363
364 printf (" x for a composite object.\n");
365
366
367 printf ("To select an object, press space.\n");
368
369
370 printf ("To disable the selected object, press d.\n");
371
372
373 printf ("To enable the selected object, press e.\n");
374
375
376 printf ("To toggle showing the geom AABBs, press a.\n");
377
378
379 printf ("To toggle showing the contact points, press t.\n");
380
381
382 printf ("To toggle dropping from random position/orientation, press r.\n");
383
384
385}
386
387
388
389
390
391
392
393
394char locase (char c)
395
396
397{
398
399
400 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
401
402
403 else return c;
404
405
406}
407
408
409
410
411
412
413
414
415// called when a key pressed
416
417
418
419
420
421static void command (int cmd)
422
423
424{
425
426
427 int i,j,k;
428
429
430 dReal sides[3];
431
432
433 dMass m;
434
435
436
437
438
439 cmd = locase (cmd);
440
441
442 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
443
444
445 /* || cmd == 'l' */) {
446
447
448 if (num < NUM) {
449
450
451 i = num;
452
453
454 num++;
455
456
457 }
458
459
460 else {
461
462
463 i = nextobj;
464
465
466 nextobj++;
467
468
469 if (nextobj >= num) nextobj = 0;
470
471
472
473
474
475 // destroy the body and geoms for slot i
476
477
478 dBodyDestroy (obj[i].body);
479
480
481 for (k=0; k < GPB; k++) {
482
483
484 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
485
486
487 }
488
489
490 memset (&obj[i],0,sizeof(obj[i]));
491
492
493 }
494
495
496
497
498
499 obj[i].body = dBodyCreate (world);
500
501
502 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
503
504
505
506
507
508 dMatrix3 R;
509
510
511 if (random_pos) {
512
513
514 dBodySetPosition (obj[i].body,
515
516
517 dRandReal()*2-1,dRandReal()*2+1,dRandReal()+3);
518
519
520 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
521
522
523 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
524
525
526 }
527
528
529 else {
530
531
532 dReal maxheight = 0;
533
534
535 for (k=0; k<num; k++) {
536
537
538 const dReal *pos = dBodyGetPosition (obj[k].body);
539
540
541 if (pos[2] > maxheight) maxheight = pos[2];
542
543
544 }
545
546
547 dBodySetPosition (obj[i].body, 0,maxheight+1,maxheight+3);
548
549
550 dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
551
552
553 }
554
555
556 dBodySetRotation (obj[i].body,R);
557
558
559 dBodySetData (obj[i].body,(void*) i);
560
561
562
563
564
565 if (cmd == 'b') {
566
567
568 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
569
570
571 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
572
573
574 }
575
576
577 else if (cmd == 'c') {
578
579
580 sides[0] *= 0.5;
581
582
583 dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
584
585
586 obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
587
588
589 }
590
591
592 /*
593
594
595 // cylinder option not yet implemented
596
597
598 else if (cmd == 'l') {
599
600
601 sides[1] *= 0.5;
602
603
604 dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
605
606
607 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
608
609
610 }
611
612
613 */
614
615
616 else if (cmd == 's') {
617
618
619 sides[0] *= 0.5;
620
621
622 dMassSetSphere (&m,DENSITY,sides[0]);
623
624
625 obj[i].geom[0] = dCreateSphere (space,sides[0]);
626
627
628 }
629
630
631 else if (cmd == 'x') {
632
633
634 dGeomID g2[GPB]; // encapsulated geometries
635
636
637 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
638
639
640
641
642
643 // start accumulating masses for the encapsulated geometries
644
645
646 dMass m2;
647
648
649 dMassSetZero (&m);
650
651
652
653
654
655 // set random delta positions
656
657
658 for (j=0; j<GPB; j++) {
659
660
661 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
662
663
664 }
665
666
667
668
669
670 for (k=0; k<GPB; k++) {
671
672
673 obj[i].geom[k] = dCreateGeomTransform (space);
674
675
676 dGeomTransformSetCleanup (obj[i].geom[k],1);
677
678
679 if (k==0) {
680
681
682 dReal radius = dRandReal()*0.25+0.05;
683
684
685 g2[k] = dCreateSphere (0,radius);
686
687
688 dMassSetSphere (&m2,DENSITY,radius);
689
690
691 }
692
693
694 else if (k==1) {
695
696
697 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
698
699
700 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
701
702
703 }
704
705
706 else {
707
708
709 dReal radius = dRandReal()*0.1+0.05;
710
711
712 dReal length = dRandReal()*1.0+0.1;
713
714
715 g2[k] = dCreateCCylinder (0,radius,length);
716
717
718 dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
719
720
721 }
722
723
724 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
725
726
727
728
729
730 // set the transformation (adjust the mass too)
731
732
733 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
734
735
736 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
737
738
739 dMatrix3 Rtx;
740
741
742 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
743
744
745 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
746
747
748 dGeomSetRotation (g2[k],Rtx);
749
750
751 dMassRotate (&m2,Rtx);
752
753
754
755
756
757 // add to the total mass
758
759
760 dMassAdd (&m,&m2);
761
762
763 }
764
765
766
767
768
769 // move all encapsulated objects so that the center of mass is (0,0,0)
770
771
772 for (k=0; k<2; k++) {
773
774
775 dGeomSetPosition (g2[k],
776
777
778 dpos[k][0]-m.c[0],
779
780
781 dpos[k][1]-m.c[1],
782
783
784 dpos[k][2]-m.c[2]);
785
786
787 }
788
789
790 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
791
792
793 }
794
795
796
797
798
799 for (k=0; k < GPB; k++) {
800
801
802 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
803
804
805 }
806
807
808
809
810
811 dBodySetMass (obj[i].body,&m);
812
813
814 }
815
816
817
818
819
820 if (cmd == ' ') {
821
822
823 selected++;
824
825
826 if (selected >= num) selected = 0;
827
828
829 if (selected < 0) selected = 0;
830
831
832 }
833
834
835 else if (cmd == 'd' && selected >= 0 && selected < num) {
836
837
838 dBodyDisable (obj[selected].body);
839
840
841 }
842
843
844 else if (cmd == 'e' && selected >= 0 && selected < num) {
845
846
847 dBodyEnable (obj[selected].body);
848
849
850 }
851
852
853 else if (cmd == 'a') {
854
855
856 show_aabb ^= 1;
857
858
859 }
860
861
862 else if (cmd == 't') {
863
864
865 show_contacts ^= 1;
866
867
868 }
869
870
871 else if (cmd == 'r') {
872
873
874 random_pos ^= 1;
875
876
877 }
878
879
880}
881
882
883
884
885
886
887
888
889// draw a geom
890
891
892
893
894
895void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
896
897
898{
899
900
901 int i;
902
903
904
905
906
907 if (!g) return;
908
909
910 if (!pos) pos = dGeomGetPosition (g);
911
912
913 if (!R) R = dGeomGetRotation (g);
914
915
916
917
918
919 int type = dGeomGetClass (g);
920
921
922 if (type == dBoxClass) {
923
924
925 dVector3 sides;
926
927
928 dGeomBoxGetLengths (g,sides);
929
930
931 dsDrawBox (pos,R,sides);
932
933
934 }
935
936
937 else if (type == dSphereClass) {
938
939
940 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
941
942
943 }
944
945
946 else if (type == dCCylinderClass) {
947
948
949 dReal radius,length;
950
951
952 dGeomCCylinderGetParams (g,&radius,&length);
953
954
955 dsDrawCappedCylinder (pos,R,length,radius);
956
957
958 }
959
960
961 /*
962
963
964 // cylinder option not yet implemented
965
966
967 else if (type == dCylinderClass) {
968
969
970 dReal radius,length;
971
972
973 dGeomCylinderGetParams (g,&radius,&length);
974
975
976 dsDrawCylinder (pos,R,length,radius);
977
978
979 }
980
981
982 */
983
984
985 else if (type == dGeomTransformClass) {
986
987
988 dGeomID g2 = dGeomTransformGetGeom (g);
989
990
991 const dReal *pos2 = dGeomGetPosition (g2);
992
993
994 const dReal *R2 = dGeomGetRotation (g2);
995
996
997 dVector3 actual_pos;
998
999
1000 dMatrix3 actual_R;
1001
1002
1003 dMULTIPLY0_331 (actual_pos,R,pos2);
1004
1005
1006 actual_pos[0] += pos[0];
1007
1008
1009 actual_pos[1] += pos[1];
1010
1011
1012 actual_pos[2] += pos[2];
1013
1014
1015 dMULTIPLY0_333 (actual_R,R,R2);
1016
1017
1018 drawGeom (g2,actual_pos,actual_R,0);
1019
1020
1021 }
1022
1023
1024
1025
1026
1027 if (show_aabb) {
1028
1029
1030 // draw the bounding box for this geom
1031
1032
1033 dReal aabb[6];
1034
1035
1036 dGeomGetAABB (g,aabb);
1037
1038
1039 dVector3 bbpos;
1040
1041
1042 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
1043
1044
1045 dVector3 bbsides;
1046
1047
1048 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
1049
1050
1051 dMatrix3 RI;
1052
1053
1054 dRSetIdentity (RI);
1055
1056
1057 dsSetColorAlpha (1,0,0,0.5);
1058
1059
1060 dsDrawBox (bbpos,RI,bbsides);
1061
1062
1063 }
1064
1065
1066}
1067
1068
1069
1070
1071
1072
1073
1074
1075// simulation loop
1076
1077
1078
1079
1080
1081static void simLoop (int pause)
1082
1083
1084{
1085
1086
1087 dsSetColor (0,0,2);
1088
1089
1090 dSpaceCollide (space,0,&nearCallback);
1091
1092
1093 if (!pause) dWorldStep (world,0.05);
1094
1095
1096
1097
1098
1099 dAASSERT(terrainY);
1100
1101
1102 dAASSERT(terrainZ);
1103
1104
1105 dsSetColor (0,1,0);
1106
1107
1108 dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));
1109
1110
1111 dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));
1112
1113
1114
1115
1116
1117 if (show_aabb)
1118
1119
1120 {
1121
1122
1123 dReal aabb[6];
1124
1125
1126 dGeomGetAABB (terrainY,aabb);
1127
1128
1129 dVector3 bbpos;
1130
1131
1132 int i;
1133
1134
1135 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
1136
1137
1138 dVector3 bbsides;
1139
1140
1141 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
1142
1143
1144 dMatrix3 RI;
1145
1146
1147 dRSetIdentity (RI);
1148
1149
1150 dsSetColorAlpha (1,0,0,0.5);
1151
1152
1153 dsDrawBox (bbpos,RI,bbsides);
1154
1155
1156
1157
1158
1159 dGeomGetAABB (terrainZ,aabb);
1160
1161
1162 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
1163
1164
1165 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
1166
1167
1168 dsDrawBox (bbpos,RI,bbsides);
1169
1170
1171 }
1172
1173
1174
1175
1176
1177 dsSetColor (1,1,0);
1178
1179
1180
1181
1182
1183 // remove all contact joints
1184
1185
1186 dJointGroupEmpty (contactgroup);
1187
1188
1189
1190
1191
1192 dsSetColor (1,1,0);
1193
1194
1195 dsSetTexture (DS_WOOD);
1196
1197
1198 for (int i=0; i<num; i++) {
1199
1200
1201 for (int j=0; j < GPB; j++) {
1202
1203
1204 if (i==selected) {
1205
1206
1207 dsSetColor (0,0.7,1);
1208
1209
1210 }
1211
1212
1213 else if (! dBodyIsEnabled (obj[i].body)) {
1214
1215
1216 dsSetColor (1,0,0);
1217
1218
1219 }
1220
1221
1222 else {
1223
1224
1225 dsSetColor (1,1,0);
1226
1227
1228 }
1229
1230
1231 drawGeom (obj[i].geom[j],0,0,show_aabb);
1232
1233
1234 }
1235
1236
1237 }
1238
1239
1240}
1241
1242
1243
1244
1245
1246
1247
1248
1249int main (int argc, char **argv)
1250
1251
1252{
1253
1254
1255 // setup pointers to drawstuff callback functions
1256
1257
1258 dsFunctions fn;
1259
1260
1261 fn.version = DS_VERSION;
1262
1263
1264 fn.start = &start;
1265
1266
1267 fn.step = &simLoop;
1268
1269
1270 fn.command = &command;
1271
1272
1273 fn.stop = 0;
1274
1275
1276 fn.path_to_textures = "../../drawstuff/textures";
1277
1278
1279 if(argc==2)
1280 {
1281 fn.path_to_textures = argv[1];
1282 }
1283
1284
1285
1286 // create world
1287
1288
1289
1290
1291
1292 world = dWorldCreate();
1293
1294
1295 space = dHashSpaceCreate (0);
1296
1297
1298 contactgroup = dJointGroupCreate (0);
1299
1300
1301 dWorldSetGravity (world,0,0,-0.5); //-0.5
1302
1303
1304 dWorldSetCFM (world,1e-5);
1305
1306
1307 dCreatePlane (space,0,0,1,0);
1308
1309
1310 memset (obj,0,sizeof(obj));
1311
1312
1313
1314
1315
1316 for (int i=0;i<TERRAINNODES*TERRAINNODES;i++) pTerrainHeights[i] = vTerrainHeight * dRandReal();
1317
1318
1319 terrainY = dCreateTerrainY(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
1320
1321
1322 terrainZ = dCreateTerrainZ(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
1323
1324
1325
1326
1327
1328 dMatrix3 R;
1329
1330
1331 dRFromZAxis(R, 0.2f, 0.2f, 0.2f);
1332
1333
1334 dGeomSetPosition(terrainY,0.f,0.f,0.5f);
1335
1336
1337 dGeomSetRotation(terrainY,R);
1338
1339
1340 dGeomSetPosition(terrainZ,0.f,0.f,0.5f);
1341
1342
1343 dGeomSetRotation(terrainZ,R);
1344
1345
1346
1347
1348
1349 // run simulation
1350
1351
1352 dsSimulationLoop (argc,argv,352,288,&fn);
1353
1354
1355
1356
1357
1358 dJointGroupDestroy (contactgroup);
1359
1360
1361 dSpaceDestroy (space);
1362
1363
1364 dWorldDestroy (world);
1365
1366
1367
1368
1369
1370 return 0;
1371
1372
1373}
1374
1375
generated by cgit v1.1 at 2024-11-08 10:04:28 +0000