1 // dHeightfield Collider

     2 //  Paul Cheyrou-Lagreze aka Tuan Kuranes 2006 Speed enhancements http://www.pop-3d.com

     3 //  Martijn Buijs 2006 http://home.planet.nl/~buijs512/

     4 // Based on Terrain & Cone contrib by:

     5 //  Benoit CHAPEROT 2003-2004 http://www.jstarlab.com

     6 //  Some code inspired by Magic Software

     7 

     8 

     9 #include <ode/common.h>

    10 #include <ode/collision.h>

    11 #include <ode/matrix.h>

    12 #include <ode/rotation.h>

    13 #include <ode/odemath.h>

    14 #include "collision_kernel.h"

    15 #include "collision_std.h"

    16 #include "collision_util.h"

    17 #include "heightfield.h"

    18 

    19 #define TERRAINTOL REAL(0.0f)

    20 

    21 #define dMIN(A,B)  ((A)>(B) ? B : A)

    22 #define dMAX(A,B)  ((A)>(B) ? A : B)

    23 

    24 

    25 // Three-way MIN and MAX

    26 #define dMIN3(A,B,C)	( (A)<(B) ? dMIN((A),(C)) : dMIN((B),(C)) )

    27 #define dMAX3(A,B,C)	( (A)>(B) ? dMAX((A),(C)) : dMAX((B),(C)) )

    28 

    29 #define dOPESIGN(a, op1, op2,b) \

    30     (a)[0] op1 op2 ((b)[0]); \

    31     (a)[1] op1 op2 ((b)[1]); \

    32     (a)[2] op1 op2 ((b)[2]);

    33 

    34 #define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) {  \

    35     \

    36     dVector3Copy (MyPoint, myRay.final_posr->pos);   \

    37     myRay.final_posr->R[2] = MyVector[0];       \

    38     myRay.final_posr->R[6] = MyVector[1];       \

    39     myRay.final_posr->R[10] = MyVector[2];      \

    40     dGeomMoved (&myRay);                        \

    41             }

    42 

    43 #define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \

    44     \

    45     MyPlane->p[0] = MyPlaneDef[0];  \

    46     MyPlane->p[1] = MyPlaneDef[1];  \

    47     MyPlane->p[2] = MyPlaneDef[2];  \

    48     MyPlane->p[3] = MyPlaneDef[3];  \

    49     dGeomMoved (MyPlane);           \

    50                     }

    51 //////// dxHeightfieldData /////////////////////////////////////////////////////////////

    52 

    53 // dxHeightfieldData constructor

    54 dxHeightfieldData::dxHeightfieldData()

    55 {

    56     //

    57 }

    58 

    59 

    60 // build Heightfield data

    61 void dxHeightfieldData::SetData( int nWidthSamples, int nDepthSamples,

    62                                 dReal fWidth, dReal fDepth,

    63                                 dReal fScale, dReal fOffset, dReal fThickness,

    64                                 int bWrapMode )

    65 {

    66 

    67 

    68     // x,z bounds

    69     m_fWidth = fWidth;

    70     m_fDepth = fDepth;

    71 

    72     // cache half x,z bounds

    73     m_fHalfWidth = dDIV(fWidth,REAL( 2.0 ));

    74     m_fHalfDepth = dDIV(fDepth,REAL( 2.0 ));

    75 

    76     // scale and offset

    77     m_fScale = fScale;

    78     m_fOffset = fOffset;

    79 

    80     // infinite min height bounds

    81     m_fThickness = fThickness;

    82 

    83     // number of vertices per side

    84     m_nWidthSamples = nWidthSamples;

    85     m_nDepthSamples = nDepthSamples;

    86 

    87     m_fSampleWidth = m_fWidth / ( m_nWidthSamples - 1 );

    88     m_fSampleDepth = m_fDepth / ( m_nDepthSamples - 1 );

    89 

    90     m_fInvSampleWidth = dDIV(REAL(1.0),m_fSampleWidth);

    91     m_fInvSampleDepth = dDIV(REAL(1.0),m_fSampleDepth);

    92 

    93     // finite or repeated terrain?

    94     m_bWrapMode = bWrapMode;

    95 }

    96 

    97 

    98 // recomputes heights bounds

    99 void dxHeightfieldData::ComputeHeightBounds()

   100 {

   101     static int i;

   102     static dReal h;

   103     static dReal *data_float;

   104 

   105     data_float = (dReal*)m_pHeightData;

   106     m_fMinHeight = dInfinity;

   107     m_fMaxHeight = -dInfinity;

   108 

   109     for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)

   110     {

   111        h = data_float[i];

   112        if (h < m_fMinHeight)	m_fMinHeight = h;

   113        if (h > m_fMaxHeight)	m_fMaxHeight = h;

   114     }

   115 

   116     // scale and offset

   117     m_fMinHeight = dMUL(m_fMinHeight,m_fScale);

   118     m_fMaxHeight = dMUL(m_fMaxHeight,m_fScale);

   119     m_fMinHeight += m_fOffset;

   120     m_fMaxHeight += m_fOffset;

   121 

   122     // add thickness

   123     m_fMinHeight -= m_fThickness;

   124 }

   125 

   126 

   127 // returns whether point is over terrain Cell triangle?

   128 bool dxHeightfieldData::IsOnHeightfield  ( const dReal * const CellOrigin, const dReal * const pos,  const bool isABC) const

   129 {

   130     {

   131         const dReal MaxX = CellOrigin[0] + m_fSampleWidth;

   132         const dReal TolX = dMUL(m_fSampleWidth,TERRAINTOL);

   133         if ((pos[0]<CellOrigin[0]-TolX) || (pos[0]>MaxX+TolX))	

   134             return false;

   135     }

   136 

   137     {

   138         const dReal MaxZ = CellOrigin[2] + m_fSampleDepth;

   139         const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL);

   140         if ((pos[2]<CellOrigin[2]-TolZ) || (pos[2]>MaxZ+TolZ))	

   141             return false;

   142     }

   143 

   144     // add X percentage position on cell and Z percentage position on cell

   145     const dReal pctTotal = dMUL((pos[0] - CellOrigin[0]),m_fInvSampleWidth )

   146         + dMUL((pos[2] - CellOrigin[2]),m_fInvSampleDepth);

   147 

   148     if (isABC)

   149     {

   150         if (pctTotal >= REAL(1.0) + TERRAINTOL)	

   151             return false;

   152         else	

   153             return true;

   154     }

   155     else if (pctTotal <= REAL(1.0) - TERRAINTOL)	

   156     {

   157         return false;

   158     }

   159     return true;

   160 }

   161 // returns whether point is over terrain Cell triangle?

   162 bool dxHeightfieldData::IsOnHeightfield2  ( const dReal * const CellOrigin, const dReal * const pos,  const bool isABC) const

   163 {

   164     dReal MaxX, MinX;

   165     dReal MaxZ, MinZ;

   166     if (isABC)

   167     {

   168         // point A

   169         MinX = CellOrigin[0];

   170         MaxX = CellOrigin[0] + m_fSampleWidth;

   171 

   172         MinZ = CellOrigin[2];

   173         MaxZ = CellOrigin[2] + m_fSampleDepth;

   174     }

   175     else

   176     {

   177         // point D

   178         MinX = CellOrigin[0] - m_fSampleWidth;

   179         MaxX = CellOrigin[0];

   180 

   181         MinZ = CellOrigin[2] - m_fSampleDepth;

   182         MaxZ = CellOrigin[2];

   183     }

   184 

   185     // check if inside CELL

   186     {

   187         const dReal TolX = dMUL(m_fSampleWidth,TERRAINTOL);

   188         if ((pos[0]<MinX-TolX) || (pos[0]>MaxX+TolX))	

   189             return false;

   190     }

   191 

   192     {

   193         const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL);

   194         if ((pos[2]<MinZ-TolZ) || (pos[2]>MaxZ+TolZ))	

   195             return false;

   196     }

   197 

   198     // Sum up X percentage position on cell and Z percentage position on cell

   199     const dReal pctTotal = dMUL((pos[0] - MinX),m_fInvSampleWidth) 

   200         + dMUL((pos[2] - MinZ),m_fInvSampleDepth);

   201 

   202     // check if inside respective Triangle of Cell

   203     if (isABC)	

   204     {

   205         if (pctTotal >= REAL(1.0) + TERRAINTOL)	

   206             return false;

   207         else	

   208             return true;

   209     }

   210     else if (pctTotal <= REAL(1.0) - TERRAINTOL)	

   211     {

   212         return false;

   213     }

   214     return true;

   215 }

   216 

   217 

   218 // returns height at given sample coordinates

   219 dReal dxHeightfieldData::GetHeight( int x, int z )

   220 {

   221     static dReal h;

   222     static dReal *data_float;

   223 

   224     if ( m_bWrapMode == 0 )

   225     {

   226         // Finite

   227         if ( x < 0 ) x = 0;

   228         if ( z < 0 ) z = 0;

   229         if ( x > m_nWidthSamples - 1 ) x = m_nWidthSamples - 1;

   230         if ( z > m_nDepthSamples - 1 ) z = m_nDepthSamples - 1;

   231     }

   232     else

   233     {

   234         // Infinite

   235         x %= m_nWidthSamples - 1;

   236         z %= m_nDepthSamples - 1;

   237         if ( x < 0 ) x += m_nWidthSamples - 1;

   238         if ( z < 0 ) z += m_nDepthSamples - 1;

   239     }

   240 

   241     data_float = (dReal*)m_pHeightData;

   242     h = data_float[x+(z * m_nWidthSamples)];

   243 

   244     return dMUL(h,m_fScale) + m_fOffset;

   245 }

   246 

   247 

   248 // returns height at given coordinates

   249 dReal dxHeightfieldData::GetHeight( dReal x, dReal z )

   250 {

   251     int nX	= dMUL( x,m_fInvSampleWidth )>>QFACTOR;

   252     int nZ	= dMUL( z,m_fInvSampleDepth )>>QFACTOR;

   253 

   254     dReal dx = dMUL(( x - ( nX * m_fSampleWidth ) ),m_fInvSampleWidth);

   255     dReal dz = dMUL(( z - ( nZ * m_fSampleDepth ) ),m_fInvSampleDepth);

   256 

   257 

   258 

   259     dReal y, y0;

   260 

   261     if ( dx + dz < REAL( 1.0 ) )

   262     {

   263         y0 = GetHeight( nX, nZ );

   264 

   265         y = y0 + dMUL(( GetHeight( nX + 1, nZ ) - y0 ),dx)

   266             + dMUL(( GetHeight( nX, nZ + 1 ) - y0 ),dz);

   267     }

   268     else

   269     {

   270         y0 = GetHeight( nX + 1, nZ + 1 );

   271 

   272         y = y0	+ dMUL(( GetHeight( nX + 1, nZ ) - y0 ),( REAL(1.0f) - dz )) +

   273             dMUL(( GetHeight( nX, nZ + 1 ) - y0 ),( REAL(1.0f) - dx ));

   274     }

   275 

   276     return y;

   277 }

   278 

   279 

   280 // dxHeightfieldData destructor

   281 dxHeightfieldData::~dxHeightfieldData()

   282 {

   283     static dReal *data_float;

   284 

   285 

   286     if ( m_bCopyHeightData )

   287     {

   288         data_float = (dReal*)m_pHeightData;

   289         delete [] data_float;     

   290     }

   291 }

   292 

   293 

   294 //////// dxHeightfield /////////////////////////////////////////////////////////////////

   295 

   296 

   297 // dxHeightfield constructor

   298 dxHeightfield::dxHeightfield( dSpaceID space,

   299                              dHeightfieldDataID data,

   300                              int bPlaceable )			:

   301     dxGeom( space, bPlaceable ),

   302     tempPlaneBuffer(0),

   303     tempPlaneBufferSize(0),

   304     tempTriangleBuffer(0),

   305     tempTriangleBufferSize(0),

   306     tempHeightBuffer(0),

   307     tempHeightBufferSizeX(0),

   308     tempHeightBufferSizeZ(0)

   309 {

   310     type = dHeightfieldClass;

   311     this->m_p_data = data;

   312 }

   313 

   314 

   315 // compute axis aligned bounding box

   316 void dxHeightfield::computeAABB()

   317 {

   318     const dxHeightfieldData *d = m_p_data;

   319 

   320     if ( d->m_bWrapMode == 0 )

   321     {

   322         // Finite

   323         if ( gflags & GEOM_PLACEABLE )

   324         {

   325             dReal dx[6], dy[6], dz[6];

   326 

   327             // Y-axis

   328             dy[0] = dMUL( final_posr->R[ 1],d->m_fMinHeight );

   329             dy[1] = dMUL( final_posr->R[ 5],d->m_fMinHeight );

   330             dy[2] = dMUL( final_posr->R[ 9],d->m_fMinHeight );

   331             dy[3] = dMUL( final_posr->R[ 1],d->m_fMaxHeight );

   332             dy[4] = dMUL( final_posr->R[ 5],d->m_fMaxHeight );

   333             dy[5] = dMUL( final_posr->R[ 9],d->m_fMaxHeight );

   334 

   335 

   336             // X-axis

   337             dx[0] = dMUL( final_posr->R[ 0],-d->m_fHalfWidth );

   338             dx[1] = dMUL( final_posr->R[ 4],-d->m_fHalfWidth );

   339             dx[2] = dMUL( final_posr->R[ 8],-d->m_fHalfWidth );

   340             dx[3] = dMUL( final_posr->R[ 0],d->m_fHalfWidth );

   341             dx[4] = dMUL( final_posr->R[ 4],d->m_fHalfWidth );

   342             dx[5] = dMUL( final_posr->R[ 8],d->m_fHalfWidth );

   343 

   344             // Z-axis

   345             dz[0] = dMUL( final_posr->R[ 2],-d->m_fHalfDepth );

   346             dz[1] = dMUL( final_posr->R[ 6],-d->m_fHalfDepth );

   347             dz[2] = dMUL( final_posr->R[10],-d->m_fHalfDepth );

   348             dz[3] = dMUL( final_posr->R[ 2],d->m_fHalfDepth );

   349             dz[4] = dMUL( final_posr->R[ 6],d->m_fHalfDepth );

   350             dz[5] = dMUL( final_posr->R[10],d->m_fHalfDepth );

   351 

   352 

   353             // X extents

   354             aabb[0] = final_posr->pos[0] +

   355                 dMIN3( dMIN( dx[0], dx[3] ), dMIN( dy[0], dy[3] ), dMIN( dz[0], dz[3] ) );

   356             aabb[1] = final_posr->pos[0] +

   357                 dMAX3( dMAX( dx[0], dx[3] ), dMAX( dy[0], dy[3] ), dMAX( dz[0], dz[3] ) );

   358 

   359             // Y extents

   360             aabb[2] = final_posr->pos[1] +

   361                 dMIN3( dMIN( dx[1], dx[4] ), dMIN( dy[1], dy[4] ), dMIN( dz[1], dz[4] ) );

   362             aabb[3] = final_posr->pos[1] +

   363                 dMAX3( dMAX( dx[1], dx[4] ), dMAX( dy[1], dy[4] ), dMAX( dz[1], dz[4] ) );

   364 

   365             // Z extents

   366             aabb[4] = final_posr->pos[2] +

   367                 dMIN3( dMIN( dx[2], dx[5] ), dMIN( dy[2], dy[5] ), dMIN( dz[2], dz[5] ) );

   368             aabb[5] = final_posr->pos[2] +

   369                 dMAX3( dMAX( dx[2], dx[5] ), dMAX( dy[2], dy[5] ), dMAX( dz[2], dz[5] ) );

   370         }

   371         else

   372         {

   373 

   374             aabb[0] = -d->m_fHalfWidth;		aabb[1] = +d->m_fHalfWidth;

   375             aabb[2] = d->m_fMinHeight;		aabb[3] = d->m_fMaxHeight;

   376             aabb[4] = -d->m_fHalfDepth;		aabb[5] = +d->m_fHalfDepth;

   377 

   378 

   379         }

   380     }

   381     else

   382     {

   383         // Infinite

   384         if ( gflags & GEOM_PLACEABLE )

   385         {

   386             aabb[0] = -dInfinity;			aabb[1] = +dInfinity;

   387             aabb[2] = -dInfinity;			aabb[3] = +dInfinity;

   388             aabb[4] = -dInfinity;			aabb[5] = +dInfinity;

   389         }

   390         else

   391         {

   392             aabb[0] = -dInfinity;			aabb[1] = +dInfinity;

   393             aabb[2] = d->m_fMinHeight;		aabb[3] = d->m_fMaxHeight;

   394             aabb[4] = -dInfinity;			aabb[5] = +dInfinity;

   395         }

   396     }

   397 

   398 }

   399 

   400 

   401 // dxHeightfield destructor

   402 dxHeightfield::~dxHeightfield()

   403 {

   404     delete [] tempTriangleBuffer;

   405 

   406     for (unsigned int k = 0; k < tempPlaneBufferSize; k++)

   407     {

   408         delete tempPlaneBuffer[k];

   409     }

   410     delete [] tempPlaneBuffer;

   411 

   412     resetHeightBuffer();

   413 }

   414 

   415 void dxHeightfield::resetHeightBuffer()

   416 {

   417     const size_t xSize = tempHeightBufferSizeX;

   418     for (size_t x = 0; xSize < x; x++)

   419     {

   420         delete [] tempHeightBuffer[x];

   421     }

   422     delete [] tempHeightBuffer;

   423 }

   424 //////// Heightfield data interface ////////////////////////////////////////////////////

   425 

   426 

   427 EXPORT_C dHeightfieldDataID dGeomHeightfieldDataCreate()

   428 {

   429     return new dxHeightfieldData();

   430 }

   431 

   432 

   433 EXPORT_C void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d,

   434                                        void* pUserData, dHeightfieldGetHeight* pCallback,

   435                                        dReal width, dReal depth, int widthSamples, int depthSamples,

   436                                        dReal scale, dReal offset, dReal thickness, int bWrap )

   437 {

   438 

   439 

   440     // callback

   441     d->m_nGetHeightMode = 0;

   442     d->m_pUserData = pUserData;

   443     d->m_pGetHeightCallback = pCallback;

   444 

   445     // set info

   446     d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );

   447 

   448     // default bounds

   449     d->m_fMinHeight = -dInfinity;

   450     d->m_fMaxHeight = dInfinity;

   451 }

   452 

   453 EXPORT_C void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d,

   454                                      const float *pHeightData, int bCopyHeightData,

   455                                      dReal width, dReal depth, int widthSamples, int depthSamples,

   456                                      dReal scale, dReal offset, dReal thickness, int bWrap )

   457 {

   458 

   459 

   460     // set info

   461     d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );

   462     d->m_nGetHeightMode = 3;

   463     d->m_bCopyHeightData = bCopyHeightData;

   464 

   465     if ( d->m_bCopyHeightData == 0 )

   466     {

   467         // Data is referenced only.

   468         d->m_pHeightData = pHeightData;

   469     }

   470     else

   471     {

   472         // We own the height data, allocate storage

   473         d->m_pHeightData = new dReal[ d->m_nWidthSamples * d->m_nDepthSamples ];

   474 

   475 

   476         // Copy data.

   477         memcpy( (void*)d->m_pHeightData, pHeightData,

   478             sizeof( dReal ) * d->m_nWidthSamples * d->m_nDepthSamples );

   479     }

   480 

   481     // Find height bounds

   482     d->ComputeHeightBounds();

   483 }

   484 

   485 

   486 EXPORT_C void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight )

   487 {

   488 

   489     d->m_fMinHeight = dMUL( minHeight,d->m_fScale ) + d->m_fOffset - d->m_fThickness;

   490     d->m_fMaxHeight = dMUL( maxHeight,d->m_fScale ) + d->m_fOffset;

   491 }

   492 

   493 

   494 EXPORT_C void dGeomHeightfieldDataDestroy( dHeightfieldDataID d )

   495 {

   496     delete d;

   497 }

   498 

   499 

   500 //////// Heightfield geom interface ////////////////////////////////////////////////////

   501 

   502 

   503 EXPORT_C dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable )

   504 {

   505     return new dxHeightfield( space, data, bPlaceable );

   506 }

   507 

   508 

   509 EXPORT_C void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d )

   510 {

   511     dxHeightfield* geom = (dxHeightfield*) g;

   512     geom->data = d;

   513 }

   514 

   515 

   516 EXPORT_C dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g )

   517 {

   518     dxHeightfield* geom = (dxHeightfield*) g;

   519     return geom->m_p_data;

   520 }

   521 

   522 //////// dxHeightfield /////////////////////////////////////////////////////////////////

   523 

   524 

   525 // Typedef for generic 'get point depth' function

   526 typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z );

   527 

   528 

   529 

   530 static inline bool DescendingPlaneSort(const HeightFieldPlane * const A, const HeightFieldPlane * const B)

   531 {

   532     return ((A->maxAAAB - B->maxAAAB) > dEpsilon);

   533 }

   534 

   535 void dxHeightfield::sortPlanes(const size_t numPlanes)

   536 {

   537     bool has_swapped = true;

   538     do

   539     {

   540         has_swapped = false;//reset flag

   541         for (size_t i = 0; i < numPlanes - 1; i++)

   542         {

   543             //if they are in the wrong order

   544             if (DescendingPlaneSort(tempPlaneBuffer[i], tempPlaneBuffer[i + 1]))

   545             { 

   546                 //exchange them

   547                 HeightFieldPlane * tempPlane = tempPlaneBuffer[i];

   548                 tempPlaneBuffer[i] = tempPlaneBuffer[i + 1];

   549                 tempPlaneBuffer[i + 1] = tempPlane;

   550 

   551                 //we have swapped at least once, list may not be sorted yet

   552                 has_swapped = true;

   553             }

   554         }

   555     }    //if no swaps were made during this pass, the list has been sorted

   556     while (has_swapped);

   557 }

   558 

   559 int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ, 

   560                                            dxGeom* o2, const int numMaxContactsPossible,

   561                                            int flags, dContactGeom* contact, 

   562                                            int skip )

   563 {

   564 	dContactGeom *pContact = 0;

   565     int  x, z;

   566     // check if not above or inside terrain first

   567     // while filling a heightmap partial temporary buffer

   568     const unsigned int numX = (maxX - minX) + 1;

   569     const unsigned int numZ = (maxZ - minZ) + 1;

   570     const dReal minO2Height = o2->aabb[2];

   571     const dReal maxO2Height = o2->aabb[3];

   572     unsigned int x_local, z_local;

   573     dReal maxY = - dInfinity;

   574     dReal minY = dInfinity;

   575     // localize and const for faster access

   576     const dReal cfSampleWidth = m_p_data->m_fSampleWidth;

   577     const dReal cfSampleDepth = m_p_data->m_fSampleDepth;

   578     {

   579         if (tempHeightBufferSizeX < numX || tempHeightBufferSizeZ < numZ)

   580         {

   581             resetHeightBuffer();

   582             tempHeightBufferSizeX = numX;

   583             tempHeightBufferSizeZ = numZ;

   584             tempHeightBuffer = new HeightFieldVertex *[numX];

   585             for ( x_local = 0; x_local < numX; x_local++)

   586             {

   587                 tempHeightBuffer[x_local] = new HeightFieldVertex [numZ];     

   588             }

   589         }

   590 

   591         dReal Xpos, Ypos;

   592         Xpos = minX*cfSampleWidth;

   593 

   594 

   595         for ( x = minX, x_local = 0; x_local < numX; x++, x_local++)

   596         {

   597             const dReal c_Xpos = Xpos;

   598             HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];

   599             Ypos = minZ * cfSampleDepth;

   600             for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++)

   601             {

   602                 const dReal h = m_p_data->GetHeight(x, z);

   603                 HeightFieldRow[z_local].vertex[0] = c_Xpos;

   604                 HeightFieldRow[z_local].vertex[1] = h;

   605                 HeightFieldRow[z_local].vertex[2] = Ypos;

   606                 

   607 

   608                 maxY = dMAX(maxY, h);

   609                 minY = dMIN(minY, h);

   610 

   611 

   612                 Ypos += cfSampleDepth;

   613             }

   614             Xpos += cfSampleWidth;

   615         }

   616         if (minO2Height - maxY > -dEpsilon )

   617         {

   618 			//totally above heightfield

   619             return 0;

   620         }

   621 		if (minY - maxO2Height > -dEpsilon )

   622 		{

   623 			// totally under heightfield

   624 			pContact = CONTACT(contact, 0);

   625 

   626 			pContact->pos[0] = o2->final_posr->pos[0];

   627 			pContact->pos[1] = minY;

   628 			pContact->pos[2] = o2->final_posr->pos[2];

   629 

   630 			pContact->normal[0] = 0;

   631 			pContact->normal[1] = REAL(-1.0);

   632 			pContact->normal[2] = 0;

   633 

   634 			pContact->depth =  minY - maxO2Height;

   635 

   636 			return 1;

   637 		}

   638     }

   639     int numTerrainContacts = 0;

   640     // get All Planes that could collide against.

   641     dColliderFn *geomRayNCollider = 0;

   642     dColliderFn *geomNPlaneCollider = 0;

   643     dGetDepthFn *geomNDepthGetter = 0;

   644 

   645     int max_collisionContact = numMaxContactsPossible;

   646     switch (o2->type)

   647     {

   648     case dRayClass:

   649         geomRayNCollider		= NULL;

   650         geomNPlaneCollider	    = dCollideRayPlane;

   651         geomNDepthGetter		= NULL;

   652         //max_collisionContact    = 1;

   653         break;

   654 

   655     case dSphereClass:

   656         geomRayNCollider		= dCollideRaySphere;

   657         geomNPlaneCollider  	= dCollideSpherePlane;

   658         geomNDepthGetter		= dGeomSpherePointDepth;

   659         //max_collisionContact    = 3;

   660         break;

   661 

   662     case dBoxClass:

   663         geomRayNCollider		= dCollideRayBox;

   664         geomNPlaneCollider	    = dCollideBoxPlane;

   665         geomNDepthGetter		= dGeomBoxPointDepth;

   666         //max_collisionContact    = 8;

   667         break;

   668 

   669     case dCapsuleClass:

   670         geomRayNCollider		= dCollideRayCapsule;

   671         geomNPlaneCollider  	= dCollideCapsulePlane;

   672         geomNDepthGetter		= dGeomCapsulePointDepth;

   673         // max_collisionContact    = 3;

   674         break;

   675 

   676     case dCylinderClass:

   677         geomRayNCollider		= dCollideRayCylinder;

   678         geomNPlaneCollider	    = dCollideCylinderPlane;

   679         geomNDepthGetter		= NULL;// TODO: dGeomCCylinderPointDepth

   680         //max_collisionContact    = 3;

   681         break;

   682 

   683     case dConvexClass:

   684         geomRayNCollider		= dCollideRayConvex;

   685         geomNPlaneCollider  	= dCollideConvexPlane;

   686         geomNDepthGetter		= NULL;// TODO: dGeomConvexPointDepth;

   687         //max_collisionContact    = 3;

   688         break;

   689 

   690     default:

   691         	// Shouldn't ever get here.

   692         break;

   693 

   694     }

   695 

   696 

   697     const int numMaxContacts = dMIN (max_collisionContact, HEIGHTFIELDMAXCONTACTPERCELL);

   698     

   699     dxPlane myplane(0,0,0,0,0);

   700     dxPlane* sliding_plane = &myplane;

   701     dContactGeom *PlaneContact = m_p_data->m_contacts;    

   702     flags = (flags & 0xffff0000) | HEIGHTFIELDMAXCONTACTPERCELL;

   703     dReal triplane[4];

   704     int i;

   705 

   706     // check some trivial case.

   707     // Vector Up plane

   708     if (maxY - minY < dEpsilon)

   709     {

   710         // it's a single plane.

   711         triplane[0] = 0;

   712         triplane[1] = REAL(1.0);

   713         triplane[2] = 0;

   714         triplane[3] =  minY;

   715         dGeomPlaneSetNoNormalize (sliding_plane, triplane);

   716         // find collision and compute contact points

   717         const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, flags, PlaneContact, sizeof(dContactGeom));

   718         for (i = 0; i < numPlaneContacts; i++)

   719         {

   720             pContact = CONTACT(contact, numTerrainContacts*skip);

   721             const dVector3 &pCPos = PlaneContact[i].pos;

   722             dVector3Copy (pCPos, pContact->pos);

   723             dOPESIGN(pContact->normal, =, -, triplane);

   724 

   725             pContact->depth = PlaneContact[i].depth;

   726             numTerrainContacts++;

   727             if (numTerrainContacts > numMaxContactsPossible)

   728                 break;

   729         }

   730         return numTerrainContacts;

   731     }

   732     // unique plane

   733     {

   734         // check for very simple plane heightfield

   735         dReal minXHeightDelta = dInfinity, maxXHeightDelta = - dInfinity;

   736         dReal minZHeightDelta = dInfinity, maxZHeightDelta = - dInfinity;

   737 

   738 

   739         dReal lastXHeight = tempHeightBuffer[0][0].vertex[1];

   740         for ( x_local = 1; x_local < numX; x_local++)

   741         {

   742             HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];

   743 

   744             const dReal deltaX = HeightFieldRow[0].vertex[1] - lastXHeight;

   745 

   746             maxXHeightDelta = dMAX (maxXHeightDelta,  deltaX);

   747             minXHeightDelta = dMIN (minXHeightDelta,  deltaX);

   748 

   749             dReal lastZHeight = HeightFieldRow[0].vertex[1];

   750             for ( z_local = 1; z_local < numZ; z_local++)

   751             {

   752                 const dReal deltaZ = (HeightFieldRow[z_local].vertex[1] - lastZHeight);

   753 

   754                 maxZHeightDelta = dMAX (maxZHeightDelta,  deltaZ);

   755                 minZHeightDelta = dMIN (minZHeightDelta,  deltaZ);

   756 

   757             }

   758         }

   759 

   760         if (maxZHeightDelta - minZHeightDelta < dEpsilon && 

   761             maxXHeightDelta - minXHeightDelta < dEpsilon )

   762         {

   763             // it's a single plane.

   764             const dVector3 &A = tempHeightBuffer[0][0].vertex;

   765             const dVector3 &B = tempHeightBuffer[1][0].vertex;

   766             const dVector3 &C = tempHeightBuffer[0][1].vertex;

   767 

   768             // define 2 edges and a point that will define collision plane

   769             {

   770                 dVector3 Edge1, Edge2; 

   771                 dVector3Subtract(C, A, Edge1);

   772                 dVector3Subtract(B, A, Edge2);

   773                 dVector3Cross(Edge1, Edge2, triplane);

   774             }

   775 

   776             // Define Plane

   777             // Normalize plane normal

   778             const dReal dinvlength = dDIV(REAL(1.0),dVector3Length(triplane));

   779             triplane[0] = dMUL(triplane[0],dinvlength);

   780             triplane[1] = dMUL(triplane[1],dinvlength);

   781             triplane[2] = dMUL(triplane[2],dinvlength);

   782             // get distance to origin from plane 

   783             triplane[3] = dVector3Dot(triplane, A);

   784 

   785             dGeomPlaneSetNoNormalize (sliding_plane, triplane);

   786             // find collision and compute contact points

   787             const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, flags, PlaneContact, sizeof(dContactGeom));

   788             for (i = 0; i < numPlaneContacts; i++)

   789             {

   790                 pContact = CONTACT(contact, numTerrainContacts*skip);

   791                 const dVector3 &pCPos = PlaneContact[i].pos;

   792                 dVector3Copy (pCPos, pContact->pos);

   793                 dOPESIGN(pContact->normal, =, -, triplane);

   794 

   795                 pContact->depth = PlaneContact[i].depth;

   796                 numTerrainContacts++;

   797                 if (numTerrainContacts > numMaxContactsPossible)

   798                     break;

   799             }

   800             return numTerrainContacts;

   801         }

   802     }

   803 

   804 

   805     const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2;

   806     if (tempTriangleBufferSize < numTriMax)

   807     {

   808         delete [] tempTriangleBuffer;

   809         tempTriangleBufferSize = numTriMax;

   810         tempTriangleBuffer = new HeightFieldTriangle[numTriMax];

   811     }

   812     

   813     // Sorting triangle/plane  resulting from heightfield zone

   814     // Perhaps that would be necessary in case of too much limited

   815     // maximum contact point...

   816     // or in complex mesh case (trimesh and convex)

   817     // need some test or insights on this before enabling this.

   818     const bool isContactNumPointsLimited = 

   819         true;

   820     // (numMaxContacts < 8)

   821     //    || o2->type == dConvexClass

   822     //    || o2->type == dTriMeshClass

   823     //    || (numMaxContacts < (int)numTriMax)       

   824         

   825 

   826 

   827     // if small heightfield triangle related to O2 colliding

   828     // or no Triangle colliding at all.

   829     bool needFurtherPasses = (o2->type == dTriMeshClass);

   830     //compute Ratio between Triangle size and O2 aabb size

   831 	// no FurtherPasses are needed in ray class

   832     if (o2->type != dRayClass  && needFurtherPasses == false)

   833     {

   834         const dReal xratio = dMUL((o2->aabb[1] - o2->aabb[0]),m_p_data->m_fInvSampleWidth);

   835         if (xratio > REAL(1.5))

   836             needFurtherPasses = true;

   837         else

   838         {

   839             const dReal zratio = dMUL((o2->aabb[5] - o2->aabb[4]),m_p_data->m_fInvSampleDepth);

   840             if (zratio > REAL(1.5))

   841                 needFurtherPasses = true;

   842         }

   843 

   844     }

   845 

   846     unsigned int numTri = 0;

   847     HeightFieldVertex *A, *B, *C, *D;

   848     /*    (y is up)

   849          A--------B-...x

   850          |       /|

   851          |      / |

   852          |     /  |

   853          |    /   |

   854          |   /    |

   855          |  /     |

   856          | /      |

   857          |/       |

   858          C--------D   

   859          .

   860          .

   861          .

   862          z

   863     */  

   864     // keep only triangle that does intersect geom

   865     for ( x = minX, x_local = 0; x < maxX; x++, x_local++)

   866     {

   867         HeightFieldVertex *HeightFieldRow      = tempHeightBuffer[x_local];

   868         HeightFieldVertex *HeightFieldNextRow  = tempHeightBuffer[x_local + 1];

   869 

   870         // First A

   871         C = &HeightFieldRow    [0];

   872         // First B

   873         D = &HeightFieldNextRow[0];

   874         for ( z = minZ, z_local = 0; z < maxZ; z++, z_local++)

   875         {

   876             A = C;

   877             B = D;

   878 

   879             C = &HeightFieldRow    [z_local + 1];

   880             D = &HeightFieldNextRow[z_local + 1];

   881 

   882             const dReal AHeight = A->vertex[1];

   883             const dReal BHeight = B->vertex[1];

   884             const dReal CHeight = C->vertex[1];

   885             const dReal DHeight = D->vertex[1];

   886 

   887             const bool isACollide = 0 < AHeight - minO2Height;

   888             const bool isBCollide = 0 < BHeight - minO2Height;

   889             const bool isCCollide = 0 < CHeight - minO2Height;

   890             const bool isDCollide = 0 < DHeight - minO2Height;

   891 

   892             A->state = !(isACollide);

   893             B->state = !(isBCollide);

   894             C->state = !(isCCollide);

   895             D->state = !(isCCollide);

   896 

   897             if (isACollide || isBCollide || isCCollide)

   898             {

   899                 HeightFieldTriangle * const CurrTriUp = &tempTriangleBuffer[numTri++];

   900 

   901                 CurrTriUp->state = false;

   902 

   903                 // changing point order here implies to change it in isOnHeightField

   904                 CurrTriUp->vertices[0] = A;

   905                 CurrTriUp->vertices[1] = B;

   906                 CurrTriUp->vertices[2] = C;

   907 

   908                 if (isContactNumPointsLimited)

   909                     CurrTriUp->setMinMax();

   910                 CurrTriUp->isUp = true;

   911             }

   912 

   913             if (isBCollide || isCCollide || isDCollide)

   914             {

   915                 HeightFieldTriangle * const CurrTriDown = &tempTriangleBuffer[numTri++];

   916 

   917                 CurrTriDown->state = false;

   918                 // changing point order here implies to change it in isOnHeightField

   919 

   920                 CurrTriDown->vertices[0] = D;

   921                 CurrTriDown->vertices[1] = B;

   922                 CurrTriDown->vertices[2] = C;

   923 

   924 

   925                 if (isContactNumPointsLimited)

   926                     CurrTriDown->setMinMax();

   927                 CurrTriDown->isUp = false;

   928             }

   929 

   930 

   931             if (needFurtherPasses &&

   932                 (isBCollide || isCCollide)

   933                 &&

   934                 (AHeight - CHeight > 0 &&

   935                  AHeight - BHeight > 0 &&

   936                  DHeight - CHeight > 0 &&

   937                  DHeight - BHeight > 0))

   938             {

   939                 // That means Edge BC is concave, therefore

   940                 // BC Edge and B and C vertices cannot collide

   941 

   942                 B->state = true;

   943                 C->state = true;

   944             }

   945             // should find a way to check other edges (AB, BD, CD) too for concavity

   946         }

   947     }

   948 

   949     // at least on triangle should intersect geom

   950 

   951     // pass1: VS triangle as Planes

   952     // Group Triangle by same plane definition

   953     // as Terrain often has many triangles using same plane definition

   954     // then collide against that list of triangles.

   955     {

   956 

   957         dVector3 Edge1, Edge2;

   958         //compute all triangles normals.

   959         for (unsigned int k = 0; k < numTri; k++)

   960         {

   961             HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];

   962 

   963             // define 2 edges and a point that will define collision plane

   964             dVector3Subtract(itTriangle->vertices[2]->vertex, itTriangle->vertices[0]->vertex, Edge1);

   965             dVector3Subtract(itTriangle->vertices[1]->vertex, itTriangle->vertices[0]->vertex, Edge2);

   966 

   967             // find a perpendicular vector to the triangle

   968             if  (itTriangle->isUp)

   969                 dVector3Cross(Edge1, Edge2, triplane);

   970             else

   971                 dVector3Cross(Edge2, Edge1, triplane);

   972 

   973             // Define Plane

   974             // Normalize plane normal

   975             const dReal dinvlength = dDIV(1,dVector3Length(triplane));

   976             triplane[0] = dMUL(triplane[0],dinvlength);

   977             triplane[1] = dMUL(triplane[1],dinvlength);

   978             triplane[2] = dMUL(triplane[2],dinvlength);

   979             // get distance to origin from plane 

   980             triplane[3] = dVector3Dot(triplane, itTriangle->vertices[0]->vertex);

   981 

   982             // saves normal for collision check (planes, triangles, vertices and edges.)

   983             dVector3Copy(triplane, itTriangle->planeDef);

   984             // saves distance for collision check (planes, triangles, vertices and edges.)

   985             itTriangle->planeDef[3] = triplane[3];

   986         }

   987 

   988         // group by Triangles by Planes sharing shame plane definition

   989         if (tempPlaneBufferSize  < numTri)

   990         {

   991             delete [] tempPlaneBuffer;

   992             tempPlaneBufferSize = numTri;

   993             tempPlaneBuffer = new HeightFieldPlane *[numTri];

   994 

   995             for (unsigned int k = 0; k < tempPlaneBufferSize; k++)

   996             {

   997                 tempPlaneBuffer[k] = new HeightFieldPlane();

   998             }

   999         }

  1000         unsigned int numPlanes = 0;

  1001         for (unsigned int k = 0; k < numTri; k++)

  1002         {

  1003             HeightFieldTriangle * const tri_base = &tempTriangleBuffer[k];

  1004 

  1005             if (tri_base->state == true)

  1006                 continue;// already tested or added to plane list.

  1007 

  1008             HeightFieldPlane * const currPlane = tempPlaneBuffer[numPlanes];

  1009             currPlane->resetTriangleListSize(numTri - k);

  1010             currPlane->addTriangle(tri_base);

  1011             // saves normal for collision check (planes, triangles, vertices and edges.)

  1012             dVector3Copy(tri_base->planeDef, currPlane->planeDef);

  1013             // saves distance for collision check (planes, triangles, vertices and edges.)

  1014             currPlane->planeDef[3]= tri_base->planeDef[3];

  1015 

  1016             const dReal normx = tri_base->planeDef[0];

  1017             const dReal normy = tri_base->planeDef[1];

  1018             const dReal normz = tri_base->planeDef[2];

  1019             const dReal dist = tri_base->planeDef[3];

  1020 

  1021             for (unsigned int m = k + 1; m < numTri; m++)

  1022             {

  1023 

  1024                 HeightFieldTriangle * const tri_test = &tempTriangleBuffer[m];

  1025                 if (tri_test->state == true)

  1026                     continue;// already tested or added to plane list.

  1027 

  1028                 // normals and distance are the same.

  1029                 if (

  1030                     dFabs(normy - tri_test->planeDef[1]) < dEpsilon &&  

  1031                     dFabs(dist  - tri_test->planeDef[3]) < dEpsilon &&

  1032                     dFabs(normx - tri_test->planeDef[0]) < dEpsilon && 

  1033                     dFabs(normz - tri_test->planeDef[2]) < dEpsilon

  1034                     )

  1035                 {

  1036                     currPlane->addTriangle (tri_test);

  1037                     tri_test->state = true;

  1038                 }

  1039             }

  1040 

  1041             tri_base->state = true;

  1042             if (isContactNumPointsLimited)

  1043                 currPlane->setMinMax();

  1044 

  1045             numPlanes++;

  1046         }

  1047 

  1048         // sort planes

  1049         if (isContactNumPointsLimited)

  1050             sortPlanes(numPlanes);

  1051 

  1052         for (unsigned int k = 0; k < numPlanes; k++)

  1053         {

  1054             HeightFieldPlane * const itPlane = tempPlaneBuffer[k];

  1055 

  1056             //set Geom

  1057             dGeomPlaneSetNoNormalize (sliding_plane,  itPlane->planeDef);

  1058             //dGeomPlaneSetParams (sliding_plane, triangle_Plane[0], triangle_Plane[1], triangle_Plane[2], triangle_Plane[3]);

  1059             // find collision and compute contact points

  1060             bool didCollide = false;

  1061             const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, flags, PlaneContact, sizeof(dContactGeom));

  1062             const size_t planeTriListSize = itPlane->trianglelistCurrentSize;

  1063             for (i = 0; i < numPlaneContacts; i++)

  1064             {

  1065                 // Check if contact point found in plane is inside Triangle.

  1066                 const dVector3 &pCPos = PlaneContact[i].pos;

  1067                 bool isOnOneOfTrianglePlane = false;

  1068                 for (size_t b = 0; planeTriListSize > b; b++)

  1069                 {  

  1070                     if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0]->vertex, 

  1071                                                     pCPos, 

  1072                                                     itPlane->trianglelist[b]->isUp))

  1073                     {

  1074                         isOnOneOfTrianglePlane = true;

  1075                         break;

  1076                     }

  1077                 }

  1078                 if ( isOnOneOfTrianglePlane)

  1079                 {

  1080                     pContact = CONTACT(contact, numTerrainContacts*skip);

  1081                     dVector3Copy(pCPos, pContact->pos);

  1082                     dOPESIGN(pContact->normal, =, -, itPlane->planeDef);

  1083                     pContact->depth = PlaneContact[i].depth;

  1084                     didCollide = true;

  1085                     numTerrainContacts++;

  1086                     if ( numTerrainContacts == numMaxContacts ) 

  1087                         return numTerrainContacts;

  1088                 }

  1089             }

  1090             if (didCollide)

  1091             {

  1092                 for (size_t b = 0; planeTriListSize > b; b++)

  1093                 {                      

  1094                     // flag Triangles Vertices as collided 

  1095                     // to prevent any collision test of those

  1096                     for (i = 0; i < 3; i++)

  1097                         itPlane->trianglelist[b]->vertices[i]->state = true;

  1098                 }

  1099             }

  1100             else 

  1101             {

  1102                 // flag triangle as not collided so that Vertices or Edge

  1103                 // of that triangles will be checked.

  1104                 for (size_t b = 0; planeTriListSize > b; b++)

  1105                 { 

  1106                     itPlane->trianglelist[b]->state = false;

  1107                 }

  1108             }

  1109         }

  1110     }

  1111     

  1112 

  1113    

  1114     // pass2: VS triangle vertices

  1115     if (needFurtherPasses)

  1116     {

  1117         dxRay tempRay(0, 1); 

  1118         dReal depth;

  1119         bool vertexCollided;

  1120         //

  1121         // Find Contact Penetration Depth of each vertices

  1122         //

  1123         for (unsigned int k = 0; k < numTri; k++)

  1124         {

  1125             const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];

  1126             if (itTriangle->state == true)

  1127                 continue;// plane triangle did already collide.

  1128 

  1129             for (size_t i = 0; i < 3; i++)

  1130             {

  1131                 HeightFieldVertex *vertex = itTriangle->vertices[i];

  1132                 if (vertex->state == true)

  1133                     continue;// vertice did already collide.

  1134 

  1135                 vertexCollided = false;