aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:15:33 +0000
committerdan miller2007-10-19 05:15:33 +0000
commit79eca25c945a535a7a0325999034bae17da92412 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
parentadding ode source to /libraries (diff)
downloadopensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.zip
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.gz
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.bz2
opensim-SC_OLD-79eca25c945a535a7a0325999034bae17da92412.tar.xz
resubmitting ode
Diffstat (limited to 'libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp')
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp504
1 files changed, 504 insertions, 0 deletions
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
generated by cgit v1.1 at 2024-11-15 06:58:17 +0000