--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/src/heightfield.cpp Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,1350 @@
+// dHeightfield Collider
+// Paul Cheyrou-Lagreze aka Tuan Kuranes 2006 Speed enhancements http://www.pop-3d.com
+// Martijn Buijs 2006 http://home.planet.nl/~buijs512/
+// Based on Terrain & Cone contrib by:
+// Benoit CHAPEROT 2003-2004 http://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_util.h"
+#include "heightfield.h"
+
+#define TERRAINTOL REAL(0.0f)
+
+#define dMIN(A,B) ((A)>(B) ? B : A)
+#define dMAX(A,B) ((A)>(B) ? A : B)
+
+
+// Three-way MIN and MAX
+#define dMIN3(A,B,C) ( (A)<(B) ? dMIN((A),(C)) : dMIN((B),(C)) )
+#define dMAX3(A,B,C) ( (A)>(B) ? dMAX((A),(C)) : dMAX((B),(C)) )
+
+#define dOPESIGN(a, op1, op2,b) \
+ (a)[0] op1 op2 ((b)[0]); \
+ (a)[1] op1 op2 ((b)[1]); \
+ (a)[2] op1 op2 ((b)[2]);
+
+#define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) { \
+ \
+ dVector3Copy (MyPoint, myRay.final_posr->pos); \
+ myRay.final_posr->R[2] = MyVector[0]; \
+ myRay.final_posr->R[6] = MyVector[1]; \
+ myRay.final_posr->R[10] = MyVector[2]; \
+ dGeomMoved (&myRay); \
+ }
+
+#define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \
+ \
+ MyPlane->p[0] = MyPlaneDef[0]; \
+ MyPlane->p[1] = MyPlaneDef[1]; \
+ MyPlane->p[2] = MyPlaneDef[2]; \
+ MyPlane->p[3] = MyPlaneDef[3]; \
+ dGeomMoved (MyPlane); \
+ }
+//////// dxHeightfieldData /////////////////////////////////////////////////////////////
+
+// dxHeightfieldData constructor
+dxHeightfieldData::dxHeightfieldData()
+{
+ //
+}
+
+
+// build Heightfield data
+void dxHeightfieldData::SetData( int nWidthSamples, int nDepthSamples,
+ dReal fWidth, dReal fDepth,
+ dReal fScale, dReal fOffset, dReal fThickness,
+ int bWrapMode )
+{
+
+
+ // x,z bounds
+ m_fWidth = fWidth;
+ m_fDepth = fDepth;
+
+ // cache half x,z bounds
+ m_fHalfWidth = dDIV(fWidth,REAL( 2.0 ));
+ m_fHalfDepth = dDIV(fDepth,REAL( 2.0 ));
+
+ // scale and offset
+ m_fScale = fScale;
+ m_fOffset = fOffset;
+
+ // infinite min height bounds
+ m_fThickness = fThickness;
+
+ // number of vertices per side
+ m_nWidthSamples = nWidthSamples;
+ m_nDepthSamples = nDepthSamples;
+
+ m_fSampleWidth = m_fWidth / ( m_nWidthSamples - 1 );
+ m_fSampleDepth = m_fDepth / ( m_nDepthSamples - 1 );
+
+ m_fInvSampleWidth = dDIV(REAL(1.0),m_fSampleWidth);
+ m_fInvSampleDepth = dDIV(REAL(1.0),m_fSampleDepth);
+
+ // finite or repeated terrain?
+ m_bWrapMode = bWrapMode;
+}
+
+
+// recomputes heights bounds
+void dxHeightfieldData::ComputeHeightBounds()
+{
+ static int i;
+ static dReal h;
+ static dReal *data_float;
+
+ data_float = (dReal*)m_pHeightData;
+ m_fMinHeight = dInfinity;
+ m_fMaxHeight = -dInfinity;
+
+ for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
+ {
+ h = data_float[i];
+ if (h < m_fMinHeight) m_fMinHeight = h;
+ if (h > m_fMaxHeight) m_fMaxHeight = h;
+ }
+
+ // scale and offset
+ m_fMinHeight = dMUL(m_fMinHeight,m_fScale);
+ m_fMaxHeight = dMUL(m_fMaxHeight,m_fScale);
+ m_fMinHeight += m_fOffset;
+ m_fMaxHeight += m_fOffset;
+
+ // add thickness
+ m_fMinHeight -= m_fThickness;
+}
+
+
+// returns whether point is over terrain Cell triangle?
+bool dxHeightfieldData::IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const
+{
+ {
+ const dReal MaxX = CellOrigin[0] + m_fSampleWidth;
+ const dReal TolX = dMUL(m_fSampleWidth,TERRAINTOL);
+ if ((pos[0]<CellOrigin[0]-TolX) || (pos[0]>MaxX+TolX))
+ return false;
+ }
+
+ {
+ const dReal MaxZ = CellOrigin[2] + m_fSampleDepth;
+ const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL);
+ if ((pos[2]<CellOrigin[2]-TolZ) || (pos[2]>MaxZ+TolZ))
+ return false;
+ }
+
+ // add X percentage position on cell and Z percentage position on cell
+ const dReal pctTotal = dMUL((pos[0] - CellOrigin[0]),m_fInvSampleWidth )
+ + dMUL((pos[2] - CellOrigin[2]),m_fInvSampleDepth);
+
+ if (isABC)
+ {
+ if (pctTotal >= REAL(1.0) + TERRAINTOL)
+ return false;
+ else
+ return true;
+ }
+ else if (pctTotal <= REAL(1.0) - TERRAINTOL)
+ {
+ return false;
+ }
+ return true;
+}
+// returns whether point is over terrain Cell triangle?
+bool dxHeightfieldData::IsOnHeightfield2 ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const
+{
+ dReal MaxX, MinX;
+ dReal MaxZ, MinZ;
+ if (isABC)
+ {
+ // point A
+ MinX = CellOrigin[0];
+ MaxX = CellOrigin[0] + m_fSampleWidth;
+
+ MinZ = CellOrigin[2];
+ MaxZ = CellOrigin[2] + m_fSampleDepth;
+ }
+ else
+ {
+ // point D
+ MinX = CellOrigin[0] - m_fSampleWidth;
+ MaxX = CellOrigin[0];
+
+ MinZ = CellOrigin[2] - m_fSampleDepth;
+ MaxZ = CellOrigin[2];
+ }
+
+ // check if inside CELL
+ {
+ const dReal TolX = dMUL(m_fSampleWidth,TERRAINTOL);
+ if ((pos[0]<MinX-TolX) || (pos[0]>MaxX+TolX))
+ return false;
+ }
+
+ {
+ const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL);
+ if ((pos[2]<MinZ-TolZ) || (pos[2]>MaxZ+TolZ))
+ return false;
+ }
+
+ // Sum up X percentage position on cell and Z percentage position on cell
+ const dReal pctTotal = dMUL((pos[0] - MinX),m_fInvSampleWidth)
+ + dMUL((pos[2] - MinZ),m_fInvSampleDepth);
+
+ // check if inside respective Triangle of Cell
+ if (isABC)
+ {
+ if (pctTotal >= REAL(1.0) + TERRAINTOL)
+ return false;
+ else
+ return true;
+ }
+ else if (pctTotal <= REAL(1.0) - TERRAINTOL)
+ {
+ return false;
+ }
+ return true;
+}
+
+
+// returns height at given sample coordinates
+dReal dxHeightfieldData::GetHeight( int x, int z )
+{
+ static dReal h;
+ static dReal *data_float;
+
+ if ( m_bWrapMode == 0 )
+ {
+ // Finite
+ if ( x < 0 ) x = 0;
+ if ( z < 0 ) z = 0;
+ if ( x > m_nWidthSamples - 1 ) x = m_nWidthSamples - 1;
+ if ( z > m_nDepthSamples - 1 ) z = m_nDepthSamples - 1;
+ }
+ else
+ {
+ // Infinite
+ x %= m_nWidthSamples - 1;
+ z %= m_nDepthSamples - 1;
+ if ( x < 0 ) x += m_nWidthSamples - 1;
+ if ( z < 0 ) z += m_nDepthSamples - 1;
+ }
+
+ data_float = (dReal*)m_pHeightData;
+ h = data_float[x+(z * m_nWidthSamples)];
+
+ return dMUL(h,m_fScale) + m_fOffset;
+}
+
+
+// returns height at given coordinates
+dReal dxHeightfieldData::GetHeight( dReal x, dReal z )
+{
+ int nX = dMUL( x,m_fInvSampleWidth )>>QFACTOR;
+ int nZ = dMUL( z,m_fInvSampleDepth )>>QFACTOR;
+
+ dReal dx = dMUL(( x - ( nX * m_fSampleWidth ) ),m_fInvSampleWidth);
+ dReal dz = dMUL(( z - ( nZ * m_fSampleDepth ) ),m_fInvSampleDepth);
+
+
+
+ dReal y, y0;
+
+ if ( dx + dz < REAL( 1.0 ) )
+ {
+ y0 = GetHeight( nX, nZ );
+
+ y = y0 + dMUL(( GetHeight( nX + 1, nZ ) - y0 ),dx)
+ + dMUL(( GetHeight( nX, nZ + 1 ) - y0 ),dz);
+ }
+ else
+ {
+ y0 = GetHeight( nX + 1, nZ + 1 );
+
+ y = y0 + dMUL(( GetHeight( nX + 1, nZ ) - y0 ),( REAL(1.0f) - dz )) +
+ dMUL(( GetHeight( nX, nZ + 1 ) - y0 ),( REAL(1.0f) - dx ));
+ }
+
+ return y;
+}
+
+
+// dxHeightfieldData destructor
+dxHeightfieldData::~dxHeightfieldData()
+{
+ static dReal *data_float;
+
+
+ if ( m_bCopyHeightData )
+ {
+ data_float = (dReal*)m_pHeightData;
+ delete [] data_float;
+ }
+}
+
+
+//////// dxHeightfield /////////////////////////////////////////////////////////////////
+
+
+// dxHeightfield constructor
+dxHeightfield::dxHeightfield( dSpaceID space,
+ dHeightfieldDataID data,
+ int bPlaceable ) :
+ dxGeom( space, bPlaceable ),
+ tempPlaneBuffer(0),
+ tempPlaneBufferSize(0),
+ tempTriangleBuffer(0),
+ tempTriangleBufferSize(0),
+ tempHeightBuffer(0),
+ tempHeightBufferSizeX(0),
+ tempHeightBufferSizeZ(0)
+{
+ type = dHeightfieldClass;
+ this->m_p_data = data;
+}
+
+
+// compute axis aligned bounding box
+void dxHeightfield::computeAABB()
+{
+ const dxHeightfieldData *d = m_p_data;
+
+ if ( d->m_bWrapMode == 0 )
+ {
+ // Finite
+ if ( gflags & GEOM_PLACEABLE )
+ {
+ dReal dx[6], dy[6], dz[6];
+
+ // Y-axis
+ dy[0] = dMUL( final_posr->R[ 1],d->m_fMinHeight );
+ dy[1] = dMUL( final_posr->R[ 5],d->m_fMinHeight );
+ dy[2] = dMUL( final_posr->R[ 9],d->m_fMinHeight );
+ dy[3] = dMUL( final_posr->R[ 1],d->m_fMaxHeight );
+ dy[4] = dMUL( final_posr->R[ 5],d->m_fMaxHeight );
+ dy[5] = dMUL( final_posr->R[ 9],d->m_fMaxHeight );
+
+
+ // X-axis
+ dx[0] = dMUL( final_posr->R[ 0],-d->m_fHalfWidth );
+ dx[1] = dMUL( final_posr->R[ 4],-d->m_fHalfWidth );
+ dx[2] = dMUL( final_posr->R[ 8],-d->m_fHalfWidth );
+ dx[3] = dMUL( final_posr->R[ 0],d->m_fHalfWidth );
+ dx[4] = dMUL( final_posr->R[ 4],d->m_fHalfWidth );
+ dx[5] = dMUL( final_posr->R[ 8],d->m_fHalfWidth );
+
+ // Z-axis
+ dz[0] = dMUL( final_posr->R[ 2],-d->m_fHalfDepth );
+ dz[1] = dMUL( final_posr->R[ 6],-d->m_fHalfDepth );
+ dz[2] = dMUL( final_posr->R[10],-d->m_fHalfDepth );
+ dz[3] = dMUL( final_posr->R[ 2],d->m_fHalfDepth );
+ dz[4] = dMUL( final_posr->R[ 6],d->m_fHalfDepth );
+ dz[5] = dMUL( final_posr->R[10],d->m_fHalfDepth );
+
+
+ // X extents
+ aabb[0] = final_posr->pos[0] +
+ dMIN3( dMIN( dx[0], dx[3] ), dMIN( dy[0], dy[3] ), dMIN( dz[0], dz[3] ) );
+ aabb[1] = final_posr->pos[0] +
+ dMAX3( dMAX( dx[0], dx[3] ), dMAX( dy[0], dy[3] ), dMAX( dz[0], dz[3] ) );
+
+ // Y extents
+ aabb[2] = final_posr->pos[1] +
+ dMIN3( dMIN( dx[1], dx[4] ), dMIN( dy[1], dy[4] ), dMIN( dz[1], dz[4] ) );
+ aabb[3] = final_posr->pos[1] +
+ dMAX3( dMAX( dx[1], dx[4] ), dMAX( dy[1], dy[4] ), dMAX( dz[1], dz[4] ) );
+
+ // Z extents
+ aabb[4] = final_posr->pos[2] +
+ dMIN3( dMIN( dx[2], dx[5] ), dMIN( dy[2], dy[5] ), dMIN( dz[2], dz[5] ) );
+ aabb[5] = final_posr->pos[2] +
+ dMAX3( dMAX( dx[2], dx[5] ), dMAX( dy[2], dy[5] ), dMAX( dz[2], dz[5] ) );
+ }
+ else
+ {
+
+ aabb[0] = -d->m_fHalfWidth; aabb[1] = +d->m_fHalfWidth;
+ aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
+ aabb[4] = -d->m_fHalfDepth; aabb[5] = +d->m_fHalfDepth;
+
+
+ }
+ }
+ else
+ {
+ // Infinite
+ 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] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
+ aabb[4] = -dInfinity; aabb[5] = +dInfinity;
+ }
+ }
+
+}
+
+
+// dxHeightfield destructor
+dxHeightfield::~dxHeightfield()
+{
+ delete [] tempTriangleBuffer;
+
+ for (unsigned int k = 0; k < tempPlaneBufferSize; k++)
+ {
+ delete tempPlaneBuffer[k];
+ }
+ delete [] tempPlaneBuffer;
+
+ resetHeightBuffer();
+}
+
+void dxHeightfield::resetHeightBuffer()
+{
+ const size_t xSize = tempHeightBufferSizeX;
+ for (size_t x = 0; xSize < x; x++)
+ {
+ delete [] tempHeightBuffer[x];
+ }
+ delete [] tempHeightBuffer;
+}
+//////// Heightfield data interface ////////////////////////////////////////////////////
+
+
+EXPORT_C dHeightfieldDataID dGeomHeightfieldDataCreate()
+{
+ return new dxHeightfieldData();
+}
+
+
+EXPORT_C void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d,
+ void* pUserData, dHeightfieldGetHeight* pCallback,
+ dReal width, dReal depth, int widthSamples, int depthSamples,
+ dReal scale, dReal offset, dReal thickness, int bWrap )
+{
+
+
+ // callback
+ d->m_nGetHeightMode = 0;
+ d->m_pUserData = pUserData;
+ d->m_pGetHeightCallback = pCallback;
+
+ // set info
+ d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
+
+ // default bounds
+ d->m_fMinHeight = -dInfinity;
+ d->m_fMaxHeight = dInfinity;
+}
+
+EXPORT_C void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d,
+ const float *pHeightData, int bCopyHeightData,
+ dReal width, dReal depth, int widthSamples, int depthSamples,
+ dReal scale, dReal offset, dReal thickness, int bWrap )
+{
+
+
+ // set info
+ d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
+ d->m_nGetHeightMode = 3;
+ d->m_bCopyHeightData = bCopyHeightData;
+
+ if ( d->m_bCopyHeightData == 0 )
+ {
+ // Data is referenced only.
+ d->m_pHeightData = pHeightData;
+ }
+ else
+ {
+ // We own the height data, allocate storage
+ d->m_pHeightData = new dReal[ d->m_nWidthSamples * d->m_nDepthSamples ];
+
+
+ // Copy data.
+ memcpy( (void*)d->m_pHeightData, pHeightData,
+ sizeof( dReal ) * d->m_nWidthSamples * d->m_nDepthSamples );
+ }
+
+ // Find height bounds
+ d->ComputeHeightBounds();
+}
+
+
+EXPORT_C void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight )
+{
+
+ d->m_fMinHeight = dMUL( minHeight,d->m_fScale ) + d->m_fOffset - d->m_fThickness;
+ d->m_fMaxHeight = dMUL( maxHeight,d->m_fScale ) + d->m_fOffset;
+}
+
+
+EXPORT_C void dGeomHeightfieldDataDestroy( dHeightfieldDataID d )
+{
+ delete d;
+}
+
+
+//////// Heightfield geom interface ////////////////////////////////////////////////////
+
+
+EXPORT_C dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable )
+{
+ return new dxHeightfield( space, data, bPlaceable );
+}
+
+
+EXPORT_C void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d )
+{
+ dxHeightfield* geom = (dxHeightfield*) g;
+ geom->data = d;
+}
+
+
+EXPORT_C dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g )
+{
+ dxHeightfield* geom = (dxHeightfield*) g;
+ return geom->m_p_data;
+}
+
+//////// dxHeightfield /////////////////////////////////////////////////////////////////
+
+
+// Typedef for generic 'get point depth' function
+typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z );
+
+
+
+static inline bool DescendingPlaneSort(const HeightFieldPlane * const A, const HeightFieldPlane * const B)
+{
+ return ((A->maxAAAB - B->maxAAAB) > dEpsilon);
+}
+
+void dxHeightfield::sortPlanes(const size_t numPlanes)
+{
+ bool has_swapped = true;
+ do
+ {
+ has_swapped = false;//reset flag
+ for (size_t i = 0; i < numPlanes - 1; i++)
+ {
+ //if they are in the wrong order
+ if (DescendingPlaneSort(tempPlaneBuffer[i], tempPlaneBuffer[i + 1]))
+ {
+ //exchange them
+ HeightFieldPlane * tempPlane = tempPlaneBuffer[i];
+ tempPlaneBuffer[i] = tempPlaneBuffer[i + 1];
+ tempPlaneBuffer[i + 1] = tempPlane;
+
+ //we have swapped at least once, list may not be sorted yet
+ has_swapped = true;
+ }
+ }
+ } //if no swaps were made during this pass, the list has been sorted
+ while (has_swapped);
+}
+
+int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
+ dxGeom* o2, const int numMaxContactsPossible,
+ int flags, dContactGeom* contact,
+ int skip )
+{
+ dContactGeom *pContact = 0;
+ int x, z;
+ // check if not above or inside terrain first
+ // while filling a heightmap partial temporary buffer
+ const unsigned int numX = (maxX - minX) + 1;
+ const unsigned int numZ = (maxZ - minZ) + 1;
+ const dReal minO2Height = o2->aabb[2];
+ const dReal maxO2Height = o2->aabb[3];
+ unsigned int x_local, z_local;
+ dReal maxY = - dInfinity;
+ dReal minY = dInfinity;
+ // localize and const for faster access
+ const dReal cfSampleWidth = m_p_data->m_fSampleWidth;
+ const dReal cfSampleDepth = m_p_data->m_fSampleDepth;
+ {
+ if (tempHeightBufferSizeX < numX || tempHeightBufferSizeZ < numZ)
+ {
+ resetHeightBuffer();
+ tempHeightBufferSizeX = numX;
+ tempHeightBufferSizeZ = numZ;
+ tempHeightBuffer = new HeightFieldVertex *[numX];
+ for ( x_local = 0; x_local < numX; x_local++)
+ {
+ tempHeightBuffer[x_local] = new HeightFieldVertex [numZ];
+ }
+ }
+
+ dReal Xpos, Ypos;
+ Xpos = minX*cfSampleWidth;
+
+
+ for ( x = minX, x_local = 0; x_local < numX; x++, x_local++)
+ {
+ const dReal c_Xpos = Xpos;
+ HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
+ Ypos = minZ * cfSampleDepth;
+ for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++)
+ {
+ const dReal h = m_p_data->GetHeight(x, z);
+ HeightFieldRow[z_local].vertex[0] = c_Xpos;
+ HeightFieldRow[z_local].vertex[1] = h;
+ HeightFieldRow[z_local].vertex[2] = Ypos;
+
+
+ maxY = dMAX(maxY, h);
+ minY = dMIN(minY, h);
+
+
+ Ypos += cfSampleDepth;
+ }
+ Xpos += cfSampleWidth;
+ }
+ if (minO2Height - maxY > -dEpsilon )
+ {
+ //totally above heightfield
+ return 0;
+ }
+ if (minY - maxO2Height > -dEpsilon )
+ {
+ // totally under heightfield
+ pContact = CONTACT(contact, 0);
+
+ pContact->pos[0] = o2->final_posr->pos[0];
+ pContact->pos[1] = minY;
+ pContact->pos[2] = o2->final_posr->pos[2];
+
+ pContact->normal[0] = 0;
+ pContact->normal[1] = REAL(-1.0);
+ pContact->normal[2] = 0;
+
+ pContact->depth = minY - maxO2Height;
+
+ return 1;
+ }
+ }
+ int numTerrainContacts = 0;
+ // get All Planes that could collide against.
+ dColliderFn *geomRayNCollider = 0;
+ dColliderFn *geomNPlaneCollider = 0;
+ dGetDepthFn *geomNDepthGetter = 0;
+
+ int max_collisionContact = numMaxContactsPossible;
+ switch (o2->type)
+ {
+ case dRayClass:
+ geomRayNCollider = NULL;
+ geomNPlaneCollider = dCollideRayPlane;
+ geomNDepthGetter = NULL;
+ //max_collisionContact = 1;
+ break;
+
+ case dSphereClass:
+ geomRayNCollider = dCollideRaySphere;
+ geomNPlaneCollider = dCollideSpherePlane;
+ geomNDepthGetter = dGeomSpherePointDepth;
+ //max_collisionContact = 3;
+ break;
+
+ case dBoxClass:
+ geomRayNCollider = dCollideRayBox;
+ geomNPlaneCollider = dCollideBoxPlane;
+ geomNDepthGetter = dGeomBoxPointDepth;
+ //max_collisionContact = 8;
+ break;
+
+ case dCapsuleClass:
+ geomRayNCollider = dCollideRayCapsule;
+ geomNPlaneCollider = dCollideCapsulePlane;
+ geomNDepthGetter = dGeomCapsulePointDepth;
+ // max_collisionContact = 3;
+ break;
+
+ case dCylinderClass:
+ geomRayNCollider = dCollideRayCylinder;
+ geomNPlaneCollider = dCollideCylinderPlane;
+ geomNDepthGetter = NULL;// TODO: dGeomCCylinderPointDepth
+ //max_collisionContact = 3;
+ break;
+
+ case dConvexClass:
+ geomRayNCollider = dCollideRayConvex;
+ geomNPlaneCollider = dCollideConvexPlane;
+ geomNDepthGetter = NULL;// TODO: dGeomConvexPointDepth;
+ //max_collisionContact = 3;
+ break;
+
+ default:
+ // Shouldn't ever get here.
+ break;
+
+ }
+
+
+ const int numMaxContacts = dMIN (max_collisionContact, HEIGHTFIELDMAXCONTACTPERCELL);
+
+ dxPlane myplane(0,0,0,0,0);
+ dxPlane* sliding_plane = &myplane;
+ dContactGeom *PlaneContact = m_p_data->m_contacts;
+ flags = (flags & 0xffff0000) | HEIGHTFIELDMAXCONTACTPERCELL;
+ dReal triplane[4];
+ int i;
+
+ // check some trivial case.
+ // Vector Up plane
+ if (maxY - minY < dEpsilon)
+ {
+ // it's a single plane.
+ triplane[0] = 0;
+ triplane[1] = REAL(1.0);
+ triplane[2] = 0;
+ triplane[3] = minY;
+ dGeomPlaneSetNoNormalize (sliding_plane, triplane);
+ // find collision and compute contact points
+ const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, flags, PlaneContact, sizeof(dContactGeom));
+ for (i = 0; i < numPlaneContacts; i++)
+ {
+ pContact = CONTACT(contact, numTerrainContacts*skip);
+ const dVector3 &pCPos = PlaneContact[i].pos;
+ dVector3Copy (pCPos, pContact->pos);
+ dOPESIGN(pContact->normal, =, -, triplane);
+
+ pContact->depth = PlaneContact[i].depth;
+ numTerrainContacts++;
+ if (numTerrainContacts > numMaxContactsPossible)
+ break;
+ }
+ return numTerrainContacts;
+ }
+ // unique plane
+ {
+ // check for very simple plane heightfield
+ dReal minXHeightDelta = dInfinity, maxXHeightDelta = - dInfinity;
+ dReal minZHeightDelta = dInfinity, maxZHeightDelta = - dInfinity;
+
+
+ dReal lastXHeight = tempHeightBuffer[0][0].vertex[1];
+ for ( x_local = 1; x_local < numX; x_local++)
+ {
+ HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
+
+ const dReal deltaX = HeightFieldRow[0].vertex[1] - lastXHeight;
+
+ maxXHeightDelta = dMAX (maxXHeightDelta, deltaX);
+ minXHeightDelta = dMIN (minXHeightDelta, deltaX);
+
+ dReal lastZHeight = HeightFieldRow[0].vertex[1];
+ for ( z_local = 1; z_local < numZ; z_local++)
+ {
+ const dReal deltaZ = (HeightFieldRow[z_local].vertex[1] - lastZHeight);
+
+ maxZHeightDelta = dMAX (maxZHeightDelta, deltaZ);
+ minZHeightDelta = dMIN (minZHeightDelta, deltaZ);
+
+ }
+ }
+
+ if (maxZHeightDelta - minZHeightDelta < dEpsilon &&
+ maxXHeightDelta - minXHeightDelta < dEpsilon )
+ {
+ // it's a single plane.
+ const dVector3 &A = tempHeightBuffer[0][0].vertex;
+ const dVector3 &B = tempHeightBuffer[1][0].vertex;
+ const dVector3 &C = tempHeightBuffer[0][1].vertex;
+
+ // define 2 edges and a point that will define collision plane
+ {
+ dVector3 Edge1, Edge2;
+ dVector3Subtract(C, A, Edge1);
+ dVector3Subtract(B, A, Edge2);
+ dVector3Cross(Edge1, Edge2, triplane);
+ }
+
+ // Define Plane
+ // Normalize plane normal
+ const dReal dinvlength = dDIV(REAL(1.0),dVector3Length(triplane));
+ triplane[0] = dMUL(triplane[0],dinvlength);
+ triplane[1] = dMUL(triplane[1],dinvlength);
+ triplane[2] = dMUL(triplane[2],dinvlength);
+ // get distance to origin from plane
+ triplane[3] = dVector3Dot(triplane, A);
+
+ dGeomPlaneSetNoNormalize (sliding_plane, triplane);
+ // find collision and compute contact points
+ const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, flags, PlaneContact, sizeof(dContactGeom));
+ for (i = 0; i < numPlaneContacts; i++)
+ {
+ pContact = CONTACT(contact, numTerrainContacts*skip);
+ const dVector3 &pCPos = PlaneContact[i].pos;
+ dVector3Copy (pCPos, pContact->pos);
+ dOPESIGN(pContact->normal, =, -, triplane);
+
+ pContact->depth = PlaneContact[i].depth;
+ numTerrainContacts++;
+ if (numTerrainContacts > numMaxContactsPossible)
+ break;
+ }
+ return numTerrainContacts;
+ }
+ }
+
+
+ const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2;
+ if (tempTriangleBufferSize < numTriMax)
+ {
+ delete [] tempTriangleBuffer;
+ tempTriangleBufferSize = numTriMax;
+ tempTriangleBuffer = new HeightFieldTriangle[numTriMax];
+ }
+
+ // Sorting triangle/plane resulting from heightfield zone
+ // Perhaps that would be necessary in case of too much limited
+ // maximum contact point...
+ // or in complex mesh case (trimesh and convex)
+ // need some test or insights on this before enabling this.
+ const bool isContactNumPointsLimited =
+ true;
+ // (numMaxContacts < 8)
+ // || o2->type == dConvexClass
+ // || o2->type == dTriMeshClass
+ // || (numMaxContacts < (int)numTriMax)
+
+
+
+ // if small heightfield triangle related to O2 colliding
+ // or no Triangle colliding at all.
+ bool needFurtherPasses = (o2->type == dTriMeshClass);
+ //compute Ratio between Triangle size and O2 aabb size
+ // no FurtherPasses are needed in ray class
+ if (o2->type != dRayClass && needFurtherPasses == false)
+ {
+ const dReal xratio = dMUL((o2->aabb[1] - o2->aabb[0]),m_p_data->m_fInvSampleWidth);
+ if (xratio > REAL(1.5))
+ needFurtherPasses = true;
+ else
+ {
+ const dReal zratio = dMUL((o2->aabb[5] - o2->aabb[4]),m_p_data->m_fInvSampleDepth);
+ if (zratio > REAL(1.5))
+ needFurtherPasses = true;
+ }
+
+ }
+
+ unsigned int numTri = 0;
+ HeightFieldVertex *A, *B, *C, *D;
+ /* (y is up)
+ A--------B-...x
+ | /|
+ | / |
+ | / |
+ | / |
+ | / |
+ | / |
+ | / |
+ |/ |
+ C--------D
+ .
+ .
+ .
+ z
+ */
+ // keep only triangle that does intersect geom
+ for ( x = minX, x_local = 0; x < maxX; x++, x_local++)
+ {
+ HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
+ HeightFieldVertex *HeightFieldNextRow = tempHeightBuffer[x_local + 1];
+
+ // First A
+ C = &HeightFieldRow [0];
+ // First B
+ D = &HeightFieldNextRow[0];
+ for ( z = minZ, z_local = 0; z < maxZ; z++, z_local++)
+ {
+ A = C;
+ B = D;
+
+ C = &HeightFieldRow [z_local + 1];
+ D = &HeightFieldNextRow[z_local + 1];
+
+ const dReal AHeight = A->vertex[1];
+ const dReal BHeight = B->vertex[1];
+ const dReal CHeight = C->vertex[1];
+ const dReal DHeight = D->vertex[1];
+
+ const bool isACollide = 0 < AHeight - minO2Height;
+ const bool isBCollide = 0 < BHeight - minO2Height;
+ const bool isCCollide = 0 < CHeight - minO2Height;
+ const bool isDCollide = 0 < DHeight - minO2Height;
+
+ A->state = !(isACollide);
+ B->state = !(isBCollide);
+ C->state = !(isCCollide);
+ D->state = !(isCCollide);
+
+ if (isACollide || isBCollide || isCCollide)
+ {
+ HeightFieldTriangle * const CurrTriUp = &tempTriangleBuffer[numTri++];
+
+ CurrTriUp->state = false;
+
+ // changing point order here implies to change it in isOnHeightField
+ CurrTriUp->vertices[0] = A;
+ CurrTriUp->vertices[1] = B;
+ CurrTriUp->vertices[2] = C;
+
+ if (isContactNumPointsLimited)
+ CurrTriUp->setMinMax();
+ CurrTriUp->isUp = true;
+ }
+
+ if (isBCollide || isCCollide || isDCollide)
+ {
+ HeightFieldTriangle * const CurrTriDown = &tempTriangleBuffer[numTri++];
+
+ CurrTriDown->state = false;
+ // changing point order here implies to change it in isOnHeightField
+
+ CurrTriDown->vertices[0] = D;
+ CurrTriDown->vertices[1] = B;
+ CurrTriDown->vertices[2] = C;
+
+
+ if (isContactNumPointsLimited)
+ CurrTriDown->setMinMax();
+ CurrTriDown->isUp = false;
+ }
+
+
+ if (needFurtherPasses &&
+ (isBCollide || isCCollide)
+ &&
+ (AHeight - CHeight > 0 &&
+ AHeight - BHeight > 0 &&
+ DHeight - CHeight > 0 &&
+ DHeight - BHeight > 0))
+ {
+ // That means Edge BC is concave, therefore
+ // BC Edge and B and C vertices cannot collide
+
+ B->state = true;
+ C->state = true;
+ }
+ // should find a way to check other edges (AB, BD, CD) too for concavity
+ }
+ }
+
+ // at least on triangle should intersect geom
+
+ // pass1: VS triangle as Planes
+ // Group Triangle by same plane definition
+ // as Terrain often has many triangles using same plane definition
+ // then collide against that list of triangles.
+ {
+
+ dVector3 Edge1, Edge2;
+ //compute all triangles normals.
+ for (unsigned int k = 0; k < numTri; k++)
+ {
+ HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
+
+ // define 2 edges and a point that will define collision plane
+ dVector3Subtract(itTriangle->vertices[2]->vertex, itTriangle->vertices[0]->vertex, Edge1);
+ dVector3Subtract(itTriangle->vertices[1]->vertex, itTriangle->vertices[0]->vertex, Edge2);
+
+ // find a perpendicular vector to the triangle
+ if (itTriangle->isUp)
+ dVector3Cross(Edge1, Edge2, triplane);
+ else
+ dVector3Cross(Edge2, Edge1, triplane);
+
+ // Define Plane
+ // Normalize plane normal
+ const dReal dinvlength = dDIV(1,dVector3Length(triplane));
+ triplane[0] = dMUL(triplane[0],dinvlength);
+ triplane[1] = dMUL(triplane[1],dinvlength);
+ triplane[2] = dMUL(triplane[2],dinvlength);
+ // get distance to origin from plane
+ triplane[3] = dVector3Dot(triplane, itTriangle->vertices[0]->vertex);
+
+ // saves normal for collision check (planes, triangles, vertices and edges.)
+ dVector3Copy(triplane, itTriangle->planeDef);
+ // saves distance for collision check (planes, triangles, vertices and edges.)
+ itTriangle->planeDef[3] = triplane[3];
+ }
+
+ // group by Triangles by Planes sharing shame plane definition
+ if (tempPlaneBufferSize < numTri)
+ {
+ delete [] tempPlaneBuffer;
+ tempPlaneBufferSize = numTri;
+ tempPlaneBuffer = new HeightFieldPlane *[numTri];
+
+ for (unsigned int k = 0; k < tempPlaneBufferSize; k++)
+ {
+ tempPlaneBuffer[k] = new HeightFieldPlane();
+ }
+ }
+ unsigned int numPlanes = 0;
+ for (unsigned int k = 0; k < numTri; k++)
+ {
+ HeightFieldTriangle * const tri_base = &tempTriangleBuffer[k];
+
+ if (tri_base->state == true)
+ continue;// already tested or added to plane list.
+
+ HeightFieldPlane * const currPlane = tempPlaneBuffer[numPlanes];
+ currPlane->resetTriangleListSize(numTri - k);
+ currPlane->addTriangle(tri_base);
+ // saves normal for collision check (planes, triangles, vertices and edges.)
+ dVector3Copy(tri_base->planeDef, currPlane->planeDef);
+ // saves distance for collision check (planes, triangles, vertices and edges.)
+ currPlane->planeDef[3]= tri_base->planeDef[3];
+
+ const dReal normx = tri_base->planeDef[0];
+ const dReal normy = tri_base->planeDef[1];
+ const dReal normz = tri_base->planeDef[2];
+ const dReal dist = tri_base->planeDef[3];
+
+ for (unsigned int m = k + 1; m < numTri; m++)
+ {
+
+ HeightFieldTriangle * const tri_test = &tempTriangleBuffer[m];
+ if (tri_test->state == true)
+ continue;// already tested or added to plane list.
+
+ // normals and distance are the same.
+ if (
+ dFabs(normy - tri_test->planeDef[1]) < dEpsilon &&
+ dFabs(dist - tri_test->planeDef[3]) < dEpsilon &&
+ dFabs(normx - tri_test->planeDef[0]) < dEpsilon &&
+ dFabs(normz - tri_test->planeDef[2]) < dEpsilon
+ )
+ {
+ currPlane->addTriangle (tri_test);
+ tri_test->state = true;
+ }
+ }
+
+ tri_base->state = true;
+ if (isContactNumPointsLimited)
+ currPlane->setMinMax();
+
+ numPlanes++;
+ }
+
+ // sort planes
+ if (isContactNumPointsLimited)
+ sortPlanes(numPlanes);
+
+ for (unsigned int k = 0; k < numPlanes; k++)
+ {
+ HeightFieldPlane * const itPlane = tempPlaneBuffer[k];
+
+ //set Geom
+ dGeomPlaneSetNoNormalize (sliding_plane, itPlane->planeDef);
+ //dGeomPlaneSetParams (sliding_plane, triangle_Plane[0], triangle_Plane[1], triangle_Plane[2], triangle_Plane[3]);
+ // find collision and compute contact points
+ bool didCollide = false;
+ const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, flags, PlaneContact, sizeof(dContactGeom));
+ const size_t planeTriListSize = itPlane->trianglelistCurrentSize;
+ for (i = 0; i < numPlaneContacts; i++)
+ {
+ // Check if contact point found in plane is inside Triangle.
+ const dVector3 &pCPos = PlaneContact[i].pos;
+ bool isOnOneOfTrianglePlane = false;
+ for (size_t b = 0; planeTriListSize > b; b++)
+ {
+ if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0]->vertex,
+ pCPos,
+ itPlane->trianglelist[b]->isUp))
+ {
+ isOnOneOfTrianglePlane = true;
+ break;
+ }
+ }
+ if ( isOnOneOfTrianglePlane)
+ {
+ pContact = CONTACT(contact, numTerrainContacts*skip);
+ dVector3Copy(pCPos, pContact->pos);
+ dOPESIGN(pContact->normal, =, -, itPlane->planeDef);
+ pContact->depth = PlaneContact[i].depth;
+ didCollide = true;
+ numTerrainContacts++;
+ if ( numTerrainContacts == numMaxContacts )
+ return numTerrainContacts;
+ }
+ }
+ if (didCollide)
+ {
+ for (size_t b = 0; planeTriListSize > b; b++)
+ {
+ // flag Triangles Vertices as collided
+ // to prevent any collision test of those
+ for (i = 0; i < 3; i++)
+ itPlane->trianglelist[b]->vertices[i]->state = true;
+ }
+ }
+ else
+ {
+ // flag triangle as not collided so that Vertices or Edge
+ // of that triangles will be checked.
+ for (size_t b = 0; planeTriListSize > b; b++)
+ {
+ itPlane->trianglelist[b]->state = false;
+ }
+ }
+ }
+ }
+
+
+
+ // pass2: VS triangle vertices
+ if (needFurtherPasses)
+ {
+ dxRay tempRay(0, 1);
+ dReal depth;
+ bool vertexCollided;
+ //
+ // Find Contact Penetration Depth of each vertices
+ //
+ for (unsigned int k = 0; k < numTri; k++)
+ {
+ const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
+ if (itTriangle->state == true)
+ continue;// plane triangle did already collide.
+
+ for (size_t i = 0; i < 3; i++)
+ {
+ HeightFieldVertex *vertex = itTriangle->vertices[i];
+ if (vertex->state == true)
+ continue;// vertice did already collide.
+
+ vertexCollided = false;
+ const dVector3 &triVertex = vertex->vertex;
+ if ( geomNDepthGetter )
+ {
+ depth = geomNDepthGetter( o2,
+ triVertex[0], triVertex[1], triVertex[2] );
+ if (depth + dEpsilon < 0)
+ vertexCollided = true;
+ }
+ 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 = (minO2Height - triVertex[1]) * REAL(1000.f);
+
+ //dGeomRaySet( &tempRay, pContact->pos[0], pContact->pos[1], pContact->pos[2],
+ // - itTriangle->Normal[0], - itTriangle->Normal[1], - itTriangle->Normal[2] );
+ dGeomRaySetNoNormalize(tempRay, triVertex, itTriangle->planeDef);
+
+ if ( geomRayNCollider( &tempRay, o2, flags, PlaneContact, sizeof( dContactGeom ) ) )
+ {
+ depth = PlaneContact[0].depth;
+ vertexCollided = true;
+ }
+ }
+ if (vertexCollided)
+ {
+ pContact = CONTACT(contact, numTerrainContacts*skip);
+ //create contact using vertices
+ dVector3Copy (triVertex, pContact->pos);
+ //create contact using Plane Normal
+ dOPESIGN(pContact->normal, =, -, itTriangle->planeDef);
+
+ pContact->depth = depth;
+
+ numTerrainContacts++;
+ if ( numTerrainContacts == numMaxContacts )
+ return numTerrainContacts;
+
+ vertex->state = true;
+ }
+ }
+ }
+ }
+
+ return numTerrainContacts;
+}
+
+int dCollideHeightfield( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contact, int skip )
+{
+
+ int i;
+
+ if ((flags & 0xffff) == 0)
+ flags = (flags & 0xffff0000) | 1;
+
+ int numMaxTerrainContacts = (flags & 0xffff);
+ dxHeightfield *terrain = (dxHeightfield*) o1;
+
+ dVector3 posbak;
+ dMatrix3 Rbak;
+ dReal aabbbak[6];
+ int gflagsbak;
+ dVector3 pos0,pos1;
+ dMatrix3 R1;
+
+ int numTerrainContacts = 0;
+
+ //@@ Should find a way to set reComputeAABB to false in default case
+ // aka DHEIGHTFIELD_CORNER_ORIGIN not defined and terrain not PLACEABLE
+ // so that we can free some memory and speed up things a bit
+ // while saving some precision loss
+
+ const bool reComputeAABB = true;
+
+ //
+ // Transform O2 into Heightfield Space
+ //
+ if (reComputeAABB)
+ {
+ // Backup original o2 position, rotation and AABB.
+ dVector3Copy( o2->final_posr->pos, posbak );
+ dMatrix3Copy( o2->final_posr->R, Rbak );
+ memcpy( aabbbak, o2->aabb, sizeof( dReal ) * 6 );
+ gflagsbak = o2->gflags;
+ }
+
+ if ( terrain->gflags & GEOM_PLACEABLE )
+ {
+ // Transform o2 into heightfield space.
+ dOP( pos0, -, o2->final_posr->pos, terrain->final_posr->pos );
+ dMULTIPLY1_331( pos1, terrain->final_posr->R, pos0 );
+ dMULTIPLY1_333( R1, terrain->final_posr->R, o2->final_posr->R );
+
+ // Update o2 with transformed position and rotation.
+ dVector3Copy( pos1, o2->final_posr->pos );
+ dMatrix3Copy( R1, o2->final_posr->R );
+ }
+
+
+ o2->final_posr->pos[ 0 ] += terrain->m_p_data->m_fHalfWidth;
+ o2->final_posr->pos[ 2 ] += terrain->m_p_data->m_fHalfDepth;
+
+ // Rebuild AABB for O2
+ if (reComputeAABB)
+ o2->computeAABB();
+
+ //
+ // Collide
+ //
+
+ //check if inside boundaries
+ // using O2 aabb
+ // aabb[6] is (minx, maxx, miny, maxy, minz, maxz)
+ const bool notWrapped = terrain->m_p_data->m_bWrapMode == 0;
+
+ int nMinX;
+ int nMaxX;
+ int nMinZ;
+ int nMaxZ;
+
+ if ( notWrapped )
+ {
+ if ( o2->aabb[0] > terrain->m_p_data->m_fWidth //MinX
+ && o2->aabb[4] > terrain->m_p_data->m_fDepth)//MinZ
+ goto dCollideHeightfieldExit;
+
+ if ( o2->aabb[1] < 0 //MaxX
+ && o2->aabb[5] < 0) //MaxZ
+ goto dCollideHeightfieldExit;
+
+ }
+
+ nMinX = dMUL(o2->aabb[0],terrain->m_p_data->m_fInvSampleWidth)>>QFACTOR;
+ nMaxX = dMUL(o2->aabb[1],terrain->m_p_data->m_fInvSampleWidth)>>QFACTOR + 1;
+ nMinZ = dMUL(o2->aabb[4],terrain->m_p_data->m_fInvSampleDepth)>>QFACTOR;
+ nMaxZ = dMUL(o2->aabb[5],terrain->m_p_data->m_fInvSampleDepth)>>QFACTOR + 1;
+
+ if ( notWrapped )
+ {
+ nMinX = dMAX( nMinX, 0 );
+ nMaxX = dMIN( nMaxX, terrain->m_p_data->m_nWidthSamples - 1 );
+ nMinZ = dMAX( nMinZ, 0 );
+ nMaxZ = dMIN( nMaxZ, terrain->m_p_data->m_nDepthSamples - 1 );
+
+ }
+
+
+
+ numTerrainContacts = terrain->dCollideHeightfieldZone(
+ nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts,
+ flags,CONTACT(contact,numTerrainContacts*skip),skip );
+
+
+ dContactGeom *pContact;
+ for ( i = 0; i < numTerrainContacts; ++i )
+ {
+ pContact = CONTACT(contact,i*skip);
+ pContact->g1 = o1;
+ pContact->g2 = o2;
+ }
+
+
+ //------------------------------------------------------------------------------
+
+dCollideHeightfieldExit:
+
+ if (reComputeAABB)
+ {
+ // Restore o2 position, rotation and AABB
+ dVector3Copy( posbak, o2->final_posr->pos );
+ dMatrix3Copy( Rbak, o2->final_posr->R );
+ memcpy( o2->aabb, aabbbak, sizeof(dReal)*6 );
+ o2->gflags = gflagsbak;
+
+ //
+ // Transform Contacts to World Space
+ //
+ if ( terrain->gflags & GEOM_PLACEABLE )
+ {
+ for ( i = 0; i < numTerrainContacts; ++i )
+ {
+ pContact = CONTACT(contact,i*skip);
+ dOPE( pos0, =, pContact->pos );
+
+
+ pos0[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
+ pos0[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
+
+
+ dMULTIPLY0_331( pContact->pos, terrain->final_posr->R, pos0 );
+
+ dOP( pContact->pos, +, pContact->pos, terrain->final_posr->pos );
+ dOPE( pos0, =, pContact->normal );
+
+ dMULTIPLY0_331( pContact->normal, terrain->final_posr->R, pos0 );
+ }
+ }
+
+ else
+ {
+ for ( i = 0; i < numTerrainContacts; ++i )
+ {
+ pContact = CONTACT(contact,i*skip);
+ pContact->pos[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
+ pContact->pos[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
+ }
+ }
+
+ }
+ // Return contact count.
+ return numTerrainContacts;
+}
+
+