Menu
▾
▴
[Opende-svn] SF.net SVN: opende: [1129] trunk/ode/src
|
From: <hid...@us...> - 2006-11-03 14:08:50
|
Revision: 1129
http://svn.sourceforge.net/opende/?rev=1129&view=rev
Author: hidden_asbestos
Date: 2006-11-03 06:08:44 -0800 (Fri, 03 Nov 2006)
Log Message:
-----------
Applied Tuan Kuranes' performance patch #1583115.
Modified Paths:
--------------
trunk/ode/src/heightfield.cpp
trunk/ode/src/heightfield.h
Modified: trunk/ode/src/heightfield.cpp
===================================================================
--- trunk/ode/src/heightfield.cpp 2006-11-03 13:43:36 UTC (rev 1128)
+++ trunk/ode/src/heightfield.cpp 2006-11-03 14:08:44 UTC (rev 1129)
@@ -18,8 +18,8 @@
#include "collision_trimesh_internal.h"
#endif // dTRIMESH_ENABLED
-#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
-#define MAXCONTACT 10
+//#define CONTACT(p, skip) ((dContactGeom*) (((char*)p) + (skip)))
+#define HEIGHTFIELDMAXCONTACTPERCELL 10
#define TERRAINTOL 0.0f
#if _MSC_VER <= 1200
@@ -38,13 +38,29 @@
//////// Local Build Option ////////////////////////////////////////////////////
// Uncomment this #define to use the (0,0) corner of the geom as the origin,
-// rather than the centre. This was the way the original heightfield worked,
-// but as it does not match the way all other geoms work, so for constancy it
+// rather than the center. This was the way the original heightfield worked,
+// but as it does not match the way all other geometries work, so for constancy it
// was changed to work like this.
// #define DHEIGHTFIELD_CORNER_ORIGIN
+// use optimised version of colliding against a terrain triangles
+#define _PLANECONSTRUCTIONOPTIMISATION
+// No clue about why colliding against TRIANGLE_BORDER is that important ?
+// It's really slowing down the thing
+// and not proven to be necessary in all tests I could do.
+#define HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+
+#ifdef HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+
+// If colliding against TRIANGLE_BORDER ray,
+// recompute normal using neighbor grid cells..
+// to build larger triangles to get normals from
+#define RECOMPUTE_RAYNORMAL
+
+#endif //HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+
//////// dxHeightfieldData /////////////////////////////////////////////////////////////
// dxHeightfieldData constructor
@@ -184,29 +200,42 @@
// returns whether point is over terrain?
-bool dxHeightfieldData::IsOnHeightfield( int nx, int nz, int w, dReal *pos )
+bool dxHeightfieldData::IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const
{
- dVector3 Min,Max;
- Min[0] = nx * m_fSampleWidth;
- Min[2] = nz * m_fSampleDepth;
- Max[0] = (nx+1) * m_fSampleWidth;
- Max[2] = (nz+1) * m_fSampleDepth;
- dReal TolX = m_fSampleWidth * TERRAINTOL;
- dReal TolZ = m_fSampleDepth * TERRAINTOL;
+ {
+ const dReal MaxX = CellOrigin[0] + m_fSampleWidth;
+ const dReal TolX = m_fSampleWidth * TERRAINTOL;
+ if ((pos[0]<CellOrigin[0]-TolX) || (pos[0]>MaxX+TolX))
+ return false;
+ }
- if ((pos[0]<Min[0]-TolX) || (pos[0]>Max[0]+TolX)) return false;
- if ((pos[2]<Min[2]-TolZ) || (pos[2]>Max[2]+TolZ)) return false;
+ {
+ const dReal MaxZ = CellOrigin[2] + m_fSampleDepth;
+ const dReal TolZ = m_fSampleDepth * TERRAINTOL;
+ if ((pos[2]<CellOrigin[2]-TolZ) || (pos[2]>MaxZ+TolZ))
+ return false;
+ }
- dReal dx = (pos[0] - (dReal(nx) * m_fSampleWidth)) / m_fSampleWidth;
- dReal dz = (pos[2] - (dReal(nz) * m_fSampleDepth)) / m_fSampleDepth;
+ // add X percentage position on cell and Z percentage position on cell
+ const dReal pctTotal = (pos[0] - CellOrigin[0]) / m_fSampleWidth
+ + (pos[2] - CellOrigin[2]) / m_fSampleDepth;
- if ((w == 0) && (dx + dz > 1.f+TERRAINTOL)) return false;
- if ((w == 1) && (dx + dz < 1.f-TERRAINTOL)) return false;
-
+ if (isABC)
+ {
+ if (pctTotal >= 1.0 + TERRAINTOL)
+ return false;
+ else
+ return true;
+ }
+ else if (pctTotal <= 1.0 - TERRAINTOL)
+ {
+ return false;
+ }
return true;
}
+
// returns height at given sample coordinates
dReal dxHeightfieldData::GetHeight( int x, int z )
{
@@ -279,9 +308,10 @@
dReal dx = ( x - ( dReal( nX ) * m_fSampleWidth ) ) / m_fSampleWidth;
dReal dz = ( z - ( dReal( nZ ) * m_fSampleDepth ) ) / m_fSampleDepth;
- dIASSERT( ( dx + dEpsilon >= 0.0f ) && ( dx - dEpsilon <= 1.0f ) );
- dIASSERT( ( dz + dEpsilon >= 0.0f ) && ( dz - dEpsilon <= 1.0f ) );
+ //dIASSERT( ( dx + dEpsilon >= 0.0f ) && ( dx - dEpsilon <= 1.0f ) );
+ //dIASSERT( ( dz + dEpsilon >= 0.0f ) && ( dz - dEpsilon <= 1.0f ) );
+
dReal y, y0;
if ( dx + dz < REAL( 1.0 ) )
@@ -362,6 +392,8 @@
{
type = dHeightfieldClass;
this->m_p_data = data;
+ this->sliding_plane = new dxPlane(0, 0, 0, 0, 0);
+ dGeomDisable (sliding_plane);
}
@@ -477,7 +509,7 @@
// dxHeightfield destructor
dxHeightfield::~dxHeightfield()
{
- //
+ delete sliding_plane;//
}
@@ -492,7 +524,7 @@
void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d,
void* pUserData, dHeightfieldGetHeight* pCallback,
- dReal width, dReal depth, int widthSamples, int depthSamples,
+ dReal width, dReal depth, int widthSamples, int depthSamples,
dReal scale, dReal offset, dReal thickness, int bWrap )
{
dUASSERT( d, "argument not Heightfield data" );
@@ -701,9 +733,8 @@
// Typdef for generic 'get point depth' function
typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z );
+
-#define RECOMPUTE_RAYNORMAL
-
#define DMESS(A) \
dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
x,z,A, \
@@ -716,6 +747,480 @@
pContact->normal[1], \
pContact->normal[2]);
+
+int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
+ dxGeom* o2, const int numMaxContacts,
+ int flags, dContactGeom* contact,
+ int skip )
+{
+
+ // get All Planes that could collide against.
+ dColliderFn *geomRayNCollider;
+ dColliderFn *geomNPlaneCollider;
+ dGetDepthFn *geomNDepthGetter;
+
+ switch (o2->type)
+ {
+ case dRayClass:
+ geomRayNCollider = NULL;
+ geomNPlaneCollider = dCollideRayPlane;
+ geomNDepthGetter = NULL;
+ break;
+
+ case dSphereClass:
+ geomRayNCollider = dCollideRaySphere;
+ geomNPlaneCollider = dCollideSpherePlane;
+ geomNDepthGetter = dGeomSpherePointDepth;
+ break;
+
+ case dBoxClass:
+ geomRayNCollider = dCollideRayBox;
+ geomNPlaneCollider = dCollideBoxPlane;
+ geomNDepthGetter = dGeomBoxPointDepth;
+ break;
+
+ case dCapsuleClass:
+ geomRayNCollider = dCollideRayCapsule;
+ geomNPlaneCollider = dCollideCapsulePlane;
+ geomNDepthGetter = dGeomCapsulePointDepth;
+ break;
+
+ case dCylinderClass:
+ geomRayNCollider = dCollideRayCylinder;
+ geomNPlaneCollider = dCollideCylinderPlane;
+ geomNDepthGetter = NULL;// TODO: dGeomCylinderPointDepth;
+ break;
+
+ case dConvexClass:
+ geomRayNCollider = dCollideRayConvex;
+ geomNPlaneCollider = dCollideConvexPlane;
+ geomNDepthGetter = NULL;// TODO: dGeomConvexPointDepth;
+ break;
+
+#if dTRIMESH_ENABLED
+
+ case dTriMeshClass:
+ geomRayNCollider = dCollideRayTrimesh;
+ geomNPlaneCollider = dCollideTrimeshPlane;
+ geomNDepthGetter = NULL;// N/A?
+ break;
+
+#endif // dTRIMESH_ENABLED
+
+ default:
+ dIASSERT(0); // Shouldn't ever get here.
+ break;
+
+ }
+
+
+
+
+ dReal Plane[4];
+ dVector3 A,B,C,D;
+ dVector3 BD,CD,BC,AB,AC;
+ int numTerrainContacts = 0;
+ dGeomEnable (sliding_plane);
+ int i;
+ dContactGeom *pContact = 0;
+
+ //dContactGeom *PlaneContact = new dContactGeom[numMaxContacts];
+ //dContactGeom *PlaneContact = (dContactGeom*)new char[numMaxContacts*skip];
+ //flags = (flags & 0xffff0000) | numMaxContacts;
+
+ dContactGeom *PlaneContact = new dContactGeom[HEIGHTFIELDMAXCONTACTPERCELL];
+ flags = (flags & 0xffff0000) | HEIGHTFIELDMAXCONTACTPERCELL;
+
+ // localize and const for faster access
+ const dReal cfSampleWidth = m_p_data->m_fSampleWidth;
+ const dReal cfSampleDepth = m_p_data->m_fSampleDepth;
+
+#ifdef HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+ const bool isColliderRayEnabled = geomRayNCollider != NULL;
+ int nA[3],nB[3];
+ dContactGeom ContactA[3],ContactB[3], ContactV;
+ dxRay tempRay(0, 1);
+ // taking advantage of grid square,
+ // so those length always the same
+ const dReal lWidthSq = cfSampleWidth*cfSampleWidth;
+ const dReal lDepthSq = cfSampleDepth*cfSampleDepth;
+ const dReal lDiagSq = lWidthSq + lDepthSq;
+
+ const dReal lDiagDblDepthSq = isColliderRayEnabled ? lWidthSq + 4*lDepthSq : 0.0f;
+ const dReal lDiagDblWidthSq = isColliderRayEnabled ? 4*lWidthSq + lDepthSq : 0.0f;
+ // if CollideRayN...
+ #ifdef RECOMPUTE_RAYNORMAL
+ dVector3 E,F;
+ dVector3 CE,FB,AD;
+ dVector3 Normal[3];
+ #endif
+#endif //HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+
+ /*
+ (y is up)
+
+ A-B-E.x
+ |/|
+ C-D
+ |
+ F
+ .
+ z
+ */
+
+ C[0] = minX*cfSampleWidth - cfSampleWidth;
+ for ( int x = minX; x < maxX; ++x )
+ {
+ // Re-use previous results to set A,B
+ // using CD in next loop
+ // so we compute CD once here at start,
+ // as if it was AB.
+
+ // y values
+ C[1] = m_p_data->GetHeight(x, minZ);
+ D[1] = m_p_data->GetHeight(x+1, minZ);
+
+ C[2] = minZ*cfSampleDepth;
+ D[2] = C[2];
+
+ // x values
+ // those are set once as they do not move in next loop
+ C[0] += cfSampleWidth;
+ D[0] = C[0] + cfSampleWidth;
+ A[0] = C[0];
+ B[0] = D[0];
+
+ // calculate CD once at start
+ // then reuse previous normalized CD
+ dOP(CD,-,D,C);
+
+#ifdef RECOMPUTE_RAYNORMAL
+ if (isColliderRayEnabled)
+ F[1] = m_p_data->GetHeight(x, minZ+1);
+#endif // RECOMPUTE_RAYNORMAL
+
+ for ( int z = minZ; z < maxZ; ++z )
+ {
+ // AB = previous CD
+ // [0] doesn't change as it's X
+ A[2] = C[2];
+ B[2] = D[2];
+
+ A[1] = C[1];
+ B[1] = D[1];
+
+ // CD advance
+ C[2] = C[2] + cfSampleDepth;
+ D[2] = C[2];
+#ifdef RECOMPUTE_RAYNORMAL
+ C[1] = isColliderRayEnabled ? F[1] : m_p_data->GetHeight(x, z+1);
+#else
+ C[1] = m_p_data->GetHeight(x, z+1);
+#endif // RECOMPUTE_RAYNORMAL
+ D[1] = m_p_data->GetHeight(x+1,z+1);
+
+ // compute 4 Vector of ABCD square
+
+ dOP(BD,-,D,B);
+
+ dOP(AC,-,C,A);
+
+ // using previous result
+ // as AB now was CD in previous loop
+ // AB <= CD
+ dVector3Copy(CD, AB);
+
+ dOP(CD,-,D,C);
+
+ // compute Diagonal Vector
+ dOP(BC,-,C,B);
+
+
+ // First plane.
+ // PLANE ABC
+ dIASSERT (numTerrainContacts != numMaxContacts );
+ {
+ dCROSS(Plane,=,AC,AB);
+
+ //dNormalize3 (Plane);
+ const dReal dlength = dRecipSqrt(dLENGTHSQUARED(Plane));
+ Plane[0] *= dlength;
+ Plane[1] *= dlength;
+ Plane[2] *= dlength;
+
+ Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
+
+ // PLANE ABC
+ // should call that, but it renormalizes the normal once again (uselessly costly)
+ //dGeomPlaneSetParams (sliding_plane, Plane[0],Plane[1],Plane[2],Plane[3]);
+
+ sliding_plane->p[0] = Plane[0];
+ sliding_plane->p[1] = Plane[1];
+ sliding_plane->p[2] = Plane[2];
+ sliding_plane->p[3] = Plane[3];
+ dGeomMoved (sliding_plane);
+
+ //equivalent to that, but saving creation and deletion
+ //dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
+
+ const int numPlaneContacts = geomNPlaneCollider(o2,sliding_plane,flags,PlaneContact,sizeof(dContactGeom));
+
+ for ( i = 0; i < numPlaneContacts; i++ )
+ {
+ const dVector3 &pCPos = PlaneContact[i].pos;
+ if ( m_p_data->IsOnHeightfield( A, pCPos, true) )
+ {
+ pContact = CONTACT(contact,numTerrainContacts*skip);
+
+ pContact->pos[0] = pCPos[0];
+ pContact->pos[1] = pCPos[1];
+ pContact->pos[2] = pCPos[2];
+
+ //why this is inverted ?
+ 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;
+
+ numTerrainContacts++;
+
+ if ( numTerrainContacts == numMaxContacts )
+ break;
+ }
+ }
+
+ if (numTerrainContacts == numMaxContacts )
+ break;
+ }
+ // Second plane.
+ // PLANE DCB
+ dIASSERT (numTerrainContacts != numMaxContacts );
+ {
+ dCROSS(Plane,=,BD,CD);
+
+ //dNormalize3 (Plane);
+ const dReal dlength = dRecipSqrt(dLENGTHSQUARED(Plane));
+ Plane[0] *= dlength;
+ Plane[1] *= dlength;
+ Plane[2] *= dlength;
+
+ Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
+
+ // should call that, but it renormalizes the normal once again (uselessly costly)
+ //dGeomPlaneSetParams (sliding_plane, Plane[0],Plane[1],Plane[2],Plane[3]);
+
+ sliding_plane->p[0] = Plane[0];
+ sliding_plane->p[1] = Plane[1];
+ sliding_plane->p[2] = Plane[2];
+ sliding_plane->p[3] = Plane[3];
+ dGeomMoved (sliding_plane);
+
+ //equivalent to that, but saving creation and deletion
+ //dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
+
+ const int numPlaneContacts = geomNPlaneCollider(o2,sliding_plane,flags,PlaneContact,sizeof(dContactGeom));
+
+ for ( i = 0; i < numPlaneContacts; i++ )
+ {
+ const dVector3 &pCPos = PlaneContact[i].pos;
+ if ( m_p_data->IsOnHeightfield( A, pCPos, false) )
+ {
+ pContact = CONTACT(contact,numTerrainContacts*skip);
+
+ pContact->pos[0] = pCPos[0];
+ pContact->pos[1] = pCPos[1];
+ pContact->pos[2] = pCPos[2];
+
+ //why this is inverted ?
+ 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;
+
+ numTerrainContacts++;
+
+ if ( numTerrainContacts == numMaxContacts )
+ break;
+ }
+ }
+
+ if ( numTerrainContacts == numMaxContacts )
+ break;
+ }
+
+#ifdef HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+ dIASSERT (numTerrainContacts != numMaxContacts );
+ if ( isColliderRayEnabled)
+ {
+
+#ifdef RECOMPUTE_RAYNORMAL
+
+ E[0] = B[0] + cfSampleWidth;
+ E[1] = m_p_data->GetHeight(x+2,z);
+ E[2] = A[2];
+
+ F[0] = A[0];
+ F[1] = m_p_data->GetHeight(x,z+2);
+ F[2] = C[2] + cfSampleDepth;
+
+ dOP(AD,-,D,A);
+ //dOPEC(AD, /=, dSqrt(lDiagSq + AD[1]*AD[1]));
+ //dNormalize3(AD);
+
+ dOP(CE,-,E,C);
+ //dOPEC(CE, /=, dSqrt(lDiagDblWidthSq + CE[1]*CE[1]));
+ //dNormalize3(CE);
+
+ dOP(FB,-,B,F);
+ //dOPEC(FB, /=, dSqrt(lDiagDblDepthSq + FB[1]*FB[1]));
+ //dNormalize3(FB);
+
+
+#endif
+
+
+
+#define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) { \
+ \
+ dVector3Copy (MyPoint, myRay.final_posr->pos); \
+ myRay.final_posr->R[0*4+2] = MyVector[0]; \
+ myRay.final_posr->R[1*4+2] = MyVector[1]; \
+ myRay.final_posr->R[2*4+2] = MyVector[2]; \
+ dGeomMoved (&myRay); \
+ };
+
+#define NUM_RAYCHECK 3
+
+#ifdef RECOMPUTE_RAYNORMAL
+ //BC
+ dCROSS(Normal[0],=,BC,AD);
+ dNormalize3(Normal[0]);
+#endif
+ // ray BC and CB
+ const dReal lBC = dSqrt(lDiagSq + BC[1]*BC[1]);
+ tempRay.length = lBC;
+ dGeomRaySet(&tempRay, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
+ //dGeomRaySetNoNormalize(tempRay, B, BC);
+ nA[0] = geomRayNCollider(&tempRay,o2,flags,&ContactA[0],sizeof(dContactGeom));
+ //dGeomRaySet(&tempRay, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
+ dGeomRaySetNoNormalize(tempRay, C, -BC);
+ nB[0] = geomRayNCollider(&tempRay,o2,flags,&ContactB[0],sizeof(dContactGeom));
+
+ if (NUM_RAYCHECK >= 2)
+ {
+
+#ifdef RECOMPUTE_RAYNORMAL
+ //BD
+ dCROSS(Normal[1],=,BD,CE);
+ dNormalize3(Normal[1]);
+#endif
+
+
+
+ const dReal lBD = dSqrt(lDepthSq + BD[1]*BD[1]);
+ // ray BD and DB
+ tempRay.length = lBD;
+ dGeomRaySet(&tempRay, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
+ //dGeomRaySetNoNormalize(tempRay, B, BD);
+ nA[1] = geomRayNCollider(&tempRay,o2,flags,&ContactA[1],sizeof(dContactGeom));
+ //dGeomRaySet(&tempRay, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
+ dGeomRaySetNoNormalize(tempRay, D, -BD);
+ nB[1] = geomRayNCollider(&tempRay,o2,flags,&ContactB[1],sizeof(dContactGeom));
+
+ }
+ if (NUM_RAYCHECK >= 3)
+ {
+#ifdef RECOMPUTE_RAYNORMAL
+ //CD
+ dCROSS(Normal[2],=,CD,FB);
+ dNormalize3(Normal[2]);
+#endif
+
+ const dReal lCD = dSqrt(lWidthSq + CD[1]*CD[1]);
+ // ray CD and DC
+ tempRay.length = lCD;
+ dGeomRaySet(&tempRay, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
+ //dGeomRaySetNoNormalize(tempRay, C, CD);
+ nA[2] = geomRayNCollider(&tempRay,o2,flags,&ContactA[2],sizeof(dContactGeom));
+ //dGeomRaySet(&tempRay, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
+ dGeomRaySetNoNormalize(tempRay, D, -CD);
+ nB[2] = geomRayNCollider(&tempRay,o2,flags,&ContactB[2],sizeof(dContactGeom));
+ }
+
+ for ( i = 0; i < NUM_RAYCHECK; i++ )
+ {
+ if ( nA[i] & nB[i] )
+ {
+ pContact = CONTACT( contact,numTerrainContacts*skip );
+
+ pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0]) * 0.5f;
+ pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1]) * 0.5f;
+ pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2]) * 0.5f;
+
+#ifdef RECOMPUTE_RAYNORMAL
+
+ pContact->normal[0] = -Normal[i][0];
+ pContact->normal[1] = -Normal[i][1];
+ pContact->normal[2] = -Normal[i][2];
+
+#else // RECOMPUTE_RAYNORMAL
+
+ pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0]) * 0.5f;
+ pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1]) * 0.5f;
+ pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2]) * 0.5f;
+ dNormalize3(pContact->normal);
+
+#endif // RECOMPUTE_RAYNORMAL
+
+
+
+ //
+ // Find Contact Penetration Depth
+ //
+
+ if ( geomNDepthGetter )
+ {
+ pContact->depth = geomNDepthGetter( o2,
+ pContact->pos[0], pContact->pos[1], pContact->pos[2] );
+ numTerrainContacts++;
+ }
+ else
+ {
+ // We don't have a GetDepth function, so do a ray cast instead.
+ // NOTE: This isn't ideal, and a GetDepth function should be
+ // written for all geom classes.
+ tempRay.length = 1000.f;
+
+ //dGeomRaySet( &tempRay, pContact->pos[0], pContact->pos[1], pContact->pos[2],
+ // -pContact->normal[0], -pContact->normal[1], -pContact->normal[2] );
+ dGeomRaySetNoNormalize(tempRay, pContact->pos, -pContact->normal);
+
+ if ( geomRayNCollider( &tempRay, o2, flags, &ContactV, sizeof( dContactGeom ) ) )
+ {
+ pContact->depth = ContactV.depth;
+ numTerrainContacts++;
+ }
+ }
+
+ if ( numTerrainContacts == numMaxContacts )
+ break;
+
+ }
+ }
+ if ( numTerrainContacts == numMaxContacts )
+ break;
+ }
+#endif //HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+ }
+ if (numTerrainContacts == numMaxContacts )
+ break;
+ }
+ dGeomDisable (sliding_plane);
+ //delete [] PlaneContact;
+ return numTerrainContacts;
+}
/*
(y is up)
@@ -727,7 +1232,6 @@
.
z
*/
-
int dxHeightfield::dCollideHeightfieldUnit( int x, int z, dxGeom* o2, int numMaxContacts,
int flags, dContactGeom* contact, int skip )
{
@@ -741,8 +1245,8 @@
if ( numContacts == numMaxContacts )
return numContacts;
- dContactGeom PlaneContact[MAXCONTACT];
- flags = (flags & 0xffff0000) | MAXCONTACT;
+ dContactGeom PlaneContact[HEIGHTFIELDMAXCONTACTPERCELL];
+ flags = (flags & 0xffff0000) | HEIGHTFIELDMAXCONTACTPERCELL;
switch (o2->type)
{
@@ -798,6 +1302,11 @@
break;
}
+
+#ifndef HEIGHTFIELD_TRIANGLE_BORDER_AS_RAY
+ CollideRayN = NULL;
+#endif
+
dReal Plane[4],lBD,lCD,lBC;
dVector3 A,B,C,D,BD,CD,BC,AB,AC;
A[0] = x * m_p_data->m_fSampleWidth;
@@ -817,192 +1326,193 @@
D[1] = m_p_data->GetHeight(x+1,z+1);
dOP(BC,-,C,B);
- lBC = dLENGTH(BC);
- dOPEC(BC,/=,lBC);
+ //dOPEC(BC,/=,lBC);
- dOP(BD,-,D,B);
- lBD = dLENGTH(BD);
- dOPEC(BD,/=,lBD);
+ dOP(BD,-,D,B);
+ //dOPEC(BD,/=,lBD);
dOP(CD,-,D,C);
- lCD = dLENGTH(CD);
- dOPEC(CD,/=,lCD);
+ //dOPEC(CD,/=,lCD);
dOP(AB,-,B,A);
- dNormalize3(AB);
+ //dNormalize3(AB);
dOP(AC,-,C,A);
- dNormalize3(AC);
+ //dNormalize3(AC);
- if ( CollideRayN )
- {
+ 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));
-#ifdef RECOMPUTE_RAYNORMAL
+ for ( i = 0; i < numPlaneContacts; i++ )
+ {
+ if ( m_p_data->IsOnHeightfield( A, PlaneContact[i].pos, true) )
+ {
+ 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;
- dVector3 E,F;
- dVector3 CE,FB,AD;
- dVector3 Normal[3];
- E[0] = (x+2) * m_p_data->m_fSampleWidth;
- E[2] = z * m_p_data->m_fSampleDepth;
- E[1] = m_p_data->GetHeight(x+2,z);
- F[0] = x * m_p_data->m_fSampleWidth;
- F[2] = (z+2) * m_p_data->m_fSampleDepth;
- F[1] = m_p_data->GetHeight(x,z+2);
- dOP(AD,-,D,A);
- dNormalize3(AD);
- dOP(CE,-,E,C);
- dNormalize3(CE);
- dOP(FB,-,B,F);
- dNormalize3(FB);
+ numContacts++;
- //BC
- dCROSS(Normal[0],=,BC,AD);
- dNormalize3(Normal[0]);
+ if ( numContacts == numMaxContacts )
+ return numContacts;
+ }
+ }
- //BD
- dCROSS(Normal[1],=,BD,CE);
- dNormalize3(Normal[1]);
+ 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));
- //CD
- dCROSS(Normal[2],=,CD,FB);
- dNormalize3(Normal[2]);
+ for ( i = 0; i < numPlaneContacts; i++ )
+ {
+ if ( m_p_data->IsOnHeightfield( A, PlaneContact[i].pos, false ) )
+ {
+ 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;
-#endif
+ numContacts++;
- 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));
+ if ( numContacts == numMaxContacts )
+ return numContacts;
+ }
+ }
- 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));
+ if ( CollideRayN && numContacts == 0)
+ {
- 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));
+#ifdef RECOMPUTE_RAYNORMAL
- for ( i = 0; i < 3; i++ )
- {
- if ( nA[i] & nB[i] )
- {
- dContactGeom *pContact = CONTACT( contact,numContacts*skip );
+ dVector3 E,F;
+ dVector3 CE,FB,AD;
+ dVector3 Normal[3];
+ E[0] = (x+2) * m_p_data->m_fSampleWidth;
+ E[2] = z * m_p_data->m_fSampleDepth;
+ E[1] = m_p_data->GetHeight(x+2,z);
+ F[0] = x * m_p_data->m_fSampleWidth;
+ F[2] = (z+2) * m_p_data->m_fSampleDepth;
+ F[1] = m_p_data->GetHeight(x,z+2);
+ dOP(AD,-,D,A);
+ //dNormalize3(AD);
+ dOP(CE,-,E,C);
+ //dNormalize3(CE);
+ dOP(FB,-,B,F);
+ //dNormalize3(FB);
- 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;
+ //BC
+ dCROSS(Normal[0],=,BC,AD);
+ dNormalize3(Normal[0]);
-#ifdef RECOMPUTE_RAYNORMAL
+ //BD
+ dCROSS(Normal[1],=,BD,CE);
+ dNormalize3(Normal[1]);
- pContact->normal[0] = -Normal[i][0];
- pContact->normal[1] = -Normal[i][1];
- pContact->normal[2] = -Normal[i][2];
+ //CD
+ dCROSS(Normal[2],=,CD,FB);
+ dNormalize3(Normal[2]);
-#else // RECOMPUTE_RAYNORMAL
+#endif
- pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2;
- pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2;
- pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2;
- dNormalize3(pContact->normal);
+ lCD = dLENGTH(CD);
+ lBC = dLENGTH(BC);
+ lBD = dLENGTH(BD);
-#endif // RECOMPUTE_RAYNORMAL
+ 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));
- //
- // Find Contact Penetration Depth
- //
+ for ( i = 0; i < 3; i++ )
+ {
+ if ( nA[i] & nB[i] )
+ {
+ dContactGeom *pContact = CONTACT( contact,numContacts*skip );
- if ( GetDepth )
- {
- pContact->depth = GetDepth( o2,
- pContact->pos[0], pContact->pos[1], pContact->pos[2] );
+ 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;
- numContacts++;
- }
- else
- {
- // We don't have a GetDepth function, so do a ray cast instead.
- // NOTE: This isn't ideal, and a GetDepth function should be
- // written for all geom classes.
- 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] );
+#ifdef RECOMPUTE_RAYNORMAL
- dContactGeom ContactV;
- if ( CollideRayN( &rayV, o2, flags, &ContactV, sizeof( dContactGeom ) ) )
- {
- pContact->depth = ContactV.depth;
- numContacts++;
- }
- }
+ pContact->normal[0] = -Normal[i][0];
+ pContact->normal[1] = -Normal[i][1];
+ pContact->normal[2] = -Normal[i][2];
- if (numContacts == numMaxContacts)
- return numContacts;
+#else // RECOMPUTE_RAYNORMAL
- }
- }
- }
+ pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2;
+ pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2;
+ pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2;
+ dNormalize3(pContact->normal);
- 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));
+#endif // RECOMPUTE_RAYNORMAL
- for ( i = 0; i < numPlaneContacts; i++ )
- {
- if ( m_p_data->IsOnHeightfield( x, z, 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;
- numContacts++;
- if ( numContacts == numMaxContacts )
- return numContacts;
- }
- }
+ //
+ // Find Contact Penetration Depth
+ //
- 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));
+ if ( GetDepth )
+ {
+ pContact->depth = GetDepth( o2,
+ pContact->pos[0], pContact->pos[1], pContact->pos[2] );
- for ( i = 0; i < numPlaneContacts; i++ )
- {
- if ( m_p_data->IsOnHeightfield( x, z, 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;
+ numContacts++;
+ }
+ else
+ {
+ // We don't have a GetDepth function, so do a ray cast instead.
+ // NOTE: This isn't ideal, and a GetDepth function should be
+ // written for all geom classes.
+ 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] );
- numContacts++;
+ dContactGeom ContactV;
+ if ( CollideRayN( &rayV, o2, flags, &ContactV, sizeof( dContactGeom ) ) )
+ {
+ pContact->depth = ContactV.depth;
+ numContacts++;
+ }
+ }
- if ( numContacts == numMaxContacts )
- return numContacts;
- }
- }
+ if (numContacts == numMaxContacts)
+ return numContacts;
+ }
+ }
+ }
+
return numContacts;
}
@@ -1011,7 +1521,7 @@
{
dIASSERT( skip >= (int)sizeof(dContactGeom) );
dIASSERT( o1->type == dHeightfieldClass );
- int i,j;
+ int i;
if ((flags & 0xffff) == 0)
flags = (flags & 0xffff0000) | 1;
@@ -1108,15 +1618,26 @@
}
}
+
+#ifdef _PLANECONSTRUCTIONOPTIMISATION
+
+ numTerrainContacts = terrain->dCollideHeightfieldZone(
+ nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts,
+ flags,CONTACT(contact,numTerrainContacts*skip),skip );
+
+#else //_PLANECONSTRUCTIONOPTIMISATION
+
+ int j;
// Collide against all potential collision cells.
for ( i = nMinX; i < nMaxX; ++i )
for ( j = nMinZ; j < nMaxZ; ++j )
- {
+ {
numTerrainContacts += terrain->dCollideHeightfieldUnit(
i,j,o2,numMaxTerrainContacts - numTerrainContacts,
flags,CONTACT(contact,numTerrainContacts*skip),skip );
}
+#endif //_PLANECONSTRUCTIONOPTIMISATION
dIASSERT( numTerrainContacts <= numMaxTerrainContacts );
for ( i = 0; i < numTerrainContacts; ++i )
Modified: trunk/ode/src/heightfield.h
===================================================================
--- trunk/ode/src/heightfield.h 2006-11-03 13:43:36 UTC (rev 1128)
+++ trunk/ode/src/heightfield.h 2006-11-03 14:08:44 UTC (rev 1129)
@@ -52,7 +52,8 @@
void ComputeHeightBounds();
- bool IsOnHeightfield(int nx, int nz, int w, dReal *pos);
+ bool IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const;
+
dReal GetHeight(int x, int z);
dReal GetHeight(dReal x, dReal z);
};
@@ -74,6 +75,13 @@
int dCollideHeightfieldUnit( int x, int z, dxGeom *o2, int numMaxContacts,
int flags, dContactGeom *contact, int skip );
+
+ int dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
+ dxGeom *o2, const int numMaxContacts,
+ int flags, dContactGeom *contact, int skip );
+
+ dxPlane *sliding_plane;
+
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|