// 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" #if dTRIMESH_ENABLED #include "collision_trimesh_internal.h" #endif // dTRIMESH_ENABLED #define TERRAINTOL REAL(0.0) #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); \ } //////// Local Build Option //////////////////////////////////////////////////// // Uncomment this #define to use the (0,0) corner of the geom as the origin, // 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 // Uncomment this #define to add heightfield triangles edge colliding // Code is not guaranteed and I didn't find the need to add that as // colliding planes triangles and edge triangles seems enough. // #define _HEIGHTFIELDEDGECOLLIDING //////// 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 ) { dIASSERT( fWidth > REAL( 0.0 ) ); dIASSERT( fDepth > REAL( 0.0 ) ); dIASSERT( nWidthSamples > 0 ); dIASSERT( nDepthSamples > 0 ); // x,z bounds m_fWidth = fWidth; m_fDepth = fDepth; // cache half x,z bounds m_fHalfWidth = fWidth / REAL( 2.0 ); m_fHalfDepth = 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 = 1 / m_fSampleWidth; m_fInvSampleDepth = 1 / m_fSampleDepth; // finite or repeated terrain? m_bWrapMode = bWrapMode; } // recomputes heights bounds void dxHeightfieldData::ComputeHeightBounds() { static int i; static dReal h; static unsigned char *data_byte; static short *data_short; static float *data_float; static double *data_double; switch ( m_nGetHeightMode ) { // callback case 0: // change nothing, keep using default or user specified bounds return; // byte case 1: data_byte = (unsigned char*)m_pHeightData; m_fMinHeight = dInfinity; m_fMaxHeight = -dInfinity; for (i=0; i m_fMaxHeight) m_fMaxHeight = h; } break; // short case 2: data_short = (short*)m_pHeightData; m_fMinHeight = dInfinity; m_fMaxHeight = -dInfinity; for (i=0; i m_fMaxHeight) m_fMaxHeight = h; } break; // float case 3: data_float = (float*)m_pHeightData; m_fMinHeight = dInfinity; m_fMaxHeight = -dInfinity; for (i=0; i m_fMaxHeight) m_fMaxHeight = h; } break; // double case 4: data_double = (double*)m_pHeightData; m_fMinHeight = dInfinity; m_fMaxHeight = -dInfinity; for (i=0; i( data_double[i] ); if (h < m_fMinHeight) m_fMinHeight = h; if (h > m_fMaxHeight) m_fMaxHeight = h; } break; } // scale and offset m_fMinHeight *= m_fScale; 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 = m_fSampleWidth * TERRAINTOL; if ((pos[0]MaxX+TolX)) return false; } { const dReal MaxZ = CellOrigin[2] + m_fSampleDepth; const dReal TolZ = 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 = (pos[0] - CellOrigin[0]) * m_fInvSampleWidth + (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 = m_fSampleWidth * TERRAINTOL; if ((pos[0]MaxX+TolX)) return false; } { const dReal TolZ = 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 = (pos[0] - MinX) * m_fInvSampleWidth + (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 unsigned char *data_byte; static short *data_short; static float *data_float; static double *data_double; 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; } switch ( m_nGetHeightMode ) { // callback (dReal) case 0: h = (*m_pGetHeightCallback)(m_pUserData, x, z); break; // byte case 1: data_byte = (unsigned char*)m_pHeightData; h = data_byte[x+(z * m_nWidthSamples)]; break; // short case 2: data_short = (short*)m_pHeightData; h = data_short[x+(z * m_nWidthSamples)]; break; // float case 3: data_float = (float*)m_pHeightData; h = data_float[x+(z * m_nWidthSamples)]; break; // double case 4: data_double = (double*)m_pHeightData; h = static_cast< dReal >( data_double[x+(z * m_nWidthSamples)] ); break; } return (h * m_fScale) + m_fOffset; } // returns height at given coordinates dReal dxHeightfieldData::GetHeight( dReal x, dReal z ) { dReal dnX = dFloor( x * m_fInvSampleWidth ); dReal dnZ = dFloor( z * m_fInvSampleDepth ); dReal dx = ( x - ( dnX * m_fSampleWidth ) ) * m_fInvSampleWidth; dReal dz = ( z - ( dnZ * m_fSampleDepth ) ) * m_fInvSampleDepth; int nX = int( dnX ); int nZ = int( dnZ ); //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 ) ) { y0 = GetHeight( nX, nZ ); y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * dx + ( GetHeight( nX, nZ + 1 ) - y0 ) * dz; } else { y0 = GetHeight( nX + 1, nZ + 1 ); y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * ( REAL(1.0) - dz ) + ( GetHeight( nX, nZ + 1 ) - y0 ) * ( REAL(1.0) - dx ); } return y; } // dxHeightfieldData destructor dxHeightfieldData::~dxHeightfieldData() { static unsigned char *data_byte; static short *data_short; static float *data_float; static double *data_double; dIASSERT( m_pHeightData ); if ( m_bCopyHeightData ) { switch ( m_nGetHeightMode ) { // callback case 0: // do nothing break; // byte case 1: data_byte = (unsigned char*)m_pHeightData; delete [] data_byte; break; // short case 2: data_short = (short*)m_pHeightData; delete [] data_short; break; // float case 3: data_float = (float*)m_pHeightData; delete [] data_float; break; // double case 4: data_double = (double*)m_pHeightData; delete [] data_double; break; } } } //////// dxHeightfield ///////////////////////////////////////////////////////////////// // dxHeightfield constructor dxHeightfield::dxHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable ) : dxGeom( space, bPlaceable ), tempPlaneBuffer(0), tempPlaneInstances(0), tempPlaneBufferSize(0), tempTriangleBuffer(0), tempTriangleBufferSize(0), tempHeightBuffer(0), tempHeightInstances(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] = ( final_posr->R[ 1] * d->m_fMinHeight ); dy[1] = ( final_posr->R[ 5] * d->m_fMinHeight ); dy[2] = ( final_posr->R[ 9] * d->m_fMinHeight ); dy[3] = ( final_posr->R[ 1] * d->m_fMaxHeight ); dy[4] = ( final_posr->R[ 5] * d->m_fMaxHeight ); dy[5] = ( final_posr->R[ 9] * d->m_fMaxHeight ); #ifdef DHEIGHTFIELD_CORNER_ORIGIN // X-axis dx[0] = 0; dx[3] = ( final_posr->R[ 0] * d->m_fWidth ); dx[1] = 0; dx[4] = ( final_posr->R[ 4] * d->m_fWidth ); dx[2] = 0; dx[5] = ( final_posr->R[ 8] * d->m_fWidth ); // Z-axis dz[0] = 0; dz[3] = ( final_posr->R[ 2] * d->m_fDepth ); dz[1] = 0; dz[4] = ( final_posr->R[ 6] * d->m_fDepth ); dz[2] = 0; dz[5] = ( final_posr->R[10] * d->m_fDepth ); #else // DHEIGHTFIELD_CORNER_ORIGIN // X-axis dx[0] = ( final_posr->R[ 0] * -d->m_fHalfWidth ); dx[1] = ( final_posr->R[ 4] * -d->m_fHalfWidth ); dx[2] = ( final_posr->R[ 8] * -d->m_fHalfWidth ); dx[3] = ( final_posr->R[ 0] * d->m_fHalfWidth ); dx[4] = ( final_posr->R[ 4] * d->m_fHalfWidth ); dx[5] = ( final_posr->R[ 8] * d->m_fHalfWidth ); // Z-axis dz[0] = ( final_posr->R[ 2] * -d->m_fHalfDepth ); dz[1] = ( final_posr->R[ 6] * -d->m_fHalfDepth ); dz[2] = ( final_posr->R[10] * -d->m_fHalfDepth ); dz[3] = ( final_posr->R[ 2] * d->m_fHalfDepth ); dz[4] = ( final_posr->R[ 6] * d->m_fHalfDepth ); dz[5] = ( final_posr->R[10] * d->m_fHalfDepth ); #endif // DHEIGHTFIELD_CORNER_ORIGIN // 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 { #ifdef DHEIGHTFIELD_CORNER_ORIGIN aabb[0] = 0; aabb[1] = d->m_fWidth; aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight; aabb[4] = 0; aabb[5] = d->m_fDepth; #else // DHEIGHTFIELD_CORNER_ORIGIN 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; #endif // DHEIGHTFIELD_CORNER_ORIGIN } } 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() { resetTriangleBuffer(); resetPlaneBuffer(); resetHeightBuffer(); } void dxHeightfield::allocateTriangleBuffer(size_t numTri) { size_t alignedNumTri = AlignBufferSize(numTri, TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT); tempTriangleBufferSize = alignedNumTri; tempTriangleBuffer = new HeightFieldTriangle[alignedNumTri]; } void dxHeightfield::resetTriangleBuffer() { delete[] tempTriangleBuffer; } void dxHeightfield::allocatePlaneBuffer(size_t numTri) { size_t alignedNumTri = AlignBufferSize(numTri, TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT); tempPlaneBufferSize = alignedNumTri; tempPlaneBuffer = new HeightFieldPlane *[alignedNumTri]; tempPlaneInstances = new HeightFieldPlane[alignedNumTri]; HeightFieldPlane *ptrPlaneMatrix = tempPlaneInstances; for (size_t indexTri = 0; indexTri != alignedNumTri; indexTri++) { tempPlaneBuffer[indexTri] = ptrPlaneMatrix; ptrPlaneMatrix += 1; } } void dxHeightfield::resetPlaneBuffer() { delete[] tempPlaneInstances; delete[] tempPlaneBuffer; } void dxHeightfield::allocateHeightBuffer(size_t numX, size_t numZ) { size_t alignedNumX = AlignBufferSize(numX, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X); size_t alignedNumZ = AlignBufferSize(numZ, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z); tempHeightBufferSizeX = alignedNumX; tempHeightBufferSizeZ = alignedNumZ; tempHeightBuffer = new HeightFieldVertex *[alignedNumX]; size_t numCells = alignedNumX * alignedNumZ; tempHeightInstances = new HeightFieldVertex [numCells]; HeightFieldVertex *ptrHeightMatrix = tempHeightInstances; for (size_t indexX = 0; indexX != alignedNumX; indexX++) { tempHeightBuffer[indexX] = ptrHeightMatrix; ptrHeightMatrix += alignedNumZ; } } void dxHeightfield::resetHeightBuffer() { delete[] tempHeightInstances; delete[] tempHeightBuffer; } //////// Heightfield data interface //////////////////////////////////////////////////// dHeightfieldDataID dGeomHeightfieldDataCreate() { return new dxHeightfieldData(); } void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d, void* pUserData, dHeightfieldGetHeight* pCallback, dReal width, dReal depth, int widthSamples, int depthSamples, dReal scale, dReal offset, dReal thickness, int bWrap ) { dUASSERT( d, "argument not Heightfield data" ); dIASSERT( pCallback ); dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. dIASSERT( depthSamples >= 2 ); // 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; } void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d, const unsigned char *pHeightData, int bCopyHeightData, dReal width, dReal depth, int widthSamples, int depthSamples, dReal scale, dReal offset, dReal thickness, int bWrap ) { dUASSERT( d, "Argument not Heightfield data" ); dIASSERT( pHeightData ); dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. dIASSERT( depthSamples >= 2 ); // set info d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); d->m_nGetHeightMode = 1; 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 unsigned char[ d->m_nWidthSamples * d->m_nDepthSamples ]; dIASSERT( d->m_pHeightData ); // Copy data. memcpy( (void*)d->m_pHeightData, pHeightData, sizeof( unsigned char ) * d->m_nWidthSamples * d->m_nDepthSamples ); } // Find height bounds d->ComputeHeightBounds(); } void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d, const short* pHeightData, int bCopyHeightData, dReal width, dReal depth, int widthSamples, int depthSamples, dReal scale, dReal offset, dReal thickness, int bWrap ) { dUASSERT( d, "Argument not Heightfield data" ); dIASSERT( pHeightData ); dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. dIASSERT( depthSamples >= 2 ); // set info d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); d->m_nGetHeightMode = 2; 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 short[ d->m_nWidthSamples * d->m_nDepthSamples ]; dIASSERT( d->m_pHeightData ); // Copy data. memcpy( (void*)d->m_pHeightData, pHeightData, sizeof( short ) * d->m_nWidthSamples * d->m_nDepthSamples ); } // Find height bounds d->ComputeHeightBounds(); } 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 ) { dUASSERT( d, "Argument not Heightfield data" ); dIASSERT( pHeightData ); dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. dIASSERT( depthSamples >= 2 ); // 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 float[ d->m_nWidthSamples * d->m_nDepthSamples ]; dIASSERT( d->m_pHeightData ); // Copy data. memcpy( (void*)d->m_pHeightData, pHeightData, sizeof( float ) * d->m_nWidthSamples * d->m_nDepthSamples ); } // Find height bounds d->ComputeHeightBounds(); } void dGeomHeightfieldDataBuildDouble( dHeightfieldDataID d, const double *pHeightData, int bCopyHeightData, dReal width, dReal depth, int widthSamples, int depthSamples, dReal scale, dReal offset, dReal thickness, int bWrap ) { dUASSERT( d, "Argument not Heightfield data" ); dIASSERT( pHeightData ); dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell. dIASSERT( depthSamples >= 2 ); // set info d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap ); d->m_nGetHeightMode = 4; 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 double[ d->m_nWidthSamples * d->m_nDepthSamples ]; dIASSERT( d->m_pHeightData ); // Copy data. memcpy( (void*)d->m_pHeightData, pHeightData, sizeof( double ) * d->m_nWidthSamples * d->m_nDepthSamples ); } // Find height bounds d->ComputeHeightBounds(); } void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight ) { dUASSERT(d, "Argument not Heightfield data"); d->m_fMinHeight = ( minHeight * d->m_fScale ) + d->m_fOffset - d->m_fThickness; d->m_fMaxHeight = ( maxHeight * d->m_fScale ) + d->m_fOffset; } void dGeomHeightfieldDataDestroy( dHeightfieldDataID d ) { dUASSERT(d, "argument not Heightfield data"); delete d; } //////// Heightfield geom interface //////////////////////////////////////////////////// dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable ) { return new dxHeightfield( space, data, bPlaceable ); } void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d ) { dxHeightfield* geom = (dxHeightfield*) g; geom->data = d; } 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 ); #define DMESS(A) \ dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \ x,z,A, \ pContact->depth, \ dGeomSphereGetRadius(o2), \ pContact->pos[0], \ pContact->pos[1], \ pContact->pos[2], \ pContact->normal[0], \ pContact->normal[1], \ pContact->normal[2]); static inline bool DescendingTriangleSort(const HeightFieldTriangle * const A, const HeightFieldTriangle * const B) { return ((A->maxAAAB - B->maxAAAB) > dEpsilon); } 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); } static inline dReal DistancePointToLine(const dVector3 &_point, const dVector3 &_pt0, const dVector3 &_Edge, const dReal _Edgelength) { dVector3 v; dVector3Subtract(_point, _pt0, v); dVector3 s; dVector3Copy (_Edge, s); const dReal dot = dVector3Dot(v, _Edge) / _Edgelength; dVector3Scale(s, dot); dVector3Subtract(v, s, v); return dVector3Length(v); } 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(); allocateHeightBuffer(numX, 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] = - 1; pContact->normal[2] = 0; pContact->depth = minY - maxO2Height; return 1; } } // get All Planes that could collide against. dColliderFn *geomRayNCollider; dColliderFn *geomNPlaneCollider; dGetDepthFn *geomNDepthGetter; // int max_collisionContact = numMaxContactsPossible; -- not used 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; #if dTRIMESH_ENABLED case dTriMeshClass: geomRayNCollider = dCollideRayTrimesh; geomNPlaneCollider = dCollideTrimeshPlane; geomNDepthGetter = NULL;// TODO: dGeomTrimeshPointDepth; //max_collisionContact = 3; break; #endif // dTRIMESH_ENABLED default: dIASSERT(0); // Shouldn't ever get here. break; } dxPlane myplane(0,0,0,0,0); dxPlane* sliding_plane = &myplane; 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] = 1; triplane[2] = 0; triplane[3] = minY; dGeomPlaneSetNoNormalize (sliding_plane, triplane); // find collision and compute contact points const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip); dIASSERT(numTerrainContacts <= numMaxContactsPossible); for (i = 0; i < numTerrainContacts; i++) { pContact = CONTACT(contact, i*skip); dOPESIGN(pContact->normal, =, -, triplane); } 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 = REAL(1.0) / dVector3Length(triplane); triplane[0] *= dinvlength; triplane[1] *= dinvlength; 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 numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip); dIASSERT(numTerrainContacts <= numMaxContactsPossible); for (i = 0; i < numTerrainContacts; i++) { pContact = CONTACT(contact, i*skip); dOPESIGN(pContact->normal, =, -, triplane); } return numTerrainContacts; } } int numTerrainContacts = 0; dContactGeom *PlaneContact = m_p_data->m_contacts; const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2; if (tempTriangleBufferSize < numTriMax) { resetTriangleBuffer(); allocateTriangleBuffer(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 = (o2->aabb[1] - o2->aabb[0]) * m_p_data->m_fInvSampleWidth; if (xratio > REAL(1.5)) needFurtherPasses = true; else { const dReal zratio = (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 dIASSERT (numTri != 0); // 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 = REAL(1.0) / dVector3Length(triplane); triplane[0] *= dinvlength; triplane[1] *= dinvlength; 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) { resetPlaneBuffer(); allocatePlaneBuffer(numTri); } 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); #if !defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) /* Note by Oleh_Derevenko: It seems to be incorrect to limit contact count by some particular value since some of them (and even all of them) may be culled in following condition. However I do not see an easy way to fix this. If not that culling the flags modification should be changed here and additionally repeated after some contacts have been generated (in "if (didCollide)"). The maximum of contacts in flags would then be set to minimum of contacts remaining and HEIGHTFIELDMAXCONTACTPERCELL. */ int planeTestFlags = (flags & ~NUMC_MASK) | HEIGHTFIELDMAXCONTACTPERCELL; dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); #else // if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) int numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); int planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane; dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); #endif 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, planeTestFlags, 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; for (size_t b = 0; planeTriListSize > b; b++) { if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0]->vertex, pCPos, itPlane->trianglelist[b]->isUp)) { pContact = CONTACT(contact, numTerrainContacts*skip); dVector3Copy(pCPos, pContact->pos); dOPESIGN(pContact->normal, =, -, itPlane->planeDef); pContact->depth = PlaneContact[i].depth; numTerrainContacts++; if ( numTerrainContacts == numMaxContactsPossible ) return numTerrainContacts; didCollide = true; break; } } } if (didCollide) { #if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2) /* Note by Oleh_Derevenko: This code is not used - see another note above */ numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane; dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); #endif 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; // Only one contact is necessary for ray test int rayTestFlags = (flags & ~NUMC_MASK) | 1; dIASSERT((1 & ~NUMC_MASK) == 0); // // 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.0); //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, rayTestFlags, 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 == numMaxContactsPossible ) return numTerrainContacts; vertex->state = true; } } } } #ifdef _HEIGHTFIELDEDGECOLLIDING // pass3: VS triangle Edges if (needFurtherPasses) { dVector3 Edge; dxRay edgeRay(0, 1); int numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); int triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri; dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); for (unsigned int k = 0; k < numTri; k++) { const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k]; if (itTriangle->state == true) continue;// plane did already collide. for (size_t m = 0; m < 3; m++) { const size_t next = (m + 1) % 3; HeightFieldVertex *vertex0 = itTriangle->vertices[m]; HeightFieldVertex *vertex1 = itTriangle->vertices[next]; // not concave or under the AABB // nor triangle already collided against vertices if (vertex0->state == true && vertex1->state == true) continue;// plane did already collide. dVector3Subtract(vertex1->vertex, vertex0->vertex, Edge); edgeRay.length = dVector3Length (Edge); dGeomRaySetNoNormalize(edgeRay, vertex1->vertex, Edge); int prevTerrainContacts = numTerrainContacts; pContact = CONTACT(contact, prevTerrainContacts*skip); const int numCollision = geomRayNCollider(&edgeRay,o2,triTestFlags,pContact,skip); dIASSERT(numCollision <= numMaxContactsPerTri); if (numCollision) { numTerrainContacts += numCollision; do { pContact = CONTACT(contact, prevTerrainContacts*skip); //create contact using Plane Normal dOPESIGN(pContact->normal, =, -, itTriangle->planeDef); pContact->depth = DistancePointToLine(pContact->pos, vertex1->vertex, Edge, edgeRay.length); } while (++prevTerrainContacts != numTerrainContacts); if ( numTerrainContacts == numMaxContactsPossible ) return numTerrainContacts; numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL); triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri; dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0); } } itTriangle->vertices[0]->state = true; itTriangle->vertices[1]->state = true; itTriangle->vertices[2]->state = true; } } #endif // _HEIGHTFIELDEDGECOLLIDING return numTerrainContacts; } int dCollideHeightfield( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contact, int skip ) { dIASSERT( skip >= (int)sizeof(dContactGeom) ); dIASSERT( o1->type == dHeightfieldClass ); dIASSERT((flags & NUMC_MASK) >= 1); int i; // if ((flags & NUMC_MASK) == 0) -- An assertion check is made on entry // { flags = (flags & ~NUMC_MASK) | 1; dIASSERT((1 & ~NUMC_MASK) == 0); } int numMaxTerrainContacts = (flags & NUMC_MASK); 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 #ifndef DHEIGHTFIELD_CORNER_ORIGIN const bool reComputeAABB = true; #else const bool reComputeAABB = ( terrain->gflags & GEOM_PLACEABLE ) ? true : false; #endif //DHEIGHTFIELD_CORNER_ORIGIN // // 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 ); } #ifndef DHEIGHTFIELD_CORNER_ORIGIN o2->final_posr->pos[ 0 ] += terrain->m_p_data->m_fHalfWidth; o2->final_posr->pos[ 2 ] += terrain->m_p_data->m_fHalfDepth; #endif // DHEIGHTFIELD_CORNER_ORIGIN // 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 wrapped = terrain->m_p_data->m_bWrapMode != 0; int nMinX; int nMaxX; int nMinZ; int nMaxZ; if ( !wrapped ) { 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 = int(dFloor(o2->aabb[0] * terrain->m_p_data->m_fInvSampleWidth)); nMaxX = int(dFloor(o2->aabb[1] * terrain->m_p_data->m_fInvSampleWidth)) + 1; nMinZ = int(dFloor(o2->aabb[4] * terrain->m_p_data->m_fInvSampleDepth)); nMaxZ = int(dFloor(o2->aabb[5] * terrain->m_p_data->m_fInvSampleDepth)) + 1; if ( !wrapped ) { 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 ); dIASSERT ((nMinX < nMaxX) || (nMinZ < nMaxZ)) } numTerrainContacts += terrain->dCollideHeightfieldZone( nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts, flags,CONTACT(contact,numTerrainContacts*skip),skip ); dIASSERT( numTerrainContacts <= numMaxTerrainContacts ); 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 ); #ifndef DHEIGHTFIELD_CORNER_ORIGIN pos0[ 0 ] -= terrain->m_p_data->m_fHalfWidth; pos0[ 2 ] -= terrain->m_p_data->m_fHalfDepth; #endif // !DHEIGHTFIELD_CORNER_ORIGIN 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 ); } } #ifndef DHEIGHTFIELD_CORNER_ORIGIN 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; } } #endif // !DHEIGHTFIELD_CORNER_ORIGIN } // Return contact count. return numTerrainContacts; }