From f205de7847da7ae1c10212d82e7042d0100b4ce0 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:24:38 +0000 Subject: from the start... checking in ode-0.9 --- .../TerrainAndCone/collision_std_internal.h | 100 ++ libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp | 504 +++++++ .../ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp | 662 ++++++++++ .../ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp | 659 ++++++++++ .../ode-0.9/contrib/TerrainAndCone/readme.txt | 322 +++++ .../contrib/TerrainAndCone/test_boxstackb.cpp | 1375 ++++++++++++++++++++ 6 files changed, 3622 insertions(+) create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/readme.txt create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp (limited to 'libraries/ode-0.9/contrib/TerrainAndCone') 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 @@ +//Benoit CHAPEROT 2003-2004 www.jstarlab.com +#ifndef _ODE_COLLISION_STD_INTERNAL_H_ +#define _ODE_COLLISION_STD_INTERNAL_H_ + +#include +#include "collision_kernel.h" + +struct dxSphere : public dxGeom { + dReal radius; // sphere radius + dxSphere (dSpaceID space, dReal _radius); + void computeAABB(); +}; + + +struct dxBox : public dxGeom { + dVector3 side; // side lengths (x,y,z) + dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz); + void computeAABB(); +}; + + +struct dxCCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + + +struct dxPlane : public dxGeom { + dReal p[4]; + dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d); + void computeAABB(); +}; + +struct dxCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + +struct dxCone : public dxGeom { + dReal radius,lz; + dxCone(dSpaceID space, dReal _radius,dReal _length); + ~dxCone(); + void computeAABB(); +}; + +struct dxRay : public dxGeom { + dReal length; + dxRay (dSpaceID space, dReal _length); + void computeAABB(); +}; + +struct dxTerrainY : public dxGeom { + dReal m_vLength; + dReal *m_pHeights; + dReal m_vMinHeight; + dReal m_vMaxHeight; + dReal m_vNodeLength; + int m_nNumNodesPerSide; + int m_nNumNodesPerSideShift; + int m_nNumNodesPerSideMask; + int m_bFinite; + dxTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable); + ~dxTerrainY(); + void computeAABB(); + dReal GetHeight(dReal x,dReal z); + dReal GetHeight(int x,int z); + int dCollideTerrainUnit(int x,int z,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip); + bool IsOnTerrain(int nx,int nz,int w,dReal *pos); +}; + +struct dxTerrainZ : public dxGeom { + dReal m_vLength; + dReal *m_pHeights; + dReal m_vMinHeight; + dReal m_vMaxHeight; + dReal m_vNodeLength; + int m_nNumNodesPerSide; + int m_nNumNodesPerSideShift; + int m_nNumNodesPerSideMask; + int m_bFinite; + dxTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable); + ~dxTerrainZ(); + void computeAABB(); + dReal GetHeight(dReal x,dReal y); + dReal GetHeight(int x,int y); + int dCollideTerrainUnit(int x,int y,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip); + bool IsOnTerrain(int nx,int ny,int w,dReal *pos); +}; + +#ifndef MIN +#define MIN(a,b) ((ab)?a:b) +#endif + +#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 @@ +//Benoit CHAPEROT 2003-2004 www.jstarlab.com +//some code inspired by Magic Software +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_std_internal.h" +#include "collision_util.h" +#include +#include "windows.h" +#include "ode\ode.h" + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) +const dReal fEPSILON = 1e-9f; + +dxCone::dxCone (dSpaceID space, dReal _radius,dReal _length) : +dxGeom (space,1) +{ + dAASSERT(_radius > 0.f); + dAASSERT(_length > 0.f); + type = dConeClass; + radius = _radius; + lz = _length; +} + +dxCone::~dxCone() +{ +} + +void dxCone::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal xrange = dFabs(R[2] * lz) + radius; + dReal yrange = dFabs(R[6] * lz) + radius; + dReal zrange = dFabs(R[10] * lz) + radius; + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + +dGeomID dCreateCone(dSpaceID space, dReal _radius,dReal _length) +{ + return new dxCone(space,_radius,_length); +} + +void dGeomConeSetParams (dGeomID g, dReal _radius, dReal _length) +{ + dUASSERT (g && g->type == dConeClass,"argument not a cone"); + dAASSERT (_radius > 0.f); + dAASSERT (_length > 0.f); + g->recomputePosr(); + dxCone *c = (dxCone*) g; + c->radius = _radius; + c->lz = _length; + dGeomMoved (g); +} + + +void dGeomConeGetParams (dGeomID g, dReal *_radius, dReal *_length) +{ + dUASSERT (g && g->type == dConeClass,"argument not a cone"); + g->recomputePosr(); + dxCone *c = (dxCone*) g; + *_radius = c->radius; + *_length = c->lz; +} + +//positive inside +dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dConeClass,"argument not a cone"); + + g->recomputePosr(); + dxCone *cone = (dxCone*) g; + + dVector3 tmp,q; + tmp[0] = x - cone->final_posr->pos[0]; + tmp[1] = y - cone->final_posr->pos[1]; + tmp[2] = z - cone->final_posr->pos[2]; + dMULTIPLY1_331 (q,cone->final_posr->R,tmp); + + dReal r = cone->radius; + dReal h = cone->lz; + + dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]); + dReal d1 = q[2]; + dReal d2 = h-q[2]; + + if (d0 < d1) { + if (d0 < d2) return d0; else return d2; + } + else { + if (d1 < d2) return d1; else return d2; + } +} + +//plane plane +bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4], + dVector3 LinePos,dVector3 LineDir) +{ + // If Cross(N0,N1) is zero, then either planes are parallel and separated + // or the same plane. In both cases, 'false' is returned. Otherwise, + // the intersection line is + // + // L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1 + // + // for some coefficients c0 and c1 and for t any real number (the line + // parameter). Taking dot products with the normals, + // + // d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1) + // d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1) + // + // which are two equations in two unknowns. The solution is + // + // c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det + // c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det + // + // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2. +/* + Real fN00 = rkPlane0.Normal().SquaredLength(); + Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal()); + Real fN11 = rkPlane1.Normal().SquaredLength(); + Real fDet = fN00*fN11 - fN01*fN01; + + if ( Math::FAbs(fDet) < gs_fEpsilon ) + return false; + + Real fInvDet = 1.0f/fDet; + Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet; + Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet; + + rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal()); + rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal(); + return true; +*/ + dReal fN00 = dLENGTHSQUARED(Plane0); + dReal fN01 = dDOT(Plane0,Plane1); + dReal fN11 = dLENGTHSQUARED(Plane1); + dReal fDet = fN00*fN11 - fN01*fN01; + + if ( fabs(fDet) < fEPSILON) + return false; + + dReal fInvDet = 1.0f/fDet; + dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet; + dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet; + + dCROSS(LineDir,=,Plane0,Plane1); + dNormalize3(LineDir); + + dVector3 Temp0,Temp1; + dOPC(Temp0,*,Plane0,fC0); + dOPC(Temp1,*,Plane1,fC1); + dOP(LinePos,+,Temp0,Temp1); + + return true; +} + +//plane ray +bool FindIntersectionPlaneRay(const dReal Plane[4], + const dVector3 &LinePos,const dVector3 &LineDir, + dReal &u,dVector3 &Pos) +{ +/* + u = (A*X1 + B*Y1 + C*Z1 + D) / (A*(X1-X2) + B*(Y1-Y2)+C*(Z1-Z2)) +*/ + dReal fDet = -dDot(Plane,LineDir,3); + + if ( fabs(fDet) < fEPSILON) + return false; + + u = (dDot(Plane,LinePos,3) - Plane[3]) / fDet; + dOPC(Pos,*,LineDir,u); + dOPE(Pos,+=,LinePos); + + return true; +} + +int SolveQuadraticPolynomial(dReal a,dReal b,dReal c,dReal &x0,dReal &x1) +{ + dReal d = b*b - 4*a*c; + int NumRoots = 0; + dReal dr; + + if (d < 0.f) + return NumRoots; + + if (d == 0.f) + { + NumRoots = 1; + dr = 0.f; + } + else + { + NumRoots = 2; + dr = sqrtf(d); + } + + x0 = (-b -dr) / (2.f * a); + x1 = (-b +dr) / (2.f * a); + + return NumRoots; +} +/* +const int VALID_INTERSECTION = 1<<0; +const int POS_TEST_FAILEDT0 = 1<<0; +const int POS_TEST_FAILEDT1 = 1<<1; +*/ +int ProcessConeRayIntersectionPoint( dReal r,dReal h, + const dVector3 &q,const dVector3 &v,dReal t, + dVector3 &p, + dVector3 &n, + int &f) +{ + dOPC(p,*,v,t); + dOPE(p,+=,q); + n[0] = 2*p[0]; + n[1] = 2*p[1]; + n[2] = -2*p[2]*r*r/(h*h); + + f = 0; + if (p[2] > h) return 0; + if (p[2] < 0) return 0; + if (t > 1) return 0; + if (t < 0) return 0; + + return 1; +} + +//cone ray +//line in cone space (position,direction) +//distance from line position (direction normalized)(if any) +//return the number of intersection +int FindIntersectionConeRay(dReal r,dReal h, + const dVector3 &q,const dVector3 &v,dContactGeom *pContact) +{ + dVector3 qp,vp; + dOPE(qp,=,q); + dOPE(vp,=,v); + qp[2] = h-q[2]; + vp[2] = -v[2]; + dReal ts = (r/h); + ts *= ts; + dReal a = vp[0]*vp[0] + vp[1]*vp[1] - ts*vp[2]*vp[2]; + dReal b = 2.f*qp[0]*vp[0] + 2.f*qp[1]*vp[1] - 2.f*ts*qp[2]*vp[2]; + dReal c = qp[0]*qp[0] + qp[1]*qp[1] - ts*qp[2]*qp[2]; + +/* + dReal a = v[0]*v[0] + v[1]*v[1] - (v[2]*v[2]*r*r) / (h*h); + 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); + dReal c = q[0]*q[0] + q[1]*q[1] + 2*r*r*q[2]/h - r*r*q[2]/(h*h) - r*r; +*/ + int nNumRoots=SolveQuadraticPolynomial(a,b,c,pContact[0].depth,pContact[1].depth); + int flag = 0; + + dContactGeom ValidContact[2]; + + int nNumValidContacts = 0; + for (int i=0;i=0) && (d<=1)) + { + dOPC(vp,*,v,d); + dOP(qp,+,q,vp); + + if (qp[0]*qp[0]+qp[1]*qp[1] < r*r) + { + dOPE(ValidContact[nNumValidContacts].pos,=,qp); + ValidContact[nNumValidContacts].normal[0] = 0.f; + ValidContact[nNumValidContacts].normal[1] = 0.f; + ValidContact[nNumValidContacts].normal[2] = -1.f; + ValidContact[nNumValidContacts].depth = d; + nNumValidContacts++; + } + } + } + + if (nNumValidContacts == 2) + { + if (ValidContact[0].depth > ValidContact[1].depth) + { + pContact[0] = ValidContact[1]; + pContact[1] = ValidContact[0]; + } + else + { + pContact[0] = ValidContact[0]; + pContact[1] = ValidContact[1]; + } + } + else if (nNumValidContacts == 1) + { + pContact[0] = ValidContact[0]; + } + + return nNumValidContacts; +} + +int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConeClass); + dIASSERT (o2->type == dPlaneClass); + dxCone *cone = (dxCone*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + dVector3 p0,p1,pp0,pp1; + dOPE(p0,=,cone->final_posr->pos); + p1[0] = cone->final_posr->R[0*4+2] * cone->lz + p0[0]; + p1[1] = cone->final_posr->R[1*4+2] * cone->lz + p0[1]; + p1[2] = cone->final_posr->R[2*4+2] * cone->lz + p0[2]; + + dReal u; + FindIntersectionPlaneRay(plane->p,p0,plane->p,u,pp0); + FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1); + + if (dDISTANCE(pp0,pp1) < fEPSILON) + { + p1[0] = cone->final_posr->R[0*4+0] * cone->lz + p0[0]; + p1[1] = cone->final_posr->R[1*4+0] * cone->lz + p0[1]; + p1[2] = cone->final_posr->R[2*4+0] * cone->lz + p0[2]; + FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1); + dIASSERT(dDISTANCE(pp0,pp1) >= fEPSILON); + } + dVector3 h,r0,r1; + h[0] = cone->final_posr->R[0*4+2]; + h[1] = cone->final_posr->R[1*4+2]; + h[2] = cone->final_posr->R[2*4+2]; + + dOP(r0,-,pp0,pp1); + dCROSS(r1,=,h,r0); + dCROSS(r0,=,r1,h); + dNormalize3(r0); + dOPEC(h,*=,cone->lz); + dOPEC(r0,*=,cone->radius); + + dVector3 p[3]; + dOP(p[0],+,cone->final_posr->pos,h); + dOP(p[1],+,cone->final_posr->pos,r0); + dOP(p[2],-,cone->final_posr->pos,r0); + + int numMaxContacts = flags & 0xffff; + if (numMaxContacts == 0) + numMaxContacts = 1; + + int n=0; + for (int i=0;i<3;i++) + { + dReal d = dGeomPlanePointDepth(o2, p[i][0], p[i][1], p[i][2]); + + if (d>0.f) + { + CONTACT(contact,n*skip)->g1 = o1; + CONTACT(contact,n*skip)->g2 = o2; + dOPE(CONTACT(contact,n*skip)->normal,=,plane->p); + dOPE(CONTACT(contact,n*skip)->pos,=,p[i]); + CONTACT(contact,n*skip)->depth = d; + n++; + + if (n == numMaxContacts) + return n; + } + } + + return n; +} + +int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dConeClass); + dxRay *ray = (dxRay*) o1; + dxCone *cone = (dxCone*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + dVector3 tmp,q,v; + tmp[0] = ray->final_posr->pos[0] - cone->final_posr->pos[0]; + tmp[1] = ray->final_posr->pos[1] - cone->final_posr->pos[1]; + tmp[2] = ray->final_posr->pos[2] - cone->final_posr->pos[2]; + dMULTIPLY1_331 (q,cone->final_posr->R,tmp); + tmp[0] = ray->final_posr->R[0*4+2] * ray->length; + tmp[1] = ray->final_posr->R[1*4+2] * ray->length; + tmp[2] = ray->final_posr->R[2*4+2] * ray->length; + dMULTIPLY1_331 (v,cone->final_posr->R,tmp); + + dReal r = cone->radius; + dReal h = cone->lz; + + dContactGeom Contact[2]; + + if (FindIntersectionConeRay(r,h,q,v,Contact)) + { + dMULTIPLY0_331(contact->normal,cone->final_posr->R,Contact[0].normal); + dMULTIPLY0_331(contact->pos,cone->final_posr->R,Contact[0].pos); + dOPE(contact->pos,+=,cone->final_posr->pos); + contact->depth = Contact[0].depth * dLENGTH(v); +/* + dMatrix3 RI; + dRSetIdentity (RI); + dVector3 ss; + ss[0] = 0.01f; + ss[1] = 0.01f; + ss[2] = 0.01f; + + dsSetColorAlpha (1,0,0,0.8f); + dsDrawBox(contact->pos,RI,ss); +*/ + return 1; + } + + return 0; +} + +int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConeClass); + dIASSERT (o2->type == dSphereClass); + dxCone *cone = (dxCone*) o1; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideSphereSphere(&ASphere, o2, flags, contact, skip); +} + +int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConeClass); + dIASSERT (o2->type == dBoxClass); + dxCone *cone = (dxCone*) o1; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideSphereBox(&ASphere, o2, flags, contact, skip); +} + +int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCCylinderClass); + dIASSERT (o2->type == dConeClass); + dxCone *cone = (dxCone*) o2; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideCCylinderSphere(o1, &ASphere, flags, contact, skip); +} + +extern int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dConeClass); + dxCone *cone = (dxCone*) o2; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideSTL(o1, &ASphere, flags, contact, skip); +} + + + + + 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 @@ +//Benoit CHAPEROT 2003-2004 www.jstarlab.com +//some code inspired by Magic Software +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_std_internal.h" +#include "collision_util.h" +//#include +#include "windows.h" +#include "ode\ode.h" + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) +#define MAXCONTACT 10 +#define TERRAINTOL 0.0f + +static bool IsAPowerOfTwo(int f) +{ + dAASSERT(f!=0); + while ((f&1) != 1) + f >>= 1; + + return (f == 1); +} + +static int GetPowerOfTwo(int f) +{ + dAASSERT(f!=0); + int n = 0; + while ((f&1) != 1) + { + n++; + f >>= 1; + } + + return n; +} + +dxTerrainY::dxTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) : +dxGeom (space,bPlaceable) +{ + dIASSERT(IsAPowerOfTwo(nNumNodesPerSide)); + dIASSERT(pHeights); + dIASSERT(vLength > 0.f); + dIASSERT(nNumNodesPerSide > 0); + type = dTerrainYClass; + m_vLength = vLength; + m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide]; + dIASSERT(m_pHeights); + m_nNumNodesPerSide = nNumNodesPerSide; + m_vNodeLength = m_vLength / m_nNumNodesPerSide; + m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide); + m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1; + m_vMinHeight = dInfinity; + m_vMaxHeight = -dInfinity; + m_bFinite = bFinite; + + for (int i=0;i m_vMaxHeight) m_vMaxHeight = m_pHeights[i]; + } +} + +dxTerrainY::~dxTerrainY() +{ + dIASSERT(m_pHeights); + delete [] m_pHeights; +} + +void dxTerrainY::computeAABB() +{ + if (m_bFinite) + { + if (gflags & GEOM_PLACEABLE) + { + dReal dx[6],dy[6],dz[6]; + dx[0] = 0; + dx[1] = final_posr->R[0] * m_vLength; + dx[2] = final_posr->R[1] * m_vMinHeight; + dx[3] = final_posr->R[1] * m_vMaxHeight; + dx[4] = 0; + dx[5] = final_posr->R[2] * m_vLength; + + dy[0] = 0; + dy[1] = final_posr->R[4] * m_vLength; + dy[2] = final_posr->R[5] * m_vMinHeight; + dy[3] = final_posr->R[5] * m_vMaxHeight; + dy[4] = 0; + dy[5] = final_posr->R[6] * m_vLength; + + dz[0] = 0; + dz[1] = final_posr->R[8] * m_vLength; + dz[2] = final_posr->R[9] * m_vMinHeight; + dz[3] = final_posr->R[9] * m_vMaxHeight; + dz[4] = 0; + dz[5] = final_posr->R[10] * m_vLength; + + aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]); + aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]); + aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]); + aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]); + aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]); + aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]); + } + else + { + aabb[0] = 0; + aabb[1] = m_vLength; + aabb[2] = m_vMinHeight; + aabb[3] = m_vMaxHeight; + aabb[4] = 0; + aabb[5] = m_vLength; + } + } + else + { + if (gflags & GEOM_PLACEABLE) + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + } + else + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = m_vMinHeight; + aabb[3] = m_vMaxHeight; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + } + } +} + +dReal dxTerrainY::GetHeight(int x,int z) +{ + return m_pHeights[ (((unsigned int)(z) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift) + + ((unsigned int)(x) & m_nNumNodesPerSideMask)]; +} + +dReal dxTerrainY::GetHeight(dReal x,dReal z) +{ + int nX = int(floor(x / m_vNodeLength)); + int nZ = int(floor(z / m_vNodeLength)); + dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength; + dReal dz = (z - (dReal(nZ) * m_vNodeLength)) / m_vNodeLength; + dIASSERT((dx >= 0.f) && (dx <= 1.f)); + dIASSERT((dz >= 0.f) && (dz <= 1.f)); + + dReal y,y0; + + if (dx + dz < 1.f) + { + y0 = GetHeight(nX,nZ); + y = y0 + + (GetHeight(nX+1,nZ) - y0) * dx + + (GetHeight(nX,nZ+1) - y0) * dz; + } + else + { + y0 = GetHeight(nX+1,nZ+1); + y = y0 + + (GetHeight(nX+1,nZ) - y0) * (1.f - dz) + + (GetHeight(nX,nZ+1) - y0) * (1.f - dx); + } + + return y; +} + +bool dxTerrainY::IsOnTerrain(int nx,int nz,int w,dReal *pos) +{ + dVector3 Min,Max; + Min[0] = nx * m_vNodeLength; + Min[2] = nz * m_vNodeLength; + Max[0] = (nx+1) * m_vNodeLength; + Max[2] = (nz+1) * m_vNodeLength; + dReal Tol = m_vNodeLength * TERRAINTOL; + + if ((pos[0]Max[0]+Tol)) + return false; + + if ((pos[2]Max[2]+Tol)) + return false; + + dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength; + dReal dz = (pos[2] - (dReal(nz) * m_vNodeLength)) / m_vNodeLength; + + if ((w == 0) && (dx + dz > 1.f+TERRAINTOL)) + return false; + + if ((w == 1) && (dx + dz < 1.f-TERRAINTOL)) + return false; + + return true; +} + +dGeomID dCreateTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) +{ + return new dxTerrainY(space, pHeights,vLength,nNumNodesPerSide,bFinite,bPlaceable); +} + +dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dTerrainYClass,"argument not a terrain"); + g->recomputePosr(); + dxTerrainY *t = (dxTerrainY*) g; + return t->GetHeight(x,z) - y; +} + +typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z); +#define RECOMPUTE_RAYNORMAL +//#define DO_RAYDEPTH + +#define DMESS(A) \ + dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \ + x,z,A, \ + pContact->depth, \ + dGeomSphereGetRadius(o2), \ + pContact->pos[0], \ + pContact->pos[1], \ + pContact->pos[2], \ + pContact->normal[0], \ + pContact->normal[1], \ + pContact->normal[2]); +/* +(y is up) + +A-B-E.x +|/| +C-D +| +F +. +z +*/ +int dxTerrainY::dCollideTerrainUnit( + int x,int z,dxGeom *o2,int numMaxContacts, + int flags,dContactGeom *contact, int skip) +{ + dColliderFn *CollideRayN; + dColliderFn *CollideNPlane; + dGetDepthFn *GetDepth; + int numContacts = 0; + int numPlaneContacts = 0; + int i; + + if (numContacts == numMaxContacts) + return numContacts; + + dContactGeom PlaneContact[MAXCONTACT]; + flags = (flags & 0xffff0000) | MAXCONTACT; + + switch (o2->type) + { + case dSphereClass: + CollideRayN = dCollideRaySphere; + CollideNPlane = dCollideSpherePlane; + GetDepth = dGeomSpherePointDepth; + break; + case dBoxClass: + CollideRayN = dCollideRayBox; + CollideNPlane = dCollideBoxPlane; + GetDepth = dGeomBoxPointDepth; + break; + case dCCylinderClass: + CollideRayN = dCollideRayCCylinder; + CollideNPlane = dCollideCCylinderPlane; + GetDepth = dGeomCCylinderPointDepth; + break; + case dRayClass: + CollideRayN = NULL; + CollideNPlane = dCollideRayPlane; + GetDepth = NULL; + break; + case dConeClass: + CollideRayN = dCollideRayCone; + CollideNPlane = dCollideConePlane; + GetDepth = dGeomConePointDepth; + break; + default: + dIASSERT(0); + } + + dReal Plane[4],lBD,lCD,lBC; + dVector3 A,B,C,D,BD,CD,BC,AB,AC; + A[0] = x * m_vNodeLength; + A[2] = z* m_vNodeLength; + A[1] = GetHeight(x,z); + B[0] = (x+1) * m_vNodeLength; + B[2] = z * m_vNodeLength; + B[1] = GetHeight(x+1,z); + C[0] = x * m_vNodeLength; + C[2] = (z+1) * m_vNodeLength; + C[1] = GetHeight(x,z+1); + D[0] = (x+1) * m_vNodeLength; + D[2] = (z+1) * m_vNodeLength; + D[1] = GetHeight(x+1,z+1); + + dOP(BC,-,C,B); + lBC = dLENGTH(BC); + dOPEC(BC,/=,lBC); + + dOP(BD,-,D,B); + lBD = dLENGTH(BD); + dOPEC(BD,/=,lBD); + + dOP(CD,-,D,C); + lCD = dLENGTH(CD); + dOPEC(CD,/=,lCD); + + dOP(AB,-,B,A); + dNormalize3(AB); + + dOP(AC,-,C,A); + dNormalize3(AC); + + if (CollideRayN) + { +#ifdef RECOMPUTE_RAYNORMAL + dVector3 E,F; + dVector3 CE,FB,AD; + dVector3 Normal[3]; + E[0] = (x+2) * m_vNodeLength; + E[2] = z * m_vNodeLength; + E[1] = GetHeight(x+2,z); + F[0] = x * m_vNodeLength; + F[2] = (z+2) * m_vNodeLength; + F[1] = GetHeight(x,z+2); + dOP(AD,-,D,A); + dNormalize3(AD); + dOP(CE,-,E,C); + dNormalize3(CE); + dOP(FB,-,B,F); + dNormalize3(FB); + + //BC + dCROSS(Normal[0],=,BC,AD); + dNormalize3(Normal[0]); + + //BD + dCROSS(Normal[1],=,BD,CE); + dNormalize3(Normal[1]); + + //CD + dCROSS(Normal[2],=,CD,FB); + dNormalize3(Normal[2]); +#endif + int nA[3],nB[3]; + dContactGeom ContactA[3],ContactB[3]; + dxRay rayBC(0,lBC); + dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]); + nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom)); + dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]); + nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom)); + + dxRay rayBD(0,lBD); + dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]); + nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom)); + dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]); + nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom)); + + dxRay rayCD(0,lCD); + dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]); + nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom)); + dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]); + nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom)); + + for (i=0;i<3;i++) + { + if (nA[i] & nB[i]) + { + dContactGeom *pContact = CONTACT(contact,numContacts*skip); + pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2; + pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2; + pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2; +#ifdef RECOMPUTE_RAYNORMAL + pContact->normal[0] = -Normal[i][0]; + pContact->normal[1] = -Normal[i][1]; + pContact->normal[2] = -Normal[i][2]; +#else + pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f; + pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f; + pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f; + dNormalize3(pContact->normal); +#endif +#ifdef DO_RAYDEPTH + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } +#else + + if (GetDepth == NULL) + { + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } + } + else + { + pContact->depth = GetDepth(o2, + pContact->pos[0], + pContact->pos[1], + pContact->pos[2]); + numContacts++; + } + +#endif + if (numContacts == numMaxContacts) + return numContacts; + + } + } + } + + dCROSS(Plane,=,AC,AB); + dNormalize3(Plane); + Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2]; + dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + + //DMESS(0); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + dCROSS(Plane,=,BD,CD); + dNormalize3(Plane); + Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2]; + dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + //DMESS(1); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + return numContacts; +} + +int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTerrainYClass); + int i,j; + + if ((flags & 0xffff) == 0) + flags = (flags & 0xffff0000) | 1; + + int numMaxTerrainContacts = (flags & 0xffff); + dxTerrainY *terrain = (dxTerrainY*) o1; + + dReal aabbbak[6]; + int gflagsbak; + + dVector3 pos0; + int numTerrainContacts = 0; + + dxPosR *bak; + dxPosR X1; + + if (terrain->gflags & GEOM_PLACEABLE) + { + dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos); + dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0); + dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R); + bak = o2->final_posr; + o2->final_posr = &X1; + memcpy(aabbbak,o2->aabb,sizeof(dReal)*6); + gflagsbak = o2->gflags; + o2->computeAABB(); + } + + int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength)); + int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1; + int nMinZ = int(floor(o2->aabb[4] / terrain->m_vNodeLength)); + int nMaxZ = int(floor(o2->aabb[5] / terrain->m_vNodeLength)) + 1; + + if (terrain->m_bFinite) + { + nMinX = MAX(nMinX,0); + nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide); + nMinZ = MAX(nMinZ,0); + nMaxZ = MIN(nMaxZ,terrain->m_nNumNodesPerSide); + + if ((nMinX >= nMaxX) || (nMinZ >= nMaxZ)) + goto dCollideTerrainYExit; + } + + dVector3 AabbTop; + AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2; + AabbTop[2] = (o2->aabb[4]+o2->aabb[5]) / 2; + AabbTop[1] = o2->aabb[3]; + if (o2->type != dRayClass) + { + dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[2]) - AabbTop[1]; + if (AabbTopDepth > 0.f) + { + contact->depth = AabbTopDepth; + dReal MaxDepth = (o2->aabb[3]-o2->aabb[2]) / 2; + if (contact->depth > MaxDepth) + contact->depth = MaxDepth; + contact->g1 = o1; + contact->g2 = o2; + dOPE(contact->pos,=,AabbTop); + contact->normal[0] = 0.f; + contact->normal[1] = -1.f; + contact->normal[2] = 0.f; + + numTerrainContacts = 1; + goto dCollideTerrainYExit; + } + } + + for (i=nMinX;idCollideTerrainUnit( + i,j,o2,numMaxTerrainContacts - numTerrainContacts, + flags,CONTACT(contact,numTerrainContacts*skip),skip ); + } + } + + dIASSERT(numTerrainContacts <= numMaxTerrainContacts); + + for (i=0; ig1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + +dCollideTerrainYExit: + + if (terrain->gflags & GEOM_PLACEABLE) + { + o2->final_posr = bak; + memcpy(o2->aabb,aabbbak,sizeof(dReal)*6); + o2->gflags = gflagsbak; + + for (i=0; ipos); + dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0); + dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos); + + dOPE(pos0,=,CONTACT(contact,i*skip)->normal); + dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0); + } + } + + return numTerrainContacts; +} +/* +void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos) +{ + float A[3],B[3],C[3],D[3]; + float R[12]; + float pos[3]; + if (pR) + memcpy(R,pR,sizeof(R)); + else + { + memset(R,0,sizeof(R)); + R[0] = 1.f; + R[5] = 1.f; + R[10] = 1.f; + } + + if (ppos) + memcpy(pos,ppos,sizeof(pos)); + else + memset(pos,0,sizeof(pos)); + + float vx,vz; + vx = vLength * x; + vz = vLength * z; + + int i; + for (i=0;i +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_std_internal.h" +#include "collision_util.h" +//#include +#include "windows.h" +#include "ode\ode.h" + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) +#define MAXCONTACT 10 +#define TERRAINTOL 0.0f + +static bool IsAPowerOfTwo(int f) +{ + dAASSERT(f!=0); + while ((f&1) != 1) + f >>= 1; + + return (f == 1); +} + +static int GetPowerOfTwo(int f) +{ + dAASSERT(f!=0); + int n = 0; + while ((f&1) != 1) + { + n++; + f >>= 1; + } + + return n; +} + +dxTerrainZ::dxTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) : +dxGeom (space,bPlaceable) +{ + dIASSERT(IsAPowerOfTwo(nNumNodesPerSide)); + dIASSERT(pHeights); + dIASSERT(vLength > 0.f); + dIASSERT(nNumNodesPerSide > 0); + type = dTerrainZClass; + m_vLength = vLength; + m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide]; + dIASSERT(m_pHeights); + m_nNumNodesPerSide = nNumNodesPerSide; + m_vNodeLength = m_vLength / m_nNumNodesPerSide; + m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide); + m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1; + m_vMinHeight = dInfinity; + m_vMaxHeight = -dInfinity; + m_bFinite = bFinite; + + for (int i=0;i m_vMaxHeight) m_vMaxHeight = m_pHeights[i]; + } +} + +dxTerrainZ::~dxTerrainZ() +{ + dIASSERT(m_pHeights); + delete [] m_pHeights; +} + +void dxTerrainZ::computeAABB() +{ + if (m_bFinite) + { + if (gflags & GEOM_PLACEABLE) + { + dReal dx[6],dy[6],dz[6]; + dx[0] = 0; + dx[1] = final_posr->R[0] * m_vLength; + dx[2] = 0; + dx[3] = final_posr->R[1] * m_vLength; + dx[4] = final_posr->R[2] * m_vMinHeight; + dx[5] = final_posr->R[2] * m_vMaxHeight; + + dy[0] = 0; + dy[1] = final_posr->R[4] * m_vLength; + dy[2] = 0; + dy[3] = final_posr->R[5] * m_vLength; + dy[4] = final_posr->R[6] * m_vMinHeight; + dy[5] = final_posr->R[6] * m_vMaxHeight; + + dz[0] = 0; + dz[1] = final_posr->R[8] * m_vLength; + dz[2] = 0; + dz[3] = final_posr->R[9] * m_vLength; + dz[4] = final_posr->R[10] * m_vMinHeight; + dz[5] = final_posr->R[10] * m_vMaxHeight; + + aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]); + aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]); + aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]); + aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]); + aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]); + aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]); + } + else + { + aabb[0] = 0; + aabb[1] = m_vLength; + aabb[2] = 0; + aabb[3] = m_vLength; + aabb[4] = m_vMinHeight; + aabb[5] = m_vMaxHeight; + } + } + else + { + if (gflags & GEOM_PLACEABLE) + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + } + else + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = m_vMinHeight; + aabb[5] = m_vMaxHeight; + } + } +} + +dReal dxTerrainZ::GetHeight(int x,int y) +{ + return m_pHeights[ (((unsigned int)(y) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift) + + ((unsigned int)(x) & m_nNumNodesPerSideMask)]; +} + +dReal dxTerrainZ::GetHeight(dReal x,dReal y) +{ + int nX = int(floor(x / m_vNodeLength)); + int nY = int(floor(y / m_vNodeLength)); + dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength; + dReal dy = (y - (dReal(nY) * m_vNodeLength)) / m_vNodeLength; + dIASSERT((dx >= 0.f) && (dx <= 1.f)); + dIASSERT((dy >= 0.f) && (dy <= 1.f)); + + dReal z,z0; + + if (dx + dy < 1.f) + { + z0 = GetHeight(nX,nY); + z = z0 + + (GetHeight(nX+1,nY) - z0) * dx + + (GetHeight(nX,nY+1) - z0) * dy; + } + else + { + z0 = GetHeight(nX+1,nY+1); + z = z0 + + (GetHeight(nX+1,nY) - z0) * (1.f - dy) + + (GetHeight(nX,nY+1) - z0) * (1.f - dx); + } + + return z; +} + +bool dxTerrainZ::IsOnTerrain(int nx,int ny,int w,dReal *pos) +{ + dVector3 Min,Max; + Min[0] = nx * m_vNodeLength; + Min[1] = ny * m_vNodeLength; + Max[0] = (nx+1) * m_vNodeLength; + Max[1] = (ny+1) * m_vNodeLength; + dReal Tol = m_vNodeLength * TERRAINTOL; + + if ((pos[0]Max[0]+Tol)) + return false; + + if ((pos[1]Max[1]+Tol)) + return false; + + dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength; + dReal dy = (pos[1] - (dReal(ny) * m_vNodeLength)) / m_vNodeLength; + + if ((w == 0) && (dx + dy > 1.f+TERRAINTOL)) + return false; + + if ((w == 1) && (dx + dy < 1.f-TERRAINTOL)) + return false; + + return true; +} + +dGeomID dCreateTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) +{ + return new dxTerrainZ(space, pHeights,vLength,nNumNodesPerSide, bFinite, bPlaceable); +} + +dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dTerrainZClass,"argument not a terrain"); + g->recomputePosr(); + dxTerrainZ *t = (dxTerrainZ*) g; + return t->GetHeight(x,y) - z; +} + +typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z); +#define RECOMPUTE_RAYNORMAL +//#define DO_RAYDEPTH + +#define DMESS(A) \ + dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \ + x,y,A, \ + pContact->depth, \ + dGeomSphereGetRadius(o2), \ + pContact->pos[0], \ + pContact->pos[1], \ + pContact->pos[2], \ + pContact->normal[0], \ + pContact->normal[1], \ + pContact->normal[2]); +/* +(z is up) + +y +. +F +| +C-D +|\| +A-B-E.x +*/ +int dxTerrainZ::dCollideTerrainUnit( + int x,int y,dxGeom *o2,int numMaxContacts, + int flags,dContactGeom *contact, int skip) +{ + dColliderFn *CollideRayN; + dColliderFn *CollideNPlane; + dGetDepthFn *GetDepth; + int numContacts = 0; + int numPlaneContacts = 0; + int i; + + if (numContacts == numMaxContacts) + return numContacts; + + dContactGeom PlaneContact[MAXCONTACT]; + flags = (flags & 0xffff0000) | MAXCONTACT; + + switch (o2->type) + { + case dSphereClass: + CollideRayN = dCollideRaySphere; + CollideNPlane = dCollideSpherePlane; + GetDepth = dGeomSpherePointDepth; + break; + case dBoxClass: + CollideRayN = dCollideRayBox; + CollideNPlane = dCollideBoxPlane; + GetDepth = dGeomBoxPointDepth; + break; + case dCCylinderClass: + CollideRayN = dCollideRayCCylinder; + CollideNPlane = dCollideCCylinderPlane; + GetDepth = dGeomCCylinderPointDepth; + break; + case dRayClass: + CollideRayN = NULL; + CollideNPlane = dCollideRayPlane; + GetDepth = NULL; + break; + case dConeClass: + CollideRayN = dCollideRayCone; + CollideNPlane = dCollideConePlane; + GetDepth = dGeomConePointDepth; + break; + default: + dIASSERT(0); + } + + dReal Plane[4],lBD,lCD,lBC; + dVector3 A,B,C,D,BD,CD,BC,AB,AC; + A[0] = x * m_vNodeLength; + A[1] = y * m_vNodeLength; + A[2] = GetHeight(x,y); + B[0] = (x+1) * m_vNodeLength; + B[1] = y * m_vNodeLength; + B[2] = GetHeight(x+1,y); + C[0] = x * m_vNodeLength; + C[1] = (y+1) * m_vNodeLength; + C[2] = GetHeight(x,y+1); + D[0] = (x+1) * m_vNodeLength; + D[1] = (y+1) * m_vNodeLength; + D[2] = GetHeight(x+1,y+1); + + dOP(BC,-,C,B); + lBC = dLENGTH(BC); + dOPEC(BC,/=,lBC); + + dOP(BD,-,D,B); + lBD = dLENGTH(BD); + dOPEC(BD,/=,lBD); + + dOP(CD,-,D,C); + lCD = dLENGTH(CD); + dOPEC(CD,/=,lCD); + + dOP(AB,-,B,A); + dNormalize3(AB); + + dOP(AC,-,C,A); + dNormalize3(AC); + + if (CollideRayN) + { +#ifdef RECOMPUTE_RAYNORMAL + dVector3 E,F; + dVector3 CE,FB,AD; + dVector3 Normal[3]; + E[0] = (x+2) * m_vNodeLength; + E[1] = y * m_vNodeLength; + E[2] = GetHeight(x+2,y); + F[0] = x * m_vNodeLength; + F[1] = (y+2) * m_vNodeLength; + F[2] = GetHeight(x,y+2); + dOP(AD,-,D,A); + dNormalize3(AD); + dOP(CE,-,E,C); + dNormalize3(CE); + dOP(FB,-,B,F); + dNormalize3(FB); + + //BC + dCROSS(Normal[0],=,AD,BC); + dNormalize3(Normal[0]); + + //BD + dCROSS(Normal[1],=,CE,BD); + dNormalize3(Normal[1]); + + //CD + dCROSS(Normal[2],=,FB,CD); + dNormalize3(Normal[2]); +#endif + int nA[3],nB[3]; + dContactGeom ContactA[3],ContactB[3]; + dxRay rayBC(0,lBC); + dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]); + nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom)); + dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]); + nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom)); + + dxRay rayBD(0,lBD); + dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]); + nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom)); + dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]); + nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom)); + + dxRay rayCD(0,lCD); + dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]); + nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom)); + dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]); + nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom)); + + for (i=0;i<3;i++) + { + if (nA[i] & nB[i]) + { + dContactGeom *pContact = CONTACT(contact,numContacts*skip); + pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2; + pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2; + pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2; +#ifdef RECOMPUTE_RAYNORMAL + pContact->normal[0] = -Normal[i][0]; + pContact->normal[1] = -Normal[i][1]; + pContact->normal[2] = -Normal[i][2]; +#else + pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f; + pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f; + pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f; + dNormalize3(pContact->normal); +#endif +#ifdef DO_RAYDEPTH + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } +#else + if (GetDepth == NULL) + { + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } + } + else + { + pContact->depth = GetDepth(o2, + pContact->pos[0], + pContact->pos[1], + pContact->pos[2]); + numContacts++; + } +#endif + if (numContacts == numMaxContacts) + return numContacts; + + } + } + } + + dCROSS(Plane,=,AB,AC); + dNormalize3(Plane); + Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2]; + dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + + //DMESS(0); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + dCROSS(Plane,=,CD,BD); + dNormalize3(Plane); + Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2]; + dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + //DMESS(1); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + return numContacts; +} + +int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTerrainZClass); + int i,j; + + if ((flags & 0xffff) == 0) + flags = (flags & 0xffff0000) | 1; + + int numMaxTerrainContacts = (flags & 0xffff); + dxTerrainZ *terrain = (dxTerrainZ*) o1; + + dReal aabbbak[6]; + int gflagsbak; + + dVector3 pos0; + int numTerrainContacts = 0; + + dxPosR *bak; + dxPosR X1; + + if (terrain->gflags & GEOM_PLACEABLE) + { + dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos); + dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0); + dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R); + bak = o2->final_posr; + o2->final_posr = &X1; + memcpy(aabbbak,o2->aabb,sizeof(dReal)*6); + gflagsbak = o2->gflags; + o2->computeAABB(); + } + + int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength)); + int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1; + int nMinY = int(floor(o2->aabb[2] / terrain->m_vNodeLength)); + int nMaxY = int(floor(o2->aabb[3] / terrain->m_vNodeLength)) + 1; + + if (terrain->m_bFinite) + { + nMinX = MAX(nMinX,0); + nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide); + nMinY = MAX(nMinY,0); + nMaxY = MIN(nMaxY,terrain->m_nNumNodesPerSide); + + if ((nMinX >= nMaxX) || (nMinY >= nMaxY)) + goto dCollideTerrainZExit; + } + + dVector3 AabbTop; + AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2; + AabbTop[1] = (o2->aabb[2]+o2->aabb[3]) / 2; + AabbTop[2] = o2->aabb[5]; + if (o2->type != dRayClass) + { + dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[1]) - AabbTop[2]; + if (AabbTopDepth > 0.f) + { + contact->depth = AabbTopDepth; + dReal MaxDepth = (o2->aabb[5]-o2->aabb[4]) / 2; + if (contact->depth > MaxDepth) + contact->depth = MaxDepth; + contact->g1 = o1; + contact->g2 = o2; + dOPE(contact->pos,=,AabbTop); + contact->normal[0] = 0.f; + contact->normal[1] = 0.f; + contact->normal[2] = -1.f; + + numTerrainContacts = 1; + goto dCollideTerrainZExit; + } + } + + for (i=nMinX;idCollideTerrainUnit( + i,j,o2,numMaxTerrainContacts - numTerrainContacts, + flags,CONTACT(contact,numTerrainContacts*skip),skip ); + } + } + + dIASSERT(numTerrainContacts <= numMaxTerrainContacts); + + for (i=0; ig1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + +dCollideTerrainZExit: + + if (terrain->gflags & GEOM_PLACEABLE) + { + o2->final_posr = bak; + memcpy(o2->aabb,aabbbak,sizeof(dReal)*6); + o2->gflags = gflagsbak; + + for (i=0; ipos); + dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0); + dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos); + + dOPE(pos0,=,CONTACT(contact,i*skip)->normal); + dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0); + } + } + + return numTerrainContacts; +} +/* +void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos) +{ + float A[3],B[3],C[3],D[3]; + float R[12]; + float pos[3]; + if (pR) + memcpy(R,pR,sizeof(R)); + else + { + memset(R,0,sizeof(R)); + R[0] = 1.f; + R[5] = 1.f; + R[10] = 1.f; + } + + if (ppos) + memcpy(pos,ppos,sizeof(pos)); + else + memset(pos,0,sizeof(pos)); + + float vx,vz; + vx = vLength * x; + vz = vLength * z; + + int i; + for (i=0;i edit each .cpp file and comment out #include "windows.h" & #include "ode\ode.h" + + +*** add to drawstuff\src\drawstuff.cpp: + +static void drawCone(float l, float r) +{ + int i; + float tmp,ny,nz,a,ca,sa; + const int n = 24; // number of sides to the cone (divisible by 4) + + a = float(M_PI*2.0)/float(n); + sa = (float) sin(a); + ca = (float) cos(a); + + // draw top + glShadeModel (GL_FLAT); + ny=1; nz=0; // normal vector = (0,ny,nz) + glBegin (GL_TRIANGLE_FAN); + glNormal3d (0,0,1); + glVertex3d (0,0,l); + for (i=0; i<=n; i++) { + if (i==1 || i==n/2+1) + setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]); + glNormal3d (ny*r,nz*r,0); + glVertex3d (ny*r,nz*r,0); + if (i==1 || i==n/2+1) + setColor (color[0],color[1],color[2],color[3]); + + // rotate ny,nz + tmp = ca*ny - sa*nz; + nz = sa*ny + ca*nz; + ny = tmp; + } + glEnd(); + + // draw bottom + ny=1; nz=0; // normal vector = (0,ny,nz) + glBegin (GL_TRIANGLE_FAN); + glNormal3d (0,0,-1); + glVertex3d (0,0,0); + for (i=0; i<=n; i++) { + if (i==1 || i==n/2+1) + setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]); + glNormal3d (0,0,-1); + glVertex3d (ny*r,nz*r,0); + if (i==1 || i==n/2+1) + setColor (color[0],color[1],color[2],color[3]); + + // rotate ny,nz + tmp = ca*ny + sa*nz; + nz = -sa*ny + ca*nz; + ny = tmp; + } + glEnd(); +} + +void dsDrawCone (const float pos[3], const float R[12], float length, float radius) +{ + if (current_state != 2) dsError ("drawing function called outside simulation loop"); + setupDrawingMode(); + glShadeModel (GL_SMOOTH); + setTransform (pos,R); + drawCone (length,radius); + glPopMatrix(); + + if (use_shadows) { + setShadowDrawingMode(); + setShadowTransform(); + setTransform (pos,R); + drawCone (length,radius); + glPopMatrix(); + glPopMatrix(); + glDepthRange (0,1); + } +} + +void dsDrawConeD (const double pos[3], const double R[12], float length, float radius) +{ + int i; + float pos2[3],R2[12]; + for (i=0; i<3; i++) pos2[i]=(float)pos[i]; + for (i=0; i<12; i++) R2[i]=(float)R[i]; + dsDrawCone(pos2,R2,length,radius); +} + +static float GetHeight(int x,int y,int nNumNodesPerSide,float *pHeights) +{ + int nNumNodesPerSideMask = nNumNodesPerSide - 1; + return pHeights[ (((unsigned int)(y) & nNumNodesPerSideMask) * nNumNodesPerSide) + + ((unsigned int)(x) & nNumNodesPerSideMask)]; +} + +void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos) +{ + float A[3],B[3],C[3],D[3]; + float R[12]; + float pos[3]; + if (pR) + memcpy(R,pR,sizeof(R)); + else + { + memset(R,0,sizeof(R)); + R[0] = 1.f; + R[5] = 1.f; + R[10] = 1.f; + } + + if (ppos) + memcpy(pos,ppos,sizeof(pos)); + else + memset(pos,0,sizeof(pos)); + + float vx,vz; + vx = vLength * x; + vz = vLength * z; + + int i; + for (i=0;i add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the libode_a_SOURCES variable in the ode/src/Makefile.am file. + +*** now you can now test using file test_boxstackb.cpp (to add in folder ode\test). + 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 @@ +/************************************************************************* + + +* * + + +* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + + +* All rights reserved. Email: russ@q12.org Web: www.q12.org * + + +* * + + +* This library is free software; you can redistribute it and/or * + + +* modify it under the terms of EITHER: * + + +* (1) The GNU Lesser General Public License as published by the Free * + + +* Software Foundation; either version 2.1 of the License, or (at * + + +* your option) any later version. The text of the GNU Lesser * + + +* General Public License is included with this library in the * + + +* file LICENSE.TXT. * + + +* (2) The BSD-style license that is included with this library in * + + +* the file LICENSE-BSD.TXT. * + + +* * + + +* This library is distributed in the hope that it will be useful, * + + +* but WITHOUT ANY WARRANTY; without even the implied warranty of * + + +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + + +* LICENSE.TXT and LICENSE-BSD.TXT for more details. * + + +* * + + +*************************************************************************/ + + + + + +#include + + +#include + + + + + +#ifdef _MSC_VER + + +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints + + +#endif + + + + + +// select correct drawing functions + + + + + +#ifdef dDOUBLE + + +#define dsDrawBox dsDrawBoxD + + +#define dsDrawSphere dsDrawSphereD + + +#define dsDrawCylinder dsDrawCylinderD + + +#define dsDrawCappedCylinder dsDrawCappedCylinderD + + +#endif + + + + + + + + +// some constants + + + + + +const dReal vTerrainLength = 4.f; + + +const dReal vTerrainHeight = 0.5f; + + +const int TERRAINNODES = 4; + + +dReal pTerrainHeights[TERRAINNODES*TERRAINNODES]; + + + + + +dGeomID terrainZ = NULL; + + +dGeomID terrainY = NULL; + + + + + +#define NUM 20 // max number of objects + + +#define DENSITY (5.0) // density of all objects + + +#define GPB 3 // maximum number of geometries per body + + +#define MAX_CONTACTS 4 // maximum number of contact points per body + + + + + + + + +// dynamics and collision objects + + + + + +struct MyObject { + + + dBodyID body; // the body + + + dGeomID geom[GPB]; // geometries representing this body + + +}; + + + + + +static int num=0; // number of objects in simulation + + +static int nextobj=0; // next object to recycle if num==NUM + + +static dWorldID world; + + +static dSpaceID space; + + +static MyObject obj[NUM]; + + +static dJointGroupID contactgroup; + + +static int selected = -1; // selected object + + +static int show_aabb = 0; // show geom AABBs? + + +static int show_contacts = 0; // show contact points? + + +static int random_pos = 1; // drop objects from random position? + + + + + + + + +// this is called by dSpaceCollide when two objects in space are + + +// potentially colliding. + + + + + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) + + +{ + + + int i; + + + // if (o1->body && o2->body) return; + + + + + + // exit without doing anything if the two bodies are connected by a joint + + + dBodyID b1 = dGeomGetBody(o1); + + + dBodyID b2 = dGeomGetBody(o2); + + + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + + + + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + + + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + + + else return c; + + +} + + + + + + + + +// called when a key pressed + + + + + +static void command (int cmd) + + +{ + + + int i,j,k; + + + dReal sides[3]; + + + dMass m; + + + + + + cmd = locase (cmd); + + + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' + + + /* || cmd == 'l' */) { + + + if (num < NUM) { + + + i = num; + + + num++; + + + } + + + else { + + + i = nextobj; + + + nextobj++; + + + if (nextobj >= num) nextobj = 0; + + + + + + // destroy the body and geoms for slot i + + + dBodyDestroy (obj[i].body); + + + for (k=0; k < GPB; k++) { + + + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + + + } + + + memset (&obj[i],0,sizeof(obj[i])); + + + } + + + + + + obj[i].body = dBodyCreate (world); + + + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + + + + + dMatrix3 R; + + + if (random_pos) { + + + dBodySetPosition (obj[i].body, + + + dRandReal()*2-1,dRandReal()*2+1,dRandReal()+3); + + + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + + + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + + + } + + + else { + + + dReal maxheight = 0; + + + for (k=0; k maxheight) maxheight = pos[2]; + + + } + + + dBodySetPosition (obj[i].body, 0,maxheight+1,maxheight+3); + + + dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); + + + } + + + dBodySetRotation (obj[i].body,R); + + + dBodySetData (obj[i].body,(void*) i); + + + + + + if (cmd == 'b') { + + + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + + + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + + + } + + + else if (cmd == 'c') { + + + sides[0] *= 0.5; + + + dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); + + + obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]); + + + } + + + /* + + + // cylinder option not yet implemented + + + else if (cmd == 'l') { + + + sides[1] *= 0.5; + + + dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); + + + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + + + } + + + */ + + + else if (cmd == 's') { + + + sides[0] *= 0.5; + + + dMassSetSphere (&m,DENSITY,sides[0]); + + + obj[i].geom[0] = dCreateSphere (space,sides[0]); + + + } + + + else if (cmd == 'x') { + + + dGeomID g2[GPB]; // encapsulated geometries + + + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + + + + + // start accumulating masses for the encapsulated geometries + + + dMass m2; + + + dMassSetZero (&m); + + + + + + // set random delta positions + + + for (j=0; j= num) selected = 0; + + + if (selected < 0) selected = 0; + + + } + + + else if (cmd == 'd' && selected >= 0 && selected < num) { + + + dBodyDisable (obj[selected].body); + + + } + + + else if (cmd == 'e' && selected >= 0 && selected < num) { + + + dBodyEnable (obj[selected].body); + + + } + + + else if (cmd == 'a') { + + + show_aabb ^= 1; + + + } + + + else if (cmd == 't') { + + + show_contacts ^= 1; + + + } + + + else if (cmd == 'r') { + + + random_pos ^= 1; + + + } + + +} + + + + + + + + +// draw a geom + + + + + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) + + +{ + + + int i; + + + + + + if (!g) return; + + + if (!pos) pos = dGeomGetPosition (g); + + + if (!R) R = dGeomGetRotation (g); + + + + + + int type = dGeomGetClass (g); + + + if (type == dBoxClass) { + + + dVector3 sides; + + + dGeomBoxGetLengths (g,sides); + + + dsDrawBox (pos,R,sides); + + + } + + + else if (type == dSphereClass) { + + + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + + + } + + + else if (type == dCCylinderClass) { + + + dReal radius,length; + + + dGeomCCylinderGetParams (g,&radius,&length); + + + dsDrawCappedCylinder (pos,R,length,radius); + + + } + + + /* + + + // cylinder option not yet implemented + + + else if (type == dCylinderClass) { + + + dReal radius,length; + + + dGeomCylinderGetParams (g,&radius,&length); + + + dsDrawCylinder (pos,R,length,radius); + + + } + + + */ + + + else if (type == dGeomTransformClass) { + + + dGeomID g2 = dGeomTransformGetGeom (g); + + + const dReal *pos2 = dGeomGetPosition (g2); + + + const dReal *R2 = dGeomGetRotation (g2); + + + dVector3 actual_pos; + + + dMatrix3 actual_R; + + + dMULTIPLY0_331 (actual_pos,R,pos2); + + + actual_pos[0] += pos[0]; + + + actual_pos[1] += pos[1]; + + + actual_pos[2] += pos[2]; + + + dMULTIPLY0_333 (actual_R,R,R2); + + + drawGeom (g2,actual_pos,actual_R,0); + + + } + + + + + + if (show_aabb) { + + + // draw the bounding box for this geom + + + dReal aabb[6]; + + + dGeomGetAABB (g,aabb); + + + dVector3 bbpos; + + + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + + + dVector3 bbsides; + + + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + + + dMatrix3 RI; + + + dRSetIdentity (RI); + + + dsSetColorAlpha (1,0,0,0.5); + + + dsDrawBox (bbpos,RI,bbsides); + + + } + + +} + + + + + + + + +// simulation loop + + + + + +static void simLoop (int pause) + + +{ + + + dsSetColor (0,0,2); + + + dSpaceCollide (space,0,&nearCallback); + + + if (!pause) dWorldStep (world,0.05); + + + + + + dAASSERT(terrainY); + + + dAASSERT(terrainZ); + + + dsSetColor (0,1,0); + + + dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY)); + + + dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ)); + + + + + + if (show_aabb) + + + { + + + dReal aabb[6]; + + + dGeomGetAABB (terrainY,aabb); + + + dVector3 bbpos; + + + int i; + + + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + + + dVector3 bbsides; + + + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + + + dMatrix3 RI; + + + dRSetIdentity (RI); + + + dsSetColorAlpha (1,0,0,0.5); + + + dsDrawBox (bbpos,RI,bbsides); + + + + + + dGeomGetAABB (terrainZ,aabb); + + + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + + + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + + + dsDrawBox (bbpos,RI,bbsides); + + + } + + + + + + dsSetColor (1,1,0); + + + + + + // remove all contact joints + + + dJointGroupEmpty (contactgroup); + + + + + + dsSetColor (1,1,0); + + + dsSetTexture (DS_WOOD); + + + for (int i=0; i