diff -r 000000000000 -r 2f259fa3e83a ode/src/heightfield.cpp --- /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 +#include +#include +#include +#include +#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_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]MaxX+TolX)) + return false; + } + + { + const dReal MaxZ = CellOrigin[2] + m_fSampleDepth; + const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL); + if ((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]MaxX+TolX)) + return false; + } + + { + const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL); + if ((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; +} + +