add ode

1 files changed, 1876 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/heightfield.cpp b/libs/ode-0.16.1/ode/src/heightfield.cpp
new file mode 100644
index 0000000..71699db
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/heightfield.cpp
@@ -0,0 +1,1876 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.h>
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#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_nWidthSamples*m_nDepthSamples; i++)
+        {
+            h = data_byte[i];
+            if (h < m_fMinHeight)	m_fMinHeight = h;
+            if (h > 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_nWidthSamples*m_nDepthSamples; i++)
+        {
+            h = data_short[i];
+            if (h < m_fMinHeight)	m_fMinHeight = h;
+            if (h > 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_nWidthSamples*m_nDepthSamples; i++)
+        {
+            h = data_float[i];
+            if (h < m_fMinHeight)	m_fMinHeight = h;
+            if (h > m_fMaxHeight)	m_fMaxHeight = h;
+        }
+
+        break;
+
+        // double
+    case 4:
+        data_double = (double*)m_pHeightData;
+        m_fMinHeight = dInfinity;
+        m_fMaxHeight = -dInfinity;
+
+        for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
+        {
+            h = static_cast< dReal >( 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 <buijs512@planet.nl>
+
+    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;
+}
+
+
+