//Benoit CHAPEROT 2003-2004 www.jstarlab.com
//some code inspired by Magic Software
#include <ode/common.h>
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_kernel.h"
#include "collision_std.h"
#include "collision_std_internal.h"
#include "collision_util.h"
//#include <drawstuff/drawstuff.h>
#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<nNumNodesPerSide * nNumNodesPerSide;i++)
{
m_pHeights[i] = pHeights[i];
if (m_pHeights[i] < m_vMinHeight) m_vMinHeight = m_pHeights[i];
if (m_pHeights[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]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
return false;
if ((pos[1]<Min[1]-Tol) || (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;i<numPlaneContacts;i++)
{
if (IsOnTerrain(x,y,0,PlaneContact[i].pos))
{
dContactGeom *pContact = CONTACT(contact,numContacts*skip);
pContact->pos[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;i<numPlaneContacts;i++)
{
if (IsOnTerrain(x,y,1,PlaneContact[i].pos))
{
dContactGeom *pContact = CONTACT(contact,numContacts*skip);
pContact->pos[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;i<nMaxX;i++)
{
for (j=nMinY;j<nMaxY;j++)
{
numTerrainContacts += terrain->dCollideTerrainUnit(
i,j,o2,numMaxTerrainContacts - numTerrainContacts,
flags,CONTACT(contact,numTerrainContacts*skip),skip );
}
}
dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
for (i=0; i<numTerrainContacts; i++)
{
CONTACT(contact,i*skip)->g1 = 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; i<numTerrainContacts; i++)
{
dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
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<nNumNodesPerSide;i++)
{
for (int j=0;j<nNumNodesPerSide;j++)
{
A[0] = i * vNodeLength + vx;
A[1] = j * vNodeLength + vz;
A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
B[0] = (i+1) * vNodeLength + vx;
B[1] = j * vNodeLength + vz;
B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
C[0] = i * vNodeLength + vx;
C[1] = (j+1) * vNodeLength + vz;
C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
D[0] = (i+1) * vNodeLength + vx;
D[1] = (j+1) * vNodeLength + vz;
D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
dsDrawTriangle(pos,R,C,A,B,1);
dsDrawTriangle(pos,R,D,C,B,1);
}
}
}
*/
syntax highlighted by Code2HTML, v. 0.9.1