From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- .../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 deletions(-) delete mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h delete mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp delete mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp delete mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp delete mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/readme.txt delete 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 deleted file mode 100644 index b445353..0000000 --- a/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h +++ /dev/null @@ -1,100 +0,0 @@ -//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 deleted file mode 100644 index 8c5c3be..0000000 --- a/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp +++ /dev/null @@ -1,504 +0,0 @@ -//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 deleted file mode 100644 index ff779ac..0000000 --- a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp +++ /dev/null @@ -1,662 +0,0 @@ -//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 deleted file mode 100644 index f1fa592..0000000 --- a/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp +++ /dev/null @@ -1,1375 +0,0 @@ -/************************************************************************* - - -* * - - -* 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