/************************************************************************* * * * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * * All rights reserved. Email: russ@q12.org Web: www.q12.org * * * * This library is free software; you can redistribute it and/or * * modify it under the terms of EITHER: * * (1) The GNU Lesser General Public License as published by the Free * * Software Foundation; either version 2.1 of the License, or (at * * your option) any later version. The text of the GNU Lesser * * General Public License is included with this library in the * * file LICENSE.TXT. * * (2) The BSD-style license that is included with this library in * * the file LICENSE-BSD.TXT. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * * LICENSE.TXT and LICENSE-BSD.TXT for more details. * * * *************************************************************************/ // 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.hpp" #include "ode_collision.hpp" #include "ode_rotation.hpp" //#include "config.h" #include "matrix.h" #include "odemath.h" #include "collision_kernel.h" #include "collision_std.h" #include "collision_util.h" #include "heightfield.h" #if dTRIMESH_ENABLED #include "collision_trimesh_colliders.h" #endif // dTRIMESH_ENABLED #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(): m_fWidth( 0 ), m_fDepth( 0 ), m_fSampleWidth( 0 ), m_fSampleDepth( 0 ), m_fSampleZXAspect( 0 ), m_fInvSampleWidth( 0 ), m_fInvSampleDepth( 0 ), m_fHalfWidth( 0 ), m_fHalfDepth( 0 ), m_fMinHeight( 0 ), m_fMaxHeight( 0 ), m_fThickness( 0 ), m_fScale( 0 ), m_fOffset( 0 ), m_nWidthSamples( 0 ), m_nDepthSamples( 0 ), m_bCopyHeightData( 0 ), m_bWrapMode( 0 ), m_nGetHeightMode( 0 ), m_pHeightData( NULL ), m_pUserData( NULL ), m_pGetHeightCallback( NULL ) { memset( m_contacts, 0, sizeof( m_contacts ) ); } // 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 - REAL( 1.0 ) ); m_fSampleDepth = m_fDepth / ( m_nDepthSamples - REAL( 1.0 ) ); m_fSampleZXAspect = m_fSampleDepth / m_fSampleWidth; m_fInvSampleWidth = REAL( 1.0 ) / m_fSampleWidth; m_fInvSampleDepth = REAL( 1.0 ) / m_fSampleDepth; // finite or repeated terrain? m_bWrapMode = bWrapMode; } // recomputes heights bounds void dxHeightfieldData::ComputeHeightBounds() { int i; dReal h; unsigned char *data_byte; short *data_short; float *data_float; 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::IsOnHeightfield2 ( const HeightFieldVertex * const CellCorner, const dReal * const pos, const bool isABC) const { // WARNING!!! // This function must be written in the way to make sure that every point on // XZ plane falls in one and only one triangle. Keep that in mind if you // intend to change the code. // Also remember about computational errors and possible mismatches in // values if they are calculated differently in different places in the code. // Currently both the implementation has been optimized and effects of // computational errors have been eliminated. dReal MaxX, MinX; dReal MaxZ, MinZ; if (isABC) { // point A MinX = CellCorner->vertex[0]; if (pos[0] < MinX) return false; MaxX = (CellCorner->coords[0] + 1) * m_fSampleWidth; if (pos[0] >= MaxX) return false; MinZ = CellCorner->vertex[2]; if (pos[2] < MinZ) return false; MaxZ = (CellCorner->coords[1] + 1) * m_fSampleDepth; if (pos[2] >= MaxZ) return false; return (MaxZ - pos[2]) > (pos[0] - MinX) * m_fSampleZXAspect; } else { // point D MaxX = CellCorner->vertex[0]; if (pos[0] >= MaxX) return false; MinX = (CellCorner->coords[0] - 1) * m_fSampleWidth; if (pos[0] < MinX) return false; MaxZ = CellCorner->vertex[2]; if (pos[2] >= MaxZ) return false; MinZ = (CellCorner->coords[1] - 1) * m_fSampleDepth; if (pos[2] < MinZ) return false; return (MaxZ - pos[2]) <= (pos[0] - MinX) * m_fSampleZXAspect; } } // returns height at given sample coordinates dReal dxHeightfieldData::GetHeight( int x, int z ) { dReal h=0; unsigned char *data_byte; short *data_short; float *data_float; 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 = (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 ) ) // Use <= comparison to prefer simpler branch { 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() { unsigned char *data_byte; short *data_short; float *data_float; double *data_double; if ( m_bCopyHeightData ) { switch ( m_nGetHeightMode ) { // callback case 0: // do nothing break; // byte case 1: dIASSERT( m_pHeightData ); data_byte = (unsigned char*)m_pHeightData; delete [] data_byte; break; // short case 2: dIASSERT( m_pHeightData ); data_short = (short*)m_pHeightData; delete [] data_short; break; // float case 3: dIASSERT( m_pHeightData ); data_float = (float*)m_pHeightData; delete [] data_float; break; // double case 4: dIASSERT( m_pHeightData ); 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 if (d->m_fMinHeight != -dInfinity) { 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 ); } else { // Multiplication is performed to obtain infinity of correct sign dy[0] = ( final_posr->R[ 1] ? final_posr->R[ 1] * -dInfinity : REAL(0.0) ); dy[1] = ( final_posr->R[ 5] ? final_posr->R[ 5] * -dInfinity : REAL(0.0) ); dy[2] = ( final_posr->R[ 9] ? final_posr->R[ 9] * -dInfinity : REAL(0.0) ); } if (d->m_fMaxHeight != dInfinity) { 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 ); } else { dy[3] = ( final_posr->R[ 1] ? final_posr->R[ 1] * dInfinity : REAL(0.0) ); dy[4] = ( final_posr->R[ 5] ? final_posr->R[ 5] * dInfinity : REAL(0.0) ); dy[5] = ( final_posr->R[ 9] ? final_posr->R[ 9] * dInfinity : REAL(0.0) ); } #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(sizeint numTri) { sizeint alignedNumTri = AlignBufferSize(numTri, TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT); tempTriangleBufferSize = alignedNumTri; tempTriangleBuffer = new HeightFieldTriangle[alignedNumTri]; } void dxHeightfield::resetTriangleBuffer() { delete[] tempTriangleBuffer; } void dxHeightfield::allocatePlaneBuffer(sizeint numTri) { sizeint alignedNumTri = AlignBufferSize(numTri, TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT); tempPlaneBufferSize = alignedNumTri; tempPlaneBuffer = new HeightFieldPlane *[alignedNumTri]; tempPlaneInstances = new HeightFieldPlane[alignedNumTri]; HeightFieldPlane *ptrPlaneMatrix = tempPlaneInstances; for (sizeint indexTri = 0; indexTri != alignedNumTri; indexTri++) { tempPlaneBuffer[indexTri] = ptrPlaneMatrix; ptrPlaneMatrix += 1; } } void dxHeightfield::resetPlaneBuffer() { delete[] tempPlaneInstances; delete[] tempPlaneBuffer; } void dxHeightfield::allocateHeightBuffer(sizeint numX, sizeint numZ) { sizeint alignedNumX = AlignBufferSize(numX, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X); sizeint alignedNumZ = AlignBufferSize(numZ, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z); tempHeightBufferSizeX = alignedNumX; tempHeightBufferSizeZ = alignedNumZ; tempHeightBuffer = new HeightFieldVertex *[alignedNumX]; sizeint numCells = alignedNumX * alignedNumZ; tempHeightInstances = new HeightFieldVertex [numCells]; HeightFieldVertex *ptrHeightMatrix = tempHeightInstances; for (sizeint 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->m_p_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 sizeint numPlanes) { bool has_swapped = true; do { has_swapped = false;//reset flag for (sizeint 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; for ( x = minX, x_local = 0; x_local < numX; x++, x_local++) { Xpos = x * cfSampleWidth; // Always calculate pos via multiplication to avoid computational error accumulation during multiple additions const dReal c_Xpos = Xpos; HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local]; for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++) { Ypos = z * cfSampleDepth; // Always calculate pos via multiplication to avoid computational error accumulation during multiple additions 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; HeightFieldRow[z_local].coords[0] = x; HeightFieldRow[z_local].coords[1] = z; maxY = dMAX(maxY, h); minY = dMIN(minY, h); } } 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; pContact->side1 = -1; pContact->side2 = -1; return 1; } } // get All Planes that could collide against. dColliderFn *geomRayNCollider=0; dColliderFn *geomNPlaneCollider=0; dGetDepthFn *geomNDepthGetter=0; // 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; } /* -- This block is invalid as per Martijn Buijs The problem seems to be based on the erroneously assumption that if two of the four vertices of a 'grid' are at the same height, the entire grid can be represented as a single plane. It works for an axis aligned slope, but fails on all 4 grids of a 3x3 spike feature. Since the plane normal is constructed from only 3 vertices (only one of the two triangles) this often results in discontinuities at the grid edges (causing small jumps when the contact point moves from one grid to another). // 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 const unsigned int maxX_local = maxX - minX; const unsigned int maxZ_local = maxZ - minZ; for ( x_local = 0; x_local < maxX_local; 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_local = 0; z_local < maxZ_local; 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 = AHeight > minO2Height; const bool isBCollide = BHeight > minO2Height; const bool isCCollide = CHeight > minO2Height; const bool isDCollide = DHeight > minO2Height; A->state = !(isACollide); B->state = !(isBCollide); C->state = !(isCCollide); D->state = !(isDCollide); 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 && AHeight > BHeight && DHeight > CHeight && DHeight > BHeight)) { // 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 sizeint planeTriListSize = itPlane->trianglelistCurrentSize; for (i = 0; i < numPlaneContacts; i++) { dContactGeom *planeCurrContact = PlaneContact + i; // Check if contact point found in plane is inside Triangle. const dVector3 &pCPos = planeCurrContact->pos; for (sizeint b = 0; planeTriListSize > b; b++) { if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0], pCPos, itPlane->trianglelist[b]->isUp)) { pContact = CONTACT(contact, numTerrainContacts*skip); dVector3Copy(pCPos, pContact->pos); dOPESIGN(pContact->normal, =, -, itPlane->planeDef); pContact->depth = planeCurrContact->depth; pContact->side1 = planeCurrContact->side1; pContact->side2 = planeCurrContact->side2; 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 (sizeint 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 (sizeint 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 (sizeint 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) 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; pContact->side1 = -1; pContact->side2 = -1; 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 (sizeint m = 0; m < 3; m++) { const sizeint 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; int numTerrainOrigContacts = 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. dSubtractVectors3( 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; 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; } { // To narrow scope of following variables const dReal fInvSampleWidth = terrain->m_p_data->m_fInvSampleWidth; int nMinX = (int)dFloor(dNextAfter(o2->aabb[0] * fInvSampleWidth, -dInfinity)); int nMaxX = (int)dCeil(dNextAfter(o2->aabb[1] * fInvSampleWidth, dInfinity)); const dReal fInvSampleDepth = terrain->m_p_data->m_fInvSampleDepth; int nMinZ = (int)dFloor(dNextAfter(o2->aabb[4] * fInvSampleDepth, -dInfinity)); int nMaxZ = (int)dCeil(dNextAfter(o2->aabb[5] * fInvSampleDepth, dInfinity)); 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)); } numTerrainOrigContacts = numTerrainContacts; numTerrainContacts += terrain->dCollideHeightfieldZone( nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts, flags,CONTACT(contact,numTerrainContacts*skip),skip ); dIASSERT( numTerrainContacts <= numMaxTerrainContacts ); } dContactGeom *pContact; for ( i = numTerrainOrigContacts; i != numTerrainContacts; ++i ) { pContact = CONTACT(contact,i*skip); pContact->g1 = o1; pContact->g2 = o2; // pContact->side1 = -1; -- Oleh_Derevenko: sides must not be erased here as they are set by respective colliders during ray/plane tests // pContact->side2 = -1; } //------------------------------------------------------------------------------ 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); dCopyVector3( 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 ); dAddVectors3( pContact->pos, pContact->pos, terrain->final_posr->pos ); dCopyVector3( 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; }