Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/contrib/TerrainAndCone/dCone.cpp @ 254

Last change on this file since 254 was 216, checked in by mathiask, 18 years ago

[Physik] add ode-0.9
File size: 12.7 KB
RevLine 
[216]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
Note: See TracBrowser for help on using the repository browser.