// 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/matrix.h>

#include <ode/rotation.h>

#include <ode/odemath.h>

#include "collision_kernel.h"

#include "collision_std.h"

#include "collision_util.h"

#include "heightfield.h"

#define TERRAINTOL REAL(0.0f)

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

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

// Three-way MIN and MAX

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

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

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

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

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

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

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

    \

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

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

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

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

    dGeomMoved (&myRay);                        \

            }

#define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \

    \

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

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

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

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

    dGeomMoved (MyPlane);           \

                    }

//////// dxHeightfieldData /////////////////////////////////////////////////////////////

// dxHeightfieldData constructor

dxHeightfieldData::dxHeightfieldData()

{

    //

}

// build Heightfield data

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

                                dReal fWidth, dReal fDepth,

                                dReal fScale, dReal fOffset, dReal fThickness,

                                int bWrapMode )

{

    // x,z bounds

    m_fWidth = fWidth;

    m_fDepth = fDepth;

    // cache half x,z bounds

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

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

    // scale and offset

    m_fScale = fScale;

    m_fOffset = fOffset;

    // infinite min height bounds

    m_fThickness = fThickness;

    // number of vertices per side

    m_nWidthSamples = nWidthSamples;

    m_nDepthSamples = nDepthSamples;

    m_fSampleWidth = m_fWidth / ( m_nWidthSamples - 1 );

    m_fSampleDepth = m_fDepth / ( m_nDepthSamples - 1 );

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

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

    // finite or repeated terrain?

    m_bWrapMode = bWrapMode;

}

// recomputes heights bounds

void dxHeightfieldData::ComputeHeightBounds()

{

    static int i;

    static dReal h;

    static dReal *data_float;

    data_float = (dReal*)m_pHeightData;

    m_fMinHeight = dInfinity;

    m_fMaxHeight = -dInfinity;

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

    {

       h = data_float[i];

       if (h < m_fMinHeight)	m_fMinHeight = h;

       if (h > m_fMaxHeight)	m_fMaxHeight = h;

    }

    // scale and offset

    m_fMinHeight = dMUL(m_fMinHeight,m_fScale);

    m_fMaxHeight = dMUL(m_fMaxHeight,m_fScale);

    m_fMinHeight += m_fOffset;

    m_fMaxHeight += m_fOffset;

    // add thickness

    m_fMinHeight -= m_fThickness;

}

// returns whether point is over terrain Cell triangle?

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

{

    {

        const dReal MaxX = CellOrigin[0] + m_fSampleWidth;

        const dReal TolX = dMUL(m_fSampleWidth,TERRAINTOL);

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

            return false;

    }

    {

        const dReal MaxZ = CellOrigin[2] + m_fSampleDepth;

        const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL);

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

            return false;

    }

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

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

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

    if (isABC)

    {

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

            return false;

        else	

            return true;

    }

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

    {

        return false;

    }

    return true;

}

// returns whether point is over terrain Cell triangle?

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

{

    dReal MaxX, MinX;

    dReal MaxZ, MinZ;

    if (isABC)

    {

        // point A

        MinX = CellOrigin[0];

        MaxX = CellOrigin[0] + m_fSampleWidth;

        MinZ = CellOrigin[2];

        MaxZ = CellOrigin[2] + m_fSampleDepth;

    }

    else

    {

        // point D

        MinX = CellOrigin[0] - m_fSampleWidth;

        MaxX = CellOrigin[0];

        MinZ = CellOrigin[2] - m_fSampleDepth;

        MaxZ = CellOrigin[2];

    }

    // check if inside CELL

    {

        const dReal TolX = dMUL(m_fSampleWidth,TERRAINTOL);

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

            return false;

    }

    {

        const dReal TolZ = dMUL(m_fSampleDepth,TERRAINTOL);

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

            return false;

    }

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

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

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

    // check if inside respective Triangle of Cell

    if (isABC)	

    {

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

            return false;

        else	

            return true;

    }

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

    {

        return false;

    }

    return true;

}

// returns height at given sample coordinates

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

{

    static dReal h;

    static dReal *data_float;

    if ( m_bWrapMode == 0 )

    {

        // Finite

        if ( x < 0 ) x = 0;

        if ( z < 0 ) z = 0;

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

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

    }

    else

    {

        // Infinite

        x %= m_nWidthSamples - 1;

        z %= m_nDepthSamples - 1;

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

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

    }

    data_float = (dReal*)m_pHeightData;

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

    return dMUL(h,m_fScale) + m_fOffset;

}

// returns height at given coordinates

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

{

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

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

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

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

    dReal y, y0;

    if ( dx + dz < REAL( 1.0 ) )

    {

        y0 = GetHeight( nX, nZ );

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

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

    }

    else

    {

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

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

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

    }

    return y;

}

// dxHeightfieldData destructor

dxHeightfieldData::~dxHeightfieldData()

{

    static dReal *data_float;

    if ( m_bCopyHeightData )

    {

        data_float = (dReal*)m_pHeightData;

        delete [] data_float;     

    }

}

//////// dxHeightfield /////////////////////////////////////////////////////////////////

// dxHeightfield constructor

dxHeightfield::dxHeightfield( dSpaceID space,

                             dHeightfieldDataID data,

                             int bPlaceable )			:

    dxGeom( space, bPlaceable ),

    tempPlaneBuffer(0),

    tempPlaneBufferSize(0),

    tempTriangleBuffer(0),

    tempTriangleBufferSize(0),

    tempHeightBuffer(0),

    tempHeightBufferSizeX(0),

    tempHeightBufferSizeZ(0)

{

    type = dHeightfieldClass;

    this->m_p_data = data;

}

// compute axis aligned bounding box

void dxHeightfield::computeAABB()

{

    const dxHeightfieldData *d = m_p_data;

    if ( d->m_bWrapMode == 0 )

    {

        // Finite

        if ( gflags & GEOM_PLACEABLE )

        {

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

            // Y-axis

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

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

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

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

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

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

            // X-axis

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

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

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

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

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

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

            // Z-axis

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

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

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

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

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

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

            // X extents

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

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

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

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

            // Y extents

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

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

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

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

            // Z extents

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

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

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

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

        }

        else

        {

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

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

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

        }

    }

    else

    {

        // Infinite

        if ( gflags & GEOM_PLACEABLE )

        {

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

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

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

        }

        else

        {

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

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

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

        }

    }

}

// dxHeightfield destructor

dxHeightfield::~dxHeightfield()

{

    delete [] tempTriangleBuffer;

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

    {

        delete tempPlaneBuffer[k];

    }

    delete [] tempPlaneBuffer;

    resetHeightBuffer();

}

void dxHeightfield::resetHeightBuffer()

{

    const size_t xSize = tempHeightBufferSizeX;

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

    {

        delete [] tempHeightBuffer[x];

    }

    delete [] tempHeightBuffer;

}

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

EXPORT_C dHeightfieldDataID dGeomHeightfieldDataCreate()

{

    return new dxHeightfieldData();

}

EXPORT_C void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d,

                                       void* pUserData, dHeightfieldGetHeight* pCallback,

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

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

{

    // callback

    d->m_nGetHeightMode = 0;

    d->m_pUserData = pUserData;

    d->m_pGetHeightCallback = pCallback;

    // set info

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

    // default bounds

    d->m_fMinHeight = -dInfinity;

    d->m_fMaxHeight = dInfinity;

}

EXPORT_C void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d,

                                     const float *pHeightData, int bCopyHeightData,

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

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

{

    // set info

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

    d->m_nGetHeightMode = 3;

    d->m_bCopyHeightData = bCopyHeightData;

    if ( d->m_bCopyHeightData == 0 )

    {

        // Data is referenced only.

        d->m_pHeightData = pHeightData;

    }

    else

    {

        // We own the height data, allocate storage

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

        // Copy data.

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

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

    }

    // Find height bounds

    d->ComputeHeightBounds();

}

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

{

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

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

}

EXPORT_C void dGeomHeightfieldDataDestroy( dHeightfieldDataID d )

{

    delete d;

}

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

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

{

    return new dxHeightfield( space, data, bPlaceable );

}

EXPORT_C void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d )

{

    dxHeightfield* geom = (dxHeightfield*) g;

    geom->data = d;

}

EXPORT_C dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g )

{

    dxHeightfield* geom = (dxHeightfield*) g;

    return geom->m_p_data;

}

//////// dxHeightfield /////////////////////////////////////////////////////////////////

// Typedef for generic 'get point depth' function

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