GeometryHandlers.cpp

Go to the documentation of this file.

//-----------------------------------------------------------------------------------
//
// Pro-Vocation Light Engine (PVLE)
// Copyright (C) 2007-2009  Sukender, KinoX & Buzib
// For more information, contact us : sukender@free.fr
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 3 of the License, or
// (at your option) any later version.
//
// For any use that is not compatible with the terms of the GNU 
// General Public License, please contact the authors for alternative
// licensing options.
//
// This program 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
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//-----------------------------------------------------------------------------------

#include <PVLE/Physics/GeometryHandlers.h>

#include <PVLE/Util/TracedException.h>
#include <PVLE/Util/Util.h>
#include <PVLE/Physics/Geom.h>

// For MeshHandler
#include <osg/Array>
#include <osg/Vec3>
#include <osg/Node>
#include <osgDB/ReadFile>
#include <boost/cast.hpp>
#include <PVLE/3D/FindGeometriesVisitor.h>
#include <PVLE/3D/Commons.h>
#include <PVLE/3D/Utility3D.h>

namespace Physics {


UserGeomsInitializer::UserGeomsInitializer() {
    // Register ALL user classes IN THE CORRECT ORDER so that ODE class ID matches PVLE class ID.

/*
    // Terrain
    dGeomClass geomClass;
    geomClass.bytes = 0;        // Nothing to store in the ODE geom
    geomClass.collider = TerrainHandler::collide;
    //geomClass.aabb = TerrainHandler::aabb;
    geomClass.aabb = dInfiniteAABB;
    //geomClass.aabb_test = TerrainHandler::aabb_test;
    geomClass.aabb_test = NULL;
    geomClass.dtor = NULL;
    int odeClassID = dCreateGeomClass(&geomClass);

    ASSERT(odeClassID == GeometryHandler::TERRAIN);
*/
}


void MeshHandler::indexTriangles(UINT lengthPrimitive, UINT offset) {
    UINT max = offset + lengthPrimitive;
    // Indexes : 0 1 2, 3 4 5 ...
    pLocalIndexArray->reserve(pLocalIndexArray->size() + lengthPrimitive);
    for(UINT i=offset; i<max; ++i) pLocalIndexArray->push_back(i);
}

template<class TDrawElements>
void MeshHandler::indexTriangles(const TDrawElements * pPrimitiveSet) {
    UINT lengthPrimitive = pPrimitiveSet->getNumIndices();
    pLocalIndexArray->reserve(pLocalIndexArray->size() + lengthPrimitive);
    for(typename TDrawElements::const_iterator itData = pPrimitiveSet->begin(), itDataEnd = pPrimitiveSet->end(); itData != itDataEnd; ++itData) pLocalIndexArray->push_back(*itData);
}


void MeshHandler::indexTriangleStrip(UINT lengthPrimitive, UINT offset) {
    UINT max = offset + lengthPrimitive;
    // Indexes : 0 1 2, 1 3 2, 2 3 4 ...
    pLocalIndexArray->reserve(pLocalIndexArray->size() + (lengthPrimitive-2)*3);
    bool invert = false;
    for(UINT i=offset+2; i<max; ++i) {
        pLocalIndexArray->push_back(i-2);
        if (invert) {
            pLocalIndexArray->push_back(i);
            pLocalIndexArray->push_back(i-1);
        } else {
            pLocalIndexArray->push_back(i-1);
            pLocalIndexArray->push_back(i);
        }
        invert = !invert;
    }
}

template<class TDrawElements>
void MeshHandler::indexTriangleStrip(const TDrawElements * pPrimitiveSet) {
    UINT lengthPrimitive = pPrimitiveSet->getNumIndices();
    pLocalIndexArray->reserve(pLocalIndexArray->size() + (lengthPrimitive-2)*3);
    typename TDrawElements::const_iterator
        itData = pPrimitiveSet->begin(),
        itDataEnd = pPrimitiveSet->end();

    bool invert = false;
    GLuint a, b, c;
    ASSERT(itData != itDataEnd);    a = *itData;    ++itData;
    ASSERT(itData != itDataEnd);    b = *itData;    ++itData;
    for(; itData != itDataEnd; ++itData) {
        c = *itData;
        pLocalIndexArray->push_back(a);
        if (invert) {
            pLocalIndexArray->push_back(c);
            pLocalIndexArray->push_back(b);
        } else {
            pLocalIndexArray->push_back(b);
            pLocalIndexArray->push_back(c);
        }
        invert = !invert;
        a=b;    b=c;
    }
}


void MeshHandler::indexTriangleFan(UINT lengthPrimitive, UINT offset) {
    UINT max = offset + lengthPrimitive;
    // Indexes : 0 1 2, 0 2 3, 0 3 4 ...
    pLocalIndexArray->reserve(pLocalIndexArray->size() + (lengthPrimitive-1)*3);
    for(UINT i=offset+2; i<max; ++i) {
        pLocalIndexArray->push_back(0);
        pLocalIndexArray->push_back(i-1);
        pLocalIndexArray->push_back(i);
    }
}

template<class TDrawElements>
void MeshHandler::indexTriangleFan(const TDrawElements * pPrimitiveSet) {
    UINT lengthPrimitive = pPrimitiveSet->getNumIndices();
    pLocalIndexArray->reserve(pLocalIndexArray->size() + (lengthPrimitive-1)*3);

    typename TDrawElements::const_iterator
        itData = pPrimitiveSet->begin(),
        itDataEnd = pPrimitiveSet->end();

    GLuint o, prev, cur;
    ASSERT(itData != itDataEnd);    o = *itData;    ++itData;
    ASSERT(itData != itDataEnd);    prev = *itData; ++itData;
    for(; itData != itDataEnd; ++itData) {
        cur = *itData;
        pLocalIndexArray->push_back(o);
        pLocalIndexArray->push_back(prev);
        pLocalIndexArray->push_back(cur);
        prev=cur;
    }
}

void MeshHandler::indexQuads(UINT lengthPrimitive, UINT offset) {
    UINT max = offset + lengthPrimitive;
    // Indexes : 0 1 2, 0 2 3, 4 5 6, 4 6 7 ...
    pLocalIndexArray->reserve(pLocalIndexArray->size() + lengthPrimitive*6/4);
    for(UINT i=offset; i<max; i+=4) {
        pLocalIndexArray->push_back(i);
        pLocalIndexArray->push_back(i+1);
        pLocalIndexArray->push_back(i+2);

        pLocalIndexArray->push_back(i);
        pLocalIndexArray->push_back(i+2);
        pLocalIndexArray->push_back(i+3);
    }
}

template<class TDrawElements>
void MeshHandler::indexQuads(const TDrawElements * pPrimitiveSet) {
    UINT lengthPrimitive = pPrimitiveSet->getNumIndices();
    pLocalIndexArray->reserve(pLocalIndexArray->size() + lengthPrimitive*6/4);

    typename TDrawElements::const_iterator
        itData = pPrimitiveSet->begin(),
        itDataEnd = pPrimitiveSet->end();

    GLuint a, b, c, d;
    for(; itData != itDataEnd; ) {
                                        a = *itData;    ++itData;
        ASSERT(itData != itDataEnd);    b = *itData;    ++itData;
        ASSERT(itData != itDataEnd);    c = *itData;    ++itData;
        ASSERT(itData != itDataEnd);    d = *itData;    ++itData;

        pLocalIndexArray->push_back(a);
        pLocalIndexArray->push_back(b);
        pLocalIndexArray->push_back(c);

        pLocalIndexArray->push_back(a);
        pLocalIndexArray->push_back(c);
        pLocalIndexArray->push_back(d);
    }
}


void MeshHandler::indexQuadStrip(UINT lengthPrimitive, UINT offset) {
    UINT max = offset + lengthPrimitive;
    // Indexes : 0 1 3, 0 3 2, 2 3 5, 2 5 4 ...
    pLocalIndexArray->reserve(pLocalIndexArray->size() + (lengthPrimitive-2)*3);
    for(UINT i=offset+3; i<max; i+=2) {
        pLocalIndexArray->push_back(i-3);
        pLocalIndexArray->push_back(i-2);
        pLocalIndexArray->push_back(i);

        pLocalIndexArray->push_back(i-3);
        pLocalIndexArray->push_back(i);
        pLocalIndexArray->push_back(i-1);
    }
}

template<class TDrawElements>
void MeshHandler::indexQuadStrip(const TDrawElements * pPrimitiveSet) {
    UINT lengthPrimitive = pPrimitiveSet->getNumIndices();
    pLocalIndexArray->reserve(pLocalIndexArray->size() + (lengthPrimitive-2)*3);

    typename TDrawElements::const_iterator
        itData = pPrimitiveSet->begin(),
        itDataEnd = pPrimitiveSet->end();

    GLuint a, b, c, d;
    ASSERT(itData != itDataEnd);    a = *itData;    ++itData;
    ASSERT(itData != itDataEnd);    b = *itData;    ++itData;
    for(; itData != itDataEnd; ) {
                                        c = *itData;    ++itData;
        ASSERT(itData != itDataEnd);    d = *itData;    ++itData;

        pLocalIndexArray->push_back(a);
        pLocalIndexArray->push_back(b);
        pLocalIndexArray->push_back(d);

        pLocalIndexArray->push_back(a);
        pLocalIndexArray->push_back(d);
        pLocalIndexArray->push_back(c);

        a = c;  b = d;
    }
}


template <class TDrawElements>
void MeshHandler::indexByType(const osg::PrimitiveSet * pPrimitiveSet) {
    const TDrawElements * pCastPrimitive = boost::polymorphic_downcast<const TDrawElements *>(pPrimitiveSet);
    switch(pCastPrimitive->getMode()) {
        case osg::PrimitiveSet::TRIANGLES:      indexTriangles<TDrawElements>(pCastPrimitive); break;
        case osg::PrimitiveSet::TRIANGLE_STRIP: indexTriangleStrip<TDrawElements>(pCastPrimitive); break;
        case osg::PrimitiveSet::TRIANGLE_FAN:   indexTriangleFan<TDrawElements>(pCastPrimitive); break;
        case osg::PrimitiveSet::QUADS:          indexQuads<TDrawElements>(pCastPrimitive); break;
        case osg::PrimitiveSet::QUAD_STRIP:     indexQuadStrip<TDrawElements>(pCastPrimitive); break;
        default: THROW_TRACED_EXCEPTION("Unsupported primitive mode for physics (Handles only triangles (list/strip/fan) and quads (list/strip), not points or lines)");
    }
}



dGeomID MeshHandler::create(dSpaceID spaceId) {
    // Build mesh data
    const osg::Array * pV = pGeo->getVertexArray();
    const TIndexArray *pI = boost::polymorphic_downcast<const TIndexArray *>(pGeo->getVertexIndices());
    const osg::Array * pN = pGeo->getNormalArray();

    // Working on vertices
    ASSERT(pV->getType() == osg::Array::Vec3ArrayType);

    // Working on normals
    if (pGeo->getNormalBinding() == osg::Geometry::BIND_PER_PRIMITIVE) {
        // Normal data is available by primitive (triangle or quad).
        LOG_NOTICE << "NOT IMPLMENTED : Normal data is BIND_PER_PRIMITIVE. Fallback to autocompute." << std::endl;
        pN = NULL;
    } else if (pGeo->getNormalBinding() == osg::Geometry::BIND_PER_VERTEX) {
        // Normal data is available by vertex.
        LOG_NOTICE << "NOT IMPLMENTED : Normal data is BIND_PER_VERTEX. Fallback to autocompute." << std::endl;
        pN = NULL;
    } else {
        LOG_NOTICE << "Normal data is unuseable for physics (not BIND_PER_VERTEX or BIND_BY_PRIMITIVE). Using no normal data." << std::endl;
        pN = NULL;
    }
    if (!pN) LOG_WARN << "[Performance warning] Normal data is unavailable for physics. Fallback to auto-computation by physics engine (will compute each time necessary)." << std::endl;

    ASSERT(!pN || pN->getType() == osg::Array::Vec3ArrayType);

    // Working on indices

    // *** For dev only ***
    if (pI) THROW_TRACED_EXCEPTION("DEV ONLY : Direct index array is encountered (not implemented).");

    LOG_INFO << "Direct index data unavailable for physics. Rebuilding." << std::endl;
    if (pGeo->getNumPrimitiveSets() == 0) THROW_TRACED_EXCEPTION("No primitive found");

    pLocalIndexArray = new TIndexArray;
    pI = boost::polymorphic_downcast<const TIndexArray *>(pLocalIndexArray.get());

    UINT nbVertex = pV->getNumElements();

    osg::Geometry::PrimitiveSetList::const_iterator
        it = pGeo->getPrimitiveSetList().begin(),
        itEnd = pGeo->getPrimitiveSetList().end();
    for(; it != itEnd; ++it) {          // for all primitives sets
        // Select a type-depending method
        osg::PrimitiveSet::Type primType = (*it)->getType();

        // Indexed primitive : copy indexes
        // Non-indexed primitive : recreate indexes, depending on the mode

        if      (primType == osg::PrimitiveSet::DrawElementsUIntPrimitiveType)   indexByType<osg::DrawElementsUInt>(it->get());
        else if (primType == osg::PrimitiveSet::DrawElementsUShortPrimitiveType) indexByType<osg::DrawElementsUShort>(it->get());
        else if (primType == osg::PrimitiveSet::DrawElementsUBytePrimitiveType)  indexByType<osg::DrawElementsUByte>(it->get());
        else {
            // Non-indexed primitive
            // Set the appropriate indexing method
            void (MeshHandler::* indexFunc)(UINT, UINT) = NULL;     // Pointer to an index*() function
            switch((*it)->getMode()) {
                case osg::PrimitiveSet::TRIANGLES:      indexFunc = &MeshHandler::indexTriangles; break;
                case osg::PrimitiveSet::TRIANGLE_STRIP: indexFunc = &MeshHandler::indexTriangleStrip; break;
                case osg::PrimitiveSet::TRIANGLE_FAN:   indexFunc = &MeshHandler::indexTriangleFan; break;
                case osg::PrimitiveSet::QUADS:          indexFunc = &MeshHandler::indexQuads; break;
                case osg::PrimitiveSet::QUAD_STRIP:     indexFunc = &MeshHandler::indexQuadStrip; break;
                default: THROW_TRACED_EXCEPTION("Unsupported primitive mode for physics (Handles only triangles and quad, not points or lines)");
            }

            if (primType == osg::PrimitiveSet::DrawArraysPrimitiveType) {
                THROW_TRACED_EXCEPTION("Implementation needs testing : never encountered case");
                // One GL primitive
                // Length of the primitive == nbVertex
                (this->*indexFunc)(nbVertex, 0);
            }
            else if (primType == osg::PrimitiveSet::DrawArrayLengthsPrimitiveType) {
                // Multiple GL primitives

                // For all numbers in primitive data (primitive is a vector<GLSizei>)
                GLsizei offset = 0;
                osg::DrawArrayLengths::const_iterator
                    itData = boost::polymorphic_downcast<const osg::DrawArrayLengths *>(it->get())->begin(),
                    itDataEnd = boost::polymorphic_downcast<const osg::DrawArrayLengths *>(it->get())->end();
                for(; itData != itDataEnd; ++itData) {
                    (this->*indexFunc)(*itData, offset);
                    offset += *itData;
                }
            }
            else THROW_TRACED_EXCEPTION("Unknown primitive type");

        }   // for all primitive sets
    }


    // --- DEBUG section ---
    // Sukender : I don't know why, but pV->getDataSize() returns 3 (OSG 1.2) !!! It should return sizeof(osg::Vec3f) == 12.
    // Actually, vertex array is not of type
    //      TemplateArray<Vec3f, Vec3ArrayType, sizeof(osg::Vec3f), GL_FLOAT>, but
    //      TemplateArray<Vec3f, Vec3ArrayType, 3                 , GL_FLOAT>,
    // which of course lead to an error while calling pV->getDataSize().
    // So we use "pV->getTotalDataSize() / pV->getNumElements()" for dGeomTriMeshDataBuild*1().
    //int DEBUG_a = pV->getDataSize();
    //int DEBUG_b = pI->getDataSize();
    //int DEBUG_c = pV->getTotalDataSize() / pV->getNumElements();
    //int DEBUG_d = sizeof(float)*3;
    //const osg::TemplateArray< osg::Vec3f, osg::Array::Vec3ArrayType, sizeof(osg::Vec3f), GL_FLOAT > * p1 = dynamic_cast<const osg::TemplateArray< osg::Vec3f, osg::Array::Vec3ArrayType, sizeof(osg::Vec3f), GL_FLOAT > *>(pV);
    //const osg::TemplateArray< osg::Vec3f, osg::Array::Vec3ArrayType, 3, GL_FLOAT > * p2 = dynamic_cast<const osg::TemplateArray< osg::Vec3f, osg::Array::Vec3ArrayType, 3, GL_FLOAT > *>(pV);
    // --- END of debug section ---

#ifdef dSINGLE
    dGeomTriMeshDataBuildSingle1(
#else
    dGeomTriMeshDataBuildDouble1(
#endif
        meshDataId,
        //pV->getDataPointer(), pV->getDataSize(), pV->getNumElements(),        // [OSG 1.2] pV->getDataSize() returns 3 instead of 12 !
        pV->getDataPointer(), pV->getTotalDataSize() / pV->getNumElements(), pV->getNumElements(),
        pI->getDataPointer(), pI->getNumElements(), pI->getDataSize()*3,
        pN ? pN->getDataPointer() : NULL );

    // Build tri mesh
    id = dCreateTriMesh(spaceId, meshDataId, NULL, NULL, NULL);     // Create trimesh without callbacks
    return id;
}


MeshHandler::MeshHandler(osg::Node * pModel) : pGeo(NULL), pLocalIndexArray(NULL) {
    meshDataId = dGeomTriMeshDataCreate();

    FindGeometriesVisitor v;
    pModel->accept(v);
    if (v.vFoundGeometries.size() < 1) THROW_TRACED_EXCEPTION("Geometry not found in the given node file.");
    pOwnGeo = new osg::Geometry(*v.vFoundGeometries[0], osg::CopyOp::DEEP_COPY_ALL);
    pGeo = pOwnGeo.get();
}


MeshHandler::MeshHandler(const osg::HeightField * pGfxHeightField) : pGeo(NULL), pLocalIndexArray(NULL) {
    meshDataId = dGeomTriMeshDataCreate();

    pOwnGeo = new osg::Geometry;
    pGeo = pOwnGeo.get();

    // Create mesh data
    typedef osg::TemplateArray< osg::Vec3f, osg::Array::Vec3ArrayType, sizeof(osg::Vec3f), GL_FLOAT > TVertexArray;
    typedef osg::TemplateArray< osg::Vec3f, osg::Array::Vec3ArrayType, sizeof(osg::Vec3f), GL_FLOAT > TNormalArray;
    TVertexArray * pV = new TVertexArray;
    //TIndexArray * pI  = new TIndexArray;
    TNormalArray * pN = new TNormalArray;

    pOwnGeo->setVertexArray(pV);
    //pOwnGeo->setVertexIndices(pI);
    pOwnGeo->setNormalArray(pN);

    UINT c, r = 0;
    const UINT nbCols = pGfxHeightField->getNumColumns(), nbRows = pGfxHeightField->getNumRows();

    osg::DrawElementsUInt * pPS = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES);
    pOwnGeo->addPrimitiveSet(pPS);

    pV->reserve(nbCols * nbRows);
    //pI->reserve((nbCols-1) * (nbRows-1) * 6);
    pPS->reserve((nbCols-1) * (nbRows-1) * 6);
    pN->reserve((nbCols-1) * (nbRows-1));

// For debugging purposes : Flattens height field by setting all z-data to 0
//#define PHY_DEBUG_HEIGHT_FIELD

    // Add vertices
    for(r=0; r<nbRows; ++r) {
        for(c=0; c<nbCols; ++c) {
#ifdef PHY_DEBUG_HEIGHT_FIELD
            osg::Vec3 debugHFVec = pGfxHeightField->getVertex(c, r);
            debugHFVec._v[2] = 0;
            pV->push_back(debugHFVec);
#else
            pV->push_back(pGfxHeightField->getVertex(c, r));
#endif
        }
    }

    // Add indexes & normals
    pOwnGeo->setNormalBinding(osg::Geometry::BIND_PER_PRIMITIVE);
    // For each quad, where (c,r) is the lower-right corner
#define V(r,c) ((r)*nbCols + (c))   // Not optimized, but clear...

    osg::Vec3 v0, v1, v2, v3;
    for(r=1; r<nbRows; ++r) {
        for(c=1; c<nbCols; ++c) {
            // Add triangle (r-1, c-1), (r-1, c), (r, c-1)
            pPS->push_back(V(r-1, c-1));
            pPS->push_back(V(r-1, c));
            pPS->push_back(V(r, c-1));
            // Add triangle (r, c-1), (r-1, c), (r, c)
            pPS->push_back(V(r, c-1));
            pPS->push_back(V(r-1, c));
            pPS->push_back(V(r, c));

#ifdef PHY_DEBUG_HEIGHT_FIELD
            pN->push_back(osg::Vec3(0,0,1));
            pN->push_back(osg::Vec3(0,0,1));
#else
            // Generate normals from points
            v0 = pGfxHeightField->getVertex(c-1, r-1);
            v1 = pGfxHeightField->getVertex(c, r-1);
            v2 = pGfxHeightField->getVertex(c-1, r);
            v3 = pGfxHeightField->getVertex(c, r);
            pN->push_back((v1-v0) ^ (v2-v0));
            pN->push_back((v1-v3) ^ (v2-v3));

            // Other method : use rendering normals
            //norm1 = pGfxHeightField->getNormal(c-1, r-1);
            //norm2 = pGfxHeightField->getNormal(c, r-1);
            //norm3 = pGfxHeightField->getNormal(c-1, r);
            //norm4 = pGfxHeightField->getNormal(c, r);
            //pN->push_back((norm1 + norm2 + norm3)/3.f);
            //pN->push_back((norm3 + norm2 + norm4)/3.f);
#endif
        }
    }

}


UINT MeshHandler::getADOverrideFlag() { return AD_STEPS | AD_SAMPLES_COUNT | AD_LINEAR | AD_ANGULAR; }
//UINT MeshHandler::getADOverrideFlag() { return AD_STEPS | AD_LINEAR | AD_ANGULAR; }
UINT MeshHandler::getADSteps() { return 10; }
UINT MeshHandler::getADSamplesCount() { return 5; }
dReal MeshHandler::getADLinearThreshold() { return .1; }
dReal MeshHandler::getADAngularThreshold() { return .1; }



/*
void MeshHandler::meshStep() {
    ASSERT(!dGeomIsSpace(id));

    Geom * pGeom = static_cast<Geom *>(dGeomGetData(id));
    static graphVec3 lastPos = dGeomGetPositionV(*pGeom);
    static graphQuat lastQuat = pGeom->getQuaternion();

    graphVec3 pos = dGeomGetPositionV(*pGeom);
    graphQuat quat = pGeom->getQuaternion();

    // lastQuat * moveQuat = quat  <=>  moveQuat = lastQuat^(-1) * quat
    graphMat moveMat(lastQuat.inverse() * quat);
    moveMat.setTrans(pos - lastPos);

    dGeomTriMeshSetLastTransform(id, moveMat.ptr());
    //dGeomTriMeshDataSet(meshDataId, TRIMESH_LAST_TRANSFORMATION, moveMat.ptr());
}
*/


/*
int TerrainHandler::collideBox(dGeomID o1, dGeomID o2, int flags, dContactGeom * pContact, int skip) {
    //If a space is passed as o1 or o2 then this function will collide all objects contained in o1 with all objects contained in o2, and return the resulting contact points.

    // Convention for this method : o1 is of user class
    if (dGeomGetClass(o1) != TERRAIN) std::swap(o1, o2);
    Geom * pG1 = static_cast<Geom *>(dGeomGetData(o1));
    Geom * pG2 = static_cast<Geom *>(dGeomGetData(o2));
    TerrainHandler * pTH = boost::polymorphic_downcast<TerrainHandler *>(pG1->getHandler());

    //graphMat ltw = graphMat::inverse(WTLMatrix);

    USHORT nbMaxContacts = (UINT)flags & 0xFFFF;
    ASSERT(nbMaxContacts >0);
    // Computing half size of the box, and position, in terrain coordinates
    graphVec3 halfSize = static_cast<BoxHandler *>(pG2->getHandler())->getSize()/2 * graphMat(pG2->getQuaternion()) * pTH->WTLMatrix;
    graphVec3 pos = dGeomGetPositionV(*pG2) * pTH->WTLMatrix;

    // Find which tiles to intersect with
    //pTH->pHF->getXInterval() /2;
    //pTH->pHF->getYInterval() /2;

    //LOG_ALWAYS << pos.x() << ", " pos.y() << " => Tile " << <<std::endl;


    // -----------------------
    // Test collision
    if (pos.z() < 0) {
        pContact[0].depth = -pos.z();
        pContact[0].g1 = o1;
        pContact[0].g2 = o2;
        toPhyVec3(0,0,-1, pContact[0].normal);
        toPhyVec3(dGeomGetPositionV(*pG2).x(),dGeomGetPositionV(*pG2).y(),0, pContact[0].pos);
        return 1;
    }

    return 0;
}

int TerrainHandler::collideSphere(dGeomID o1, dGeomID o2, int flags, dContactGeom * pContact, int skip) {
    //If a space is passed as o1 or o2 then this function will collide all objects contained in o1 with all objects contained in o2, and return the resulting contact points.

    // Convention for this method : o1 is of user class
    if (dGeomGetClass(o1) != TERRAIN) std::swap(o1, o2);
    Geom * pG1 = static_cast<Geom *>(dGeomGetData(o1));
    Geom * pG2 = static_cast<Geom *>(dGeomGetData(o2));

    USHORT nbMaxContacts = (UINT)flags & 0xFFFF;

    return 0;
}
*/


HeightFieldHandler::HeightFieldHandler(const osg::HeightField * pGfxHeightField, float thickness, const graphVec3 & offset_from_graph, bool bWarp)
    : bPlaceable(true), scale(1), offset(0), thickness(thickness), bWarp(bWarp), idHF(0)
{
    widthSamples = pGfxHeightField->getNumColumns();
    depthSamples = pGfxHeightField->getNumRows();
    ASSERT(widthSamples>0);
    ASSERT(depthSamples>0);
    width = pGfxHeightField->getXInterval() * (widthSamples-1);
    depth = pGfxHeightField->getYInterval() * (depthSamples-1);

    pData = new float[widthSamples*depthSamples];
    idHFData = dGeomHeightfieldDataCreate();

    // Data copy
    UINT c,r;
    for(c=0; c<widthSamples; ++c) {
        for(r=0; r<depthSamples; ++r) {
            pData[c*widthSamples + r] = pGfxHeightField->getVertex(c, r).z();       // Result is "Height + origin.z()"
        }
    }

    graphVec3 origin = pGfxHeightField->getOrigin();
    origin.z() = 0;         // Origin's height is already included in getVertex()'s result.
    toPhyVec3(origin + offset_from_graph + graphVec3(width/2, depth/2, 0), pos);    // OSG origin is in the corner of the height field
    toPhyQuat(pGfxHeightField->getRotation(), rot);
}


HeightFieldHandler::HeightFieldHandler(const HeightFieldHandler & hf)
    : bPlaceable(hf.bPlaceable), scale(hf.scale), offset(hf.offset), thickness(hf.thickness), bWarp(hf.bWarp),
    widthSamples(hf.widthSamples), depthSamples(hf.depthSamples), width(hf.width), depth(hf.depth),
    idHF(0)
{
    ASSERT(widthSamples>0);
    ASSERT(depthSamples>0);

    pData = new float[widthSamples*depthSamples];
    idHFData = dGeomHeightfieldDataCreate();

    memcpy(pData, hf.pData, widthSamples*depthSamples*sizeof(float));       // Data copy

    pos[0] = hf.pos[0];
    pos[1] = hf.pos[1];
    pos[2] = hf.pos[2];
    pos[3] = hf.pos[3];

    rot[0] = hf.rot[0];
    rot[1] = hf.rot[1];
    rot[2] = hf.rot[2];
    rot[3] = hf.rot[3];
}


dGeomID HeightFieldHandler::create(dSpaceID spaceId) {
    // dGeomHeightfieldDataBuildCallback pour qu'ODE utilise un callback pour obtenir les données
    // dGeomHeightfieldDataBuildSingle pour qu'ODE ait (ou utilise) une copie des données

    dGeomHeightfieldDataBuildSingle(idHFData, pData, 0, width, depth, widthSamples, depthSamples, scale, offset, thickness, bWarp ? 1 : 0);

    // Give some very bounds which, while conservative, makes AABB computation more accurate than +/-INF.
    //if (!bWarp) dGeomHeightfieldDataSetBounds( idHFData, TODO, TODO);

    // Create height field, with no space (because of the transform)
    idHF = dCreateHeightfield(0, idHFData, bPlaceable ? 1 : 0);

    // Place it
    dMatrix3 R;
    graphQuat q(osg::PI_2, osg::X_AXIS);            // Rotate so Z is up, not Y (which is the default orientation)
    q *= graphQuat(osg::PI_2, osg::Z_AXIS);         // Rotate to match OSG's orientation
    graphMat r(q * toGraphQuat(rot));               // Rotate to match given orientation
    toPhyMatRotation(r, R);
    dGeomSetRotation(idHF, R);
    dGeomSetPosition(idHF, pos[0], pos[1], pos[2]); // Translate to match given position

    // Encapsulate it with a transform
    id = dCreateGeomTransform(spaceId);
    dGeomTransformSetCleanup(id, 0);
    dGeomTransformSetInfo(id, 1);
    dGeomTransformSetGeom(id, idHF);

    return id;
}


}   // namespace Physics

---