source: code/branches/physics/src/ogreode/OgreOdeGeometry.cpp @ 1943

#include "OgreOdePrecompiledHeaders.h"
#include "OgreOdeGeometry.h"

#include <OgreSceneNode.h>
#include <OgreSceneManager.h>
#include <OgreStringConverter.h>

#include "OgreOdeWorld.h"
#include "OgreOdeSpace.h"
#include "OgreOdeBody.h"
#include "OgreOdeCollision.h"
#include "OgreOdeDebugObject.h"
#include "OgreOdeDebugContact.h"

using namespace OgreOde;
using namespace Ogre;

//------------------------------------------------------------------------------------------------
int Geometry::_geometry_count = 0;
const Ogre::String Geometry::MovableType = "OgreOde_Geometry";

//------------------------------------------------------------------------------------------------
Geometry::Geometry(World *world, Space* space):
    UserDefinedObject(),
        _geom (0),
        _max_contacts (32),
        _contact_high_water_mark (0),
    _last_contact_num(0),
        _contacts (0),
        _debug_obj (0),
        _debug_node (0),
    _debug_contacts (0),
        _encapsulator (0),
        _user_data (0),
        _user_object (0),
    _world(world)
{
        mName = "";
}
//------------------------------------------------------------------------------------------------
Geometry::~Geometry()
{
    destroyDebugObject();

    delete[] _contacts;
    if (_debug_contacts)
    {         
        for (unsigned int i = 0; i < _max_contacts; i++)
            delete _debug_contacts[i];
        delete [] _debug_contacts;
    }
    if (_geom)
    {
        _world->getGeometryList().unregisterItem((unsigned long)_geom);
        dGeomDestroy(_geom); 
    }
}
//------------------------------------------------------------------------------------------------
dGeomID Geometry::getGeometryID() const
{
        return _geom;
}

//------------------------------------------------------------------------------------------------
void Geometry::setEncapsulator(Geometry* encapsulator)
{
        _encapsulator = encapsulator;

        if (_world->getShowDebugGeometries())
        {
                reparentDebugObject(encapsulator->_debug_node);
        }
}

//------------------------------------------------------------------------------------------------
void Geometry::reparentDebugObject(Ogre::Node* node)
{
        if ((node)&&(_debug_node)&&(_debug_node->getParent() != node))
        {
                Ogre::SceneNode* parent = static_cast<Ogre::SceneNode*>(_debug_node->getParent());
                parent->removeChild(_debug_node->getName());
                node->addChild(_debug_node);

                _debug_node->setPosition(getPosition());
                _debug_node->setOrientation(getOrientation());
        }
}

//------------------------------------------------------------------------------------------------
void Geometry::createDebugObject()
{
        if (!_debug_node)
        {
                Ogre::SceneNode* parent = _world->_manager->getRootSceneNode();
                _debug_node = static_cast<Ogre::Node*>(parent->createChildSceneNode(Ogre::String("OgreOde::Geometry_") + Ogre::StringConverter::toString(_geometry_count) + Ogre::String("_Debug"))); 

        if (_debug_obj)
        {
            static_cast<Ogre::SceneNode*>(_debug_node)->attachObject(_debug_obj);
        }

        if (getBody())
        {
            static_cast<Ogre::SceneNode*>(_debug_node->getParent())->removeChild(_debug_node->getName());
            getBody()->addDebugNode(_debug_node);
            if (_debug_obj) 
                                _debug_obj->setMode(DebugObject::Mode_Enabled);
                        
                        //reposition the debug node if the geometry has an offset
                        if(isOffset())
                                _debug_node->setPosition(getOffsetPosition()); 
        }
                else if (_encapsulator)
                {
                        reparentDebugObject(_encapsulator->_debug_node);
                }
                else
                {
                        if (_debug_obj) 
                _debug_obj->setMode(DebugObject::Mode_Static);
                        
                        _debug_node->setPosition(getPosition());
                        _debug_node->setOrientation(getOrientation());
                }
                _geometry_count++;
        }
}

//------------------------------------------------------------------------------------------------
void Geometry::destroyDebugObject()
{
        if (_debug_node)
    {

        Ogre::SceneNode* sn = static_cast<Ogre::SceneNode*>(_debug_node);
        sn->removeAndDestroyAllChildren ();
        sn = static_cast<Ogre::SceneNode*>(_debug_node->getParent());
        sn->removeAndDestroyChild(_debug_node->getName());
                _debug_node = 0;
        }

        if (_debug_obj)
        {
                delete _debug_obj;
                _debug_obj = 0;
        }
}

//------------------------------------------------------------------------------------------------
void Geometry::registerGeometry()
{
        dGeomSetData(_geom, (void*)this);
        assert(_world);
    _world->getGeometryList().registerItem(this);

    if (_world->getShowDebugGeometries()) 
        createDebugObject();

    setMaxContacts (_max_contacts);
}
//------------------------------------------------------------------------------------------------
dSpaceID Geometry::getSpaceID(Space* space) const
{
        if (!space) return 0;
        return space->getSpaceID();
}

//------------------------------------------------------------------------------------------------
void Geometry::setBody(Body* body)
{
        destroyDebugObject();
    if (body)
    {
        body->addGeometry (this);
        dGeomSetBody(_geom, body->getBodyID()); 
    }
    else
    {
        dGeomSetBody(_geom, 0); 
    }
        if (_world->getShowDebugGeometries()) 
        createDebugObject();
}

//------------------------------------------------------------------------------------------------
Body* Geometry::getBody()
{
        const dBodyID body = dGeomGetBody(_geom);
        if (!body) return 0;
        else return (Body*)dBodyGetData(body); 
}

//------------------------------------------------------------------------------------------------
void Geometry::notify(Body* body)
{
        if (getBody() == body)
        {
                destroyDebugObject();
        }
}

//------------------------------------------------------------------------------------------------
void Geometry::setDebug(const bool debug)
{
    destroyDebugObject();
    if (debug) 
        createDebugObject();    
}
//------------------------------------------------------------------------------------------------
void Geometry::setDebugContact(const bool debug)
{    
    if (_debug_contacts)
    {
        for (unsigned int i = 0; i < _max_contacts; i++)
            delete _debug_contacts[i];
        delete [] _debug_contacts;
        _debug_contacts = 0;
    }
    if (debug) 
    {
        _debug_contacts = new DebugContact*[_max_contacts];
        for (unsigned int i = 0; i < _max_contacts; i++)
            _debug_contacts[i] = new DebugContact(Ogre::StringConverter::toString((size_t)_geom) + 
                                                    "_Contact_" + 
                                                    Ogre::StringConverter::toString(i),
                                                    _world);
    }
}
//------------------------------------------------------------------------------------------------
void Geometry::setPosition(const Ogre::Vector3& position)
{
    dGeomSetPosition(_geom,(dReal)position.x,(dReal)position.y,(dReal)position.z);

    if ((_debug_node)&& ((!getBody()) || (_encapsulator)))
        _debug_node->setPosition(position);
}

//------------------------------------------------------------------------------------------------
void Geometry::setOrientation(const Ogre::Quaternion& orientation)
{
        dQuaternion q;
        q[0] = (dReal)orientation.w;
        q[1] = (dReal)orientation.x;
        q[2] = (dReal)orientation.y;
        q[3] = (dReal)orientation.z;
        dGeomSetQuaternion(_geom,q); 

        if ((_debug_node)&& ((!getBody()) || (_encapsulator))) _debug_node->setOrientation(orientation);
}

//------------------------------------------------------------------------------------------------
const Ogre::Vector3& Geometry::getPosition()
{
        const dReal* position = dGeomGetPosition(_geom);
        _position.x = (Ogre::Real)position[0];
        _position.y = (Ogre::Real)position[1];
        _position.z = (Ogre::Real)position[2];
        return _position;
}

//------------------------------------------------------------------------------------------------
const Ogre::Quaternion& Geometry::getOrientation()
{
        dQuaternion orientation;
        dGeomGetQuaternion(_geom,orientation); 
        _orientation.w = (Real)orientation[0];
        _orientation.x = (Real)orientation[1];
        _orientation.y = (Real)orientation[2];
        _orientation.z = (Real)orientation[3];
        return _orientation;
}

//------------------------------------------------------------------------------------------------
const AxisAlignedBox& Geometry::getAxisAlignedBox()
{
        dReal aabb[6];
        dGeomGetAABB(_geom,aabb);
        _bounding_box.setExtents((Real)aabb[0],(Real)aabb[2],(Real)aabb[4],(Real)aabb[1],(Real)aabb[3],(Real)aabb[5]);
        return _bounding_box;
}

//------------------------------------------------------------------------------------------------
Space* Geometry::getSpace()
{
        if (dGeomGetSpace(_geom) == 0) 
        {
                // not in any space
                return NULL;
        }

        return (Space*)_world->getSpaceList().findItem((size_t)dGeomGetSpace(_geom));
}


//------------------------------------------------------------------------------------------------
void Geometry::enable()
{
        dGeomEnable(_geom);
}

//------------------------------------------------------------------------------------------------
void Geometry::disable()
{
        dGeomDisable(_geom); 
}

//------------------------------------------------------------------------------------------------
bool Geometry::isEnabled()
{
        return dGeomIsEnabled(_geom) != 0; 
}

//------------------------------------------------------------------------------------------------
Geometry::Class Geometry::getClass() const
{
        return (Geometry::Class)dGeomGetClass(_geom);
}

//------------------------------------------------------------------------------------------------
void Geometry::setCategoryBitfield(unsigned long bits)
{
        dGeomSetCategoryBits(_geom,bits); 
}

//------------------------------------------------------------------------------------------------
void Geometry::setCollisionBitfield(unsigned long bits)
{
        dGeomSetCollideBits(_geom,bits); 
}

//------------------------------------------------------------------------------------------------
unsigned long Geometry::getCategoryBitfield()
{
        return dGeomGetCategoryBits(_geom); 
}

//------------------------------------------------------------------------------------------------
unsigned long Geometry::getCollisionBitfield()
{
        return dGeomGetCollideBits(_geom); 
}

//------------------------------------------------------------------------------------------------
int Geometry::collide(Geometry* geometry,CollisionListener* listener)
{
        const unsigned int num_contacts = (unsigned int) dCollide(_geom,
                                                                                geometry->getGeometryID(),
                                                                                _max_contacts,
                                                                                &(_contacts[0].geom),
                                                                                sizeof(dContact));
        if (num_contacts)
        {
                _contact_high_water_mark = std::max(_contact_high_water_mark,num_contacts);

                Contact contact;
                
                const dWorldID wid = _world->getWorldID();
                const dJointGroupID cid = _world->getContactGroupID();
                const dBodyID b1 = dGeomGetBody(_geom);
                const dBodyID b2 = dGeomGetBody(geometry->getGeometryID());

                if (listener)
                {
                        for(unsigned int i = 0;i < num_contacts;i++)
                        {
                                contact.setContact (&_contacts[i]);

                                if (listener->collision(&contact))
                                {
                                        dJointAttach(dJointCreateContact(wid,cid,&_contacts[i]),b1,b2);
                } 
                        }
                }
                else
                {
                        for(unsigned int i = 0;i < num_contacts;i++)
                        {
                                contact.setContact (&_contacts[i]);
   
                                dJointAttach(dJointCreateContact(wid,cid,&_contacts[i]), b1, b2);
                        }
                } 
    }
    _last_contact_num = num_contacts;
        return num_contacts;
}
//------------------------------------------------------------------------------------------------
void Geometry::updateDebugContact()
{
    assert (_world->getShowDebugContact ());
    assert (_debug_contacts);
    {
        unsigned int k = 0;
        while (k < _max_contacts)
        {
            _debug_contacts[k++]->setEnabled(false);
        }

        if (_last_contact_num)
        {
            Contact contact;
            for(unsigned int i = 0;i < _last_contact_num;i++)
            {
                k = 0;
                while (k < _max_contacts)
                {
                    if (_debug_contacts[k]->isEnabled() == false)
                        break;
                    k++;
                }
                assert (k < _max_contacts);

                _debug_contacts[k]->setEnabled(true);
                contact.setContact (&_contacts[i]);
                _debug_contacts[k]->update (&contact);
            }
        }
    }
    _last_contact_num = 0;
 }
//------------------------------------------------------------------------------------------------
unsigned int Geometry::getMaxContacts() const
{
        return _max_contacts;
}

//------------------------------------------------------------------------------------------------
void Geometry::setMaxContacts(unsigned int max_contacts)
{
        delete[] _contacts;
        _contacts = new dContact[max_contacts];

    if (_world->getShowDebugContact ())
    {
        if (_debug_contacts)
        {         
            for (unsigned int i = 0; i < _max_contacts; i++)
                delete _debug_contacts[i];
            delete [] _debug_contacts;
        }
        _debug_contacts = new DebugContact*[max_contacts];
        for (unsigned int i = 0; i < max_contacts; i++)
            _debug_contacts[i] = new DebugContact(Ogre::StringConverter::toString((size_t)_geom)  
                        + "_Contact_" 
                        + Ogre::StringConverter::toString(i),
            _world);
    }
    _max_contacts = max_contacts;
}

//------------------------------------------------------------------------------------------------
unsigned int Geometry::getContactHighWaterMark() const
{
        return _contact_high_water_mark;
}

//------------------------------------------------------------------------------------------------
size_t Geometry::getID() const
{
        return (size_t)_geom;
}
//------------------------------------------------------------------------------------------------
void Geometry::clearOffset()
{
        dGeomClearOffset(_geom);
}
//------------------------------------------------------------------------------------------------
int Geometry::isOffset()
{
        return dGeomIsOffset(_geom);
}
//------------------------------------------------------------------------------------------------
void Geometry::setOffsetPosition (const Ogre::Vector3 &pos)
{
        dGeomSetOffsetPosition (_geom, pos.x, pos.y, pos.z);

        //move the debug object when setting offset if we have a body only
        if ((_debug_node)&& ((getBody())/* || (_encapsulator)*/))
                _debug_node->setPosition(getOffsetPosition()); 
}
//------------------------------------------------------------------------------------------------
void Geometry::setOffsetQuaternion(const Ogre::Quaternion &quat) 
{
        dQuaternion q;
        q[0] = quat.w; 
        q[1] = quat.x; 
        q[2] = quat.y; 
        q[3] = quat.z;
        dGeomSetOffsetQuaternion (_geom, q);

        if ((_debug_node)&& ((getBody())/* || (_encapsulator)*/))
                _debug_node->setOrientation(getOffsetQuaternion()); 
}
//------------------------------------------------------------------------------------------------
void Geometry::setOffsetWorldPosition(const Ogre::Vector3 &pos) 
{
        dGeomSetOffsetWorldPosition(_geom, pos.x, pos.y, pos.z);

        //move the debug object when setting offset if we have a body only
        if ((_debug_node)&& ((getBody())/* || (_encapsulator)*/))
                _debug_node->setPosition(getOffsetPosition()); 
}
//------------------------------------------------------------------------------------------------
void Geometry::setOffsetWorldQuaternion(const Ogre::Quaternion &quat) 
{
        dQuaternion q;
        q[0] = quat.w; 
        q[1] = quat.x; 
        q[2] = quat.y; 
        q[3] = quat.z;
        dGeomSetOffsetWorldQuaternion (_geom, q);

        if ((_debug_node)&& ((getBody())/* || (_encapsulator)*/))
                _debug_node->setOrientation(getOffsetQuaternion()); 
}
//------------------------------------------------------------------------------------------------
Vector3 Geometry::getOffsetPosition() 
{
        const dReal *p =  dGeomGetOffsetPosition (_geom);
        return Ogre::Vector3(p[0], p[1], p[2]);
}
//------------------------------------------------------------------------------------------------
Quaternion Geometry::getOffsetQuaternion() 
{
        dQuaternion q;
        dGeomGetOffsetQuaternion (_geom, q);
        return Ogre::Quaternion(q[0], q[1], q[2], q[3]);
}
//-----------------------------------------------------------------------
void Geometry::_notifyAttached(Node* parent, bool isTagPoint)
{
        MovableObject::_notifyAttached(parent, isTagPoint);
        reparentDebugObject(parent);
}
//-----------------------------------------------------------------------
void Geometry::_notifyMoved()
{
        MovableObject::_notifyMoved();
                
        if (!getBody())
        {
                setPosition(mParentNode->_getDerivedPosition());
                setOrientation(mParentNode->_getDerivedOrientation());
        }
}
//------------------------------------------------------------------------------------------------
SphereGeometry::SphereGeometry(Real radius, World *world, Space* space):Geometry(world, space)
{
        _geom = dCreateSphere(getSpaceID(space),(dReal)radius); 
        registerGeometry();
}

//------------------------------------------------------------------------------------------------
void SphereGeometry::setRadius(Real radius)
{
        dGeomSphereSetRadius(_geom,(dReal)radius);
}

//------------------------------------------------------------------------------------------------
Real SphereGeometry::getRadius()
{
        return (Real)dGeomSphereGetRadius(_geom); 
}

//------------------------------------------------------------------------------------------------
Real SphereGeometry::getPointDepth(const Ogre::Vector3& point)
{
        return (Real)dGeomSpherePointDepth(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z); 
}

//------------------------------------------------------------------------------------------------
void SphereGeometry::createDebugObject()
{
        _debug_obj = new SphereDebugObject(getRadius());
        Geometry::createDebugObject();
}

//------------------------------------------------------------------------------------------------
SphereGeometry::~SphereGeometry()
{
}

//------------------------------------------------------------------------------------------------
BoxGeometry::BoxGeometry(const Ogre::Vector3& size, World *world, Space* space):Geometry(world, space)
{
        _geom = dCreateBox(getSpaceID(space),(dReal)size.x,(dReal)size.y,(dReal)size.z); 
        registerGeometry();
}

//------------------------------------------------------------------------------------------------
void BoxGeometry::setSize(const Ogre::Vector3& size)
{
        dGeomBoxSetLengths(_geom,(dReal)size.x,(dReal)size.y,(dReal)size.z); 
}

//------------------------------------------------------------------------------------------------
const Ogre::Vector3& BoxGeometry::getSize()
{
        dVector3 result;
        dGeomBoxGetLengths(_geom,result); 
        _size.x = (Real)result[0];
        _size.y = (Real)result[1];
        _size.z = (Real)result[2];
        return _size;
}

//------------------------------------------------------------------------------------------------
Real BoxGeometry::getPointDepth(const Ogre::Vector3& point)
{
        return (Real)dGeomBoxPointDepth(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z); 
}

//------------------------------------------------------------------------------------------------
void BoxGeometry::createDebugObject()
{
        _debug_obj = new BoxDebugObject(getSize());
        Geometry::createDebugObject();  
}

//------------------------------------------------------------------------------------------------
BoxGeometry::~BoxGeometry()
{
}

//------------------------------------------------------------------------------------------------
InfinitePlaneGeometry::InfinitePlaneGeometry(const Plane& plane,World *world, Space* space):Geometry(world, space)
{
        _geom = dCreatePlane(getSpaceID(space),(dReal)plane.normal.x,(dReal)plane.normal.y,(dReal)plane.normal.z,(dReal)-plane.d); 
        registerGeometry();
}

//------------------------------------------------------------------------------------------------
void InfinitePlaneGeometry::setDefinition(const Plane& plane)
{
        dGeomPlaneSetParams(_geom,(dReal)plane.normal.x,(dReal)plane.normal.y,(dReal)plane.normal.z,(dReal)-plane.d); 
}

//------------------------------------------------------------------------------------------------
const Plane& InfinitePlaneGeometry::getDefinition()
{
        dVector4 result;
        dGeomPlaneGetParams(_geom,result); 
        _plane.normal.x = result[0];
        _plane.normal.y = result[1];
        _plane.normal.z = result[2];
        _plane.d = -result[3];
        return _plane;
}

//------------------------------------------------------------------------------------------------
Real InfinitePlaneGeometry::getPointDepth(const Ogre::Vector3& point)
{
        return (Real)dGeomPlanePointDepth(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z); 
}

//------------------------------------------------------------------------------------------------
void InfinitePlaneGeometry::setPosition(const Ogre::Vector3& position)
{
}

//------------------------------------------------------------------------------------------------
void InfinitePlaneGeometry::setOrientation(const Ogre::Quaternion& orientation)
{
}

//------------------------------------------------------------------------------------------------
const Ogre::Vector3& InfinitePlaneGeometry::getPosition()
{
        return Ogre::Vector3::ZERO;
}

//------------------------------------------------------------------------------------------------
const Ogre::Quaternion& InfinitePlaneGeometry::getOrientation()
{
        return Ogre::Quaternion::ZERO;
}

//------------------------------------------------------------------------------------------------
const AxisAlignedBox& InfinitePlaneGeometry::getAxisAlignedBox()
{
        return _bounding_box;
}

//------------------------------------------------------------------------------------------------
InfinitePlaneGeometry::~InfinitePlaneGeometry()
{
}

//------------------------------------------------------------------------------------------------
CapsuleGeometry::CapsuleGeometry(Real radius,Real length,World *world, Space* space):Geometry(world, space)
{
        _geom = dCreateCapsule(getSpaceID(space),(dReal)radius,(dReal)length);
        registerGeometry();
}

//------------------------------------------------------------------------------------------------
void CapsuleGeometry::setDefinition(Real radius,Real length)
{
        dGeomCapsuleSetParams(_geom,(dReal)radius,(dReal)length);
}

//------------------------------------------------------------------------------------------------
Real CapsuleGeometry::getRadius()
{
        dReal radius,length;
        dGeomCapsuleGetParams(_geom,&radius,&length); 
        return radius;
}

//------------------------------------------------------------------------------------------------
Real CapsuleGeometry::getLength()
{
        dReal radius,length;
        dGeomCapsuleGetParams(_geom,&radius,&length); 
        return length;
}

//------------------------------------------------------------------------------------------------
Real CapsuleGeometry::getPointDepth(const Ogre::Vector3& point)
{
        return (Real)dGeomCapsulePointDepth(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z); 
}

//------------------------------------------------------------------------------------------------
void CapsuleGeometry::createDebugObject()
{
        _debug_obj = new CapsuleDebugObject(getRadius(),getLength());
        Geometry::createDebugObject();
}

//------------------------------------------------------------------------------------------------
CapsuleGeometry::~CapsuleGeometry()
{
}

//------------------------------------------------------------------------------------------------
CylinderGeometry::CylinderGeometry(Real radius,Real length,World *world, Space* space):Geometry(world, space)
{
        _geom = dCreateCCylinder(getSpaceID(space),(dReal)radius,(dReal)length);
        registerGeometry();
}

//------------------------------------------------------------------------------------------------
void CylinderGeometry::setDefinition(Real radius,Real length)
{
        dGeomCCylinderSetParams(_geom,(dReal)radius,(dReal)length);
}

//------------------------------------------------------------------------------------------------
Real CylinderGeometry::getRadius()
{
        dReal radius,length;
        dGeomCCylinderGetParams(_geom,&radius,&length); 
        return radius;
}

//------------------------------------------------------------------------------------------------
Real CylinderGeometry::getLength()
{
        dReal radius,length;
        dGeomCCylinderGetParams(_geom,&radius,&length); 
        return length;
}

//------------------------------------------------------------------------------------------------
Real CylinderGeometry::getPointDepth(const Ogre::Vector3& point)
{
        return (Real)dGeomCCylinderPointDepth(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z); 
}

//------------------------------------------------------------------------------------------------
void CylinderGeometry::createDebugObject()
{
        _debug_obj = new CylinderDebugObject(getRadius(),getLength());
        Geometry::createDebugObject();
}

//------------------------------------------------------------------------------------------------
CylinderGeometry::~CylinderGeometry()
{
}
//------------------------------------------------------------------------------------------------
RayGeometry::RayGeometry(Real length,World *world, Space* space) : Geometry(world, space)
{
        _geom = dCreateRay(getSpaceID(space),(dReal)length);
        registerGeometry();
}

//------------------------------------------------------------------------------------------------
void RayGeometry::createDebugObject()
{
        _debug_obj = new RayDebugObject(getStart(),getDirection(),getLength());
        Geometry::createDebugObject();
        if (_debug_node)
        {
                _debug_node->setPosition(Vector3::ZERO);
                _debug_node->setOrientation(Quaternion::IDENTITY);
        }
}

//------------------------------------------------------------------------------------------------
void RayGeometry::setLength(Real length)
{
        dGeomRaySetLength(_geom,(dReal)length); 
}

//------------------------------------------------------------------------------------------------
Real RayGeometry::getLength()
{
        return (Real)dGeomRayGetLength(_geom); 
}

//------------------------------------------------------------------------------------------------
void RayGeometry::setDefinition(const Ogre::Vector3& start,const Ogre::Vector3& direction)
{
        dGeomRaySet(_geom,
                                        (dReal)start.x,(dReal)start.y,(dReal)start.z,
                                        (dReal)direction.x,(dReal)direction.y,(dReal)direction.z);

        if ((_debug_node)&& ((!getBody()) || (_encapsulator))) 
                (static_cast<RayDebugObject *>(_debug_obj))->setDefinition(start, direction, (Real)dGeomRayGetLength(_geom));

}

//------------------------------------------------------------------------------------------------
const Ogre::Vector3& RayGeometry::getStart()
{
        dVector3 start,direction;
        dGeomRayGet(_geom,start,direction); 
        _start.x = (Real)start[0];
        _start.y = (Real)start[1];
        _start.z = (Real)start[2];
        return _start;
}

//------------------------------------------------------------------------------------------------
const Ogre::Vector3& RayGeometry::getDirection()
{
        dVector3 start,direction;
        dGeomRayGet(_geom,start,direction); 
        _direction.x = (Real)direction[0];
        _direction.y = (Real)direction[1];
        _direction.z = (Real)direction[2];
        return _direction;
}

//------------------------------------------------------------------------------------------------
RayGeometry::~RayGeometry()
{
}

//------------------------------------------------------------------------------------------------
TransformGeometry::TransformGeometry(World *world, Space* space) : Geometry(world, space)
{
        _geom = dCreateGeomTransform(getSpaceID(space)); 
        dGeomTransformSetCleanup(_geom,0);
        dGeomTransformSetInfo(_geom,1);
        registerGeometry();
}

//------------------------------------------------------------------------------------------------
void TransformGeometry::setEncapsulatedGeometry(Geometry* geometry)
{
        dGeomTransformSetGeom(_geom,geometry->getGeometryID()); 
        destroyDebugObject();
        if (_world->getShowDebugGeometries()) 
                createDebugObject();    

        geometry->setEncapsulator(this);
}

//------------------------------------------------------------------------------------------------
void TransformGeometry::createDebugObject()
{
        Geometry::createDebugObject();
        if (getEncapsulatedGeometry())
        {
                getEncapsulatedGeometry()->destroyDebugObject();
                getEncapsulatedGeometry()->createDebugObject();
        }
}

//------------------------------------------------------------------------------------------------
void TransformGeometry::destroyDebugObject()
{
        if (getEncapsulatedGeometry()) getEncapsulatedGeometry()->destroyDebugObject();
        Geometry::destroyDebugObject();
}

//------------------------------------------------------------------------------------------------
Geometry* TransformGeometry::getEncapsulatedGeometry() const
{
        dGeomID id = dGeomTransformGetGeom(_geom);
        if (id == 0) return 0;
        else return (Geometry*)dGeomGetData(id); 
}

//------------------------------------------------------------------------------------------------
TransformGeometry::~TransformGeometry()
{
}


//------------------------------------------------------------------------------------------------
TriangleMeshGeometry::TriangleMeshGeometry(const Ogre::Vector3* vertices,
                                           unsigned int vertex_count,
                                           const TriangleIndex* indices,
                                           unsigned int index_count,
                                           World *world, Space* space) : 
    Geometry(world, space),
    _vertex_count (vertex_count),
    _index_count (index_count)
{
        _vertex_count = vertex_count;
        _index_count = index_count;
        _vertices = new dVector3[vertex_count];
        _indices = new unsigned int[index_count];

        for(unsigned int i = 0;i < vertex_count;i++)
        {
                _vertices[i][0] = (dReal)vertices[i].x;
                _vertices[i][1] = (dReal)vertices[i].y;
                _vertices[i][2] = (dReal)vertices[i].z;
        }

        memcpy(_indices,indices,sizeof(unsigned int) * index_count);

        _data = dGeomTriMeshDataCreate(); 

        //dGeomTriMeshDataBuildSimple(_data,(const dReal*)_vertices, (int)vertex_count, _indices, (int) index_count); 
    // modified version
        dGeomTriMeshDataBuildSimple(_data,(const dReal*)_vertices, (int)vertex_count, (dTriIndex*)_indices, (int) index_count); 

        _geom = dCreateTriMesh(getSpaceID(space),_data,0,0,0);
        registerGeometry();

        _collision_listener = 0;
}
//------------------------------------------------------------------------------------------------
TriangleMeshGeometry::TriangleMeshGeometry(TriangleMeshDataPtr dataPtr, World *world, Space* space) :
        Geometry(world, space)
{
        // increases the referance count
        _dataPtr = dataPtr;     
        
        //printf ("dataPtr counter %d\n", dataPtr.useCount() );
        //printf ("_dataPtr counter %d\n", _dataPtr.useCount() );
        
        _data                   = dataPtr.getPointer()->getData();
        _vertex_count   = dataPtr.getPointer()->getVertexCount();
        _index_count    = dataPtr.getPointer()->getIndexCount();
        _vertices               = dataPtr.getPointer()->getVertices();
        _indices                = (unsigned int*) dataPtr.getPointer()->getIndices();
        
        _geom = dCreateTriMesh(getSpaceID(space), _data,0,0,0);
        registerGeometry();
        
        _collision_listener = 0;
}
//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::changeTriangleMeshData(TriangleMeshDataPtr dataPtr)
{
        if(dataPtr.isNull() == false)
        {
                _dataPtr = dataPtr;
                
                _data                   = dataPtr.getPointer()->getData();
                _vertex_count   = dataPtr.getPointer()->getVertexCount();
                _index_count    = dataPtr.getPointer()->getIndexCount();
                _vertices               = dataPtr.getPointer()->getVertices();
                _indices                = (unsigned int*) dataPtr.getPointer()->getIndices();
        
                dGeomTriMeshSetData(_geom, _data);
        
                if ( _world->getShowDebugGeometries() )
                {
                        destroyDebugObject();
                        createDebugObject();
                }
        }
        else // setting it to a NULL
        {
        
                _dataPtr = dataPtr;
                
                _data                   = NULL;
                _vertex_count   = 0;
                _index_count    = 0;
                _vertices               = NULL;
                _indices                = NULL;
                
                dGeomTriMeshSetData(_geom, _data);
                
                if ( _world->getShowDebugGeometries() )
                {
                        destroyDebugObject();
                        createDebugObject();
                }
                //printf ("TriangleMeshGeometry::changeTriangleMeshData error : The TriangleMeshDataPtr passed in isNull\n");
        }
}

//------------------------------------------------------------------------------------------------
Vector3 TriangleMeshGeometry::getPoint(unsigned int index,const Ogre::Vector3& uv)
{
        dVector3 out;
        dGeomTriMeshGetPoint(_geom, (int) index, (dReal)uv.x, (dReal)uv.y, out); 
        return Ogre::Vector3((Real)out[0],(Real)out[1],(Real)out[2]);
}

//------------------------------------------------------------------------------------------------
TriangleMeshTriangle TriangleMeshGeometry::getTriangle(int index)
{
        dVector3 v0,v1,v2;
        dGeomTriMeshGetTriangle(_geom,(int)index, &v0, &v1, &v2); 
        
        TriangleMeshTriangle tri;

        tri.v0.x = v0[0]; tri.v0.y = v0[1]; tri.v0.z = v0[2];
        tri.v1.x = v1[0]; tri.v1.y = v1[1]; tri.v1.z = v1[2];
        tri.v2.x = v2[0]; tri.v2.y = v2[1]; tri.v2.z = v2[2];
        
        return tri;
}

//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::clearTemporalCoherenceCache()
{
        dGeomTriMeshClearTCCache(_geom); 
}

//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::enableTemporalCoherence(Geometry::Class geometry_class, bool enable)
{
        assert((geometry_class == Geometry::Class_Sphere)||(geometry_class == Geometry::Class_Box));
        dGeomTriMeshEnableTC(_geom,(int)geometry_class,(enable)?1:0); 
}

//------------------------------------------------------------------------------------------------
bool TriangleMeshGeometry::isTemporalCoherenceEnabled(Geometry::Class geometry_class)
{
        return ((dGeomTriMeshIsTCEnabled(_geom,(int)geometry_class))?true:false);
}

//------------------------------------------------------------------------------------------------
int TriangleMeshGeometry::_collisionCallback(dGeomID mesh,dGeomID object,int triangle)
{
        TriangleMeshGeometry* trimesh = (TriangleMeshGeometry*)dGeomGetData(mesh);
        if (trimesh->_collision_listener)
        {
                Geometry* geometry = (object)?((Geometry*)dGeomGetData(object)):0;
                return ((trimesh->_collision_listener->collide(trimesh,geometry,triangle))?1:0);
        }
        return 1;
}

//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::setCollisionListener(TriangleMeshCollisionListener* collision_listener)
{
        _collision_listener = collision_listener;
        dGeomTriMeshSetCallback(_geom,(_collision_listener)?TriangleMeshGeometry::_collisionCallback:0); 
}

//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::_intersectionCallback(dGeomID mesh,dGeomID object,const int* triangles,int triangle_count)
{
        TriangleMeshGeometry* trimesh = (TriangleMeshGeometry*)dGeomGetData(mesh);
        if (trimesh->_intersection_listener)
        {
                Geometry* geometry = (object)?((Geometry*)dGeomGetData(object)):0;
                trimesh->_intersection_listener->intersect(trimesh,geometry,triangles,triangle_count);
        }
}

//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::setIntersectionListener(TriangleMeshIntersectionListener* intersection_listener)
{
        _intersection_listener = intersection_listener;
        dGeomTriMeshSetArrayCallback(_geom,(_intersection_listener)?TriangleMeshGeometry::_intersectionCallback:0); 
}

//------------------------------------------------------------------------------------------------
int TriangleMeshGeometry::_rayCallback(dGeomID mesh,dGeomID ray,int triangle,dReal u,dReal v)
{
        TriangleMeshGeometry* trimesh = (TriangleMeshGeometry*)dGeomGetData(mesh);
        if (trimesh->_ray_listener)
        {
                RayGeometry* ray_geometry = (ray)?((RayGeometry*)dGeomGetData(ray)):0;
                return ((trimesh->_ray_listener->collide(trimesh,ray_geometry,triangle,Vector3((Real)u,(Real)v,0.0)))?1:0);
        }
        return 1;
}

//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::setRayListener(TriangleMeshRayListener* ray_listener)
{
        _ray_listener = ray_listener;
        dGeomTriMeshSetRayCallback(_geom,(_ray_listener)?TriangleMeshGeometry::_rayCallback:0); 
}

//------------------------------------------------------------------------------------------------
void TriangleMeshGeometry::createDebugObject()
{
        if (_index_count != 0)
        {
                _debug_obj = new TriangleMeshDebugObject((_index_count / 3) * 6);
                TriangleMeshDebugObject* obj = static_cast<TriangleMeshDebugObject*>(_debug_obj);

                obj->beginDefinition();
                for(unsigned int i = 0,j = 0;i < _index_count;i+=3,j+=6)
                {
                        obj->setVertex(j,Vector3((Real)_vertices[_indices[i]][0],(Real)_vertices[_indices[i]][1],(Real)_vertices[_indices[i]][2]));
                        obj->setVertex(j+1,Vector3((Real)_vertices[_indices[i+1]][0],(Real)_vertices[_indices[i+1]][1],(Real)_vertices[_indices[i+1]][2]));

                        obj->setVertex(j+2,Vector3((Real)_vertices[_indices[i+1]][0],(Real)_vertices[_indices[i+1]][1],(Real)_vertices[_indices[i+1]][2]));
                        obj->setVertex(j+3,Vector3((Real)_vertices[_indices[i+2]][0],(Real)_vertices[_indices[i+2]][1],(Real)_vertices[_indices[i+2]][2]));

                        obj->setVertex(j+4,Vector3((Real)_vertices[_indices[i+2]][0],(Real)_vertices[_indices[i+2]][1],(Real)_vertices[_indices[i+2]][2]));
                        obj->setVertex(j+5,Vector3((Real)_vertices[_indices[i]][0],(Real)_vertices[_indices[i]][1],(Real)_vertices[_indices[i]][2]));
                }
                obj->endDefinition();

                Geometry::createDebugObject();
        }
}

//------------------------------------------------------------------------------------------------
TriangleMeshGeometry::~TriangleMeshGeometry()
{
        if (_dataPtr.isNull())
        {
                dGeomTriMeshDataDestroy(_data); 
                delete[] _vertices;
                delete[] _indices;
        }
}


//------------------------------------------------------------------------------------------------
ConvexGeometry::ConvexGeometry(const Ogre::Vector3* vertices,
                                                           unsigned int vertex_count,
                                                           const unsigned int* indices,
                                                           unsigned int index_count,
                               World *world, Space* space) : 
    Geometry(world, space),
    _vertex_count (vertex_count),
    _index_count (index_count)
{
        _vertices = new dReal[vertex_count*3];
        _indices = new unsigned int[index_count];

        for(unsigned int i = 0;i < vertex_count;i++)
        {
                _vertices[i*3 + 0] = (dReal)vertices[i].x;
                _vertices[i*3 + 1] = (dReal)vertices[i].y;
                _vertices[i*3 + 2] = (dReal)vertices[i].z;
        }

        memcpy(_indices,indices,sizeof(unsigned int) * index_count);


        _geom = dCreateConvex (getSpaceID(space),
                0,//dReal *_planes,
                0,//unsigned int _planecount,
                _vertices,
                vertex_count,
                0);//unsigned int *_polygons)

        //      dGeomSetConvex (dGeomID g,
        //              dReal *_planes,
        //              unsigned int _count,
        //              dReal *_points,
        //              unsigned int _pointcount,unsigned int *_polygons);

        registerGeometry();
}
//------------------------------------------------------------------------------------------------
void ConvexGeometry::createDebugObject()
{
        _debug_obj = new TriangleMeshDebugObject((_index_count / 3) * 6);
        TriangleMeshDebugObject* obj = static_cast<TriangleMeshDebugObject*>(_debug_obj);

        obj->beginDefinition();
        for(unsigned int i = 0,j = 0;i < _index_count;i+=3,j+=6)
        {
                obj->setVertex(j,Vector3((Real)_vertices[_indices[i]*3 + 0],(Real)_vertices[_indices[i]*3 + 1],(Real)_vertices[_indices[i]*3 + 2]));
                obj->setVertex(j+1,Vector3((Real)_vertices[_indices[i+1]*3 + 0],(Real)_vertices[_indices[i+1]*3 + 1],(Real)_vertices[_indices[i+1]*3 + 2]));

                obj->setVertex(j+2,Vector3((Real)_vertices[_indices[i+1]*3 + 0],(Real)_vertices[_indices[i+1]*3 + 1],(Real)_vertices[_indices[i+1]*3 + 2]));
                obj->setVertex(j+3,Vector3((Real)_vertices[_indices[i+2]*3 + 0],(Real)_vertices[_indices[i+2]*3 + 1],(Real)_vertices[_indices[i+2]*3 + 2]));

                obj->setVertex(j+4,Vector3((Real)_vertices[_indices[i+2]*3 + 0],(Real)_vertices[_indices[i+2]*3 + 1],(Real)_vertices[_indices[i+2]*3 + 2]));
                obj->setVertex(j+5,Vector3((Real)_vertices[_indices[i]*3 + 0],(Real)_vertices[_indices[i]*3 + 1],(Real)_vertices[_indices[i]*3 + 2]));
        }
        obj->endDefinition();

        Geometry::createDebugObject();
}
//------------------------------------------------------------------------------------------------
ConvexGeometry::~ConvexGeometry()
{
        delete[] _vertices;
        delete[] _indices;
}

//------------------------------------------------------------------------------------------------
TerrainGeometry::TerrainGeometry(World *world, Space* space,
                                                                const Ogre::Vector3 &scale,
                                int nodes_per_sideX,
                                int nodes_per_sideY,
                                Ogre::Real worldSizeX,
                                Ogre::Real worldSizeZ,
                                                                bool centered,
                                Ogre::Real thickness) :
                        Geometry(world, space),
                        _max_height (scale.y),
                        _sample_width(scale.x),
                        _sample_height(scale.z),
                        _centered(centered),
                        _halfWorldSizeX(worldSizeX * 0.5),
                        _halfWorldSizeZ(worldSizeZ * 0.5)
{
        dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();
        dGeomHeightfieldDataBuildCallback(      heightid, //getSpaceID(space), 
                                                                                this, // pUserData ?
                                                                                TerrainGeometry::_heightCallback, 
                                                                                (dReal) (worldSizeX), //X
                                                                                (dReal) (worldSizeZ), //Z
                                                                                nodes_per_sideX, // w // Vertex count along edge >= 2
                                                                                nodes_per_sideY, // h // Vertex count along edge >= 2
                                                                                REAL( 1.0 ),     //scale
                                                                                REAL( 0.0 ),    // vOffset
                                                                                thickness,      // vThickness
                                                                                 0); // nWrapMode


        // Give some very bounds which, while conservative,
        // makes AABB computation more accurate than +/-INF.
        dGeomHeightfieldDataSetBounds( heightid, REAL( 0.0 ),  _max_height );
        _geom = dCreateHeightfield( getSpaceID(space), heightid, 1 );

        _listener = 0;
        _ray = Ogre::Ray (Ogre::Vector3::ZERO, Ogre::Vector3::NEGATIVE_UNIT_Y);
        _ray_query = _world->getSceneManager()->createRayQuery(_ray);

        _ray_query->setQueryTypeMask(Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK );
        _ray_query->setWorldFragmentType(Ogre::SceneQuery::WFT_SINGLE_INTERSECTION); 

        registerGeometry();

        if (_centered)
        {
                // PLSM2 is centered by default.        
        //setPosition (Ogre::Vector3::ZERO);
        ;
        }
        else
        {
        // TSM is not centered by default.      
                setPosition (Ogre::Vector3(_halfWorldSizeX, 
                                    0, 
                                    _halfWorldSizeZ));
        }
        setOrientation(Ogre::Quaternion::ZERO);
}

//------------------------------------------------------------------------------------------------
dReal TerrainGeometry::_heightCallback(void* data,int x,int z)
{
        TerrainGeometry * const terrain = (TerrainGeometry*)data;    
        if (terrain->_listener)
        {
                return static_cast <dReal> (terrain->_listener->heightAt(Vector3((Ogre::Real)x,
                                                                terrain->_max_height,
                                                                (Ogre::Real)z)));
        }
    else
    {
        return static_cast <dReal> (terrain->getHeightAt(Vector3((Ogre::Real)x, 
                                                        terrain->_max_height, 
                                                        (Ogre::Real)z)));
    }
}

//------------------------------------------------------------------------------------------------
Ogre::Real TerrainGeometry::getPointDepth(const Ogre::Vector3& point)
{
        //return (Real)dGeomTerrainCallbackPointDepth(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z); 
        //return (Real) dGeomHeightfieldPointDepth (_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z);
        //dGetDepthFn(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z);
        return (Real) 0.0;
}

//------------------------------------------------------------------------------------------------
void TerrainGeometry::setHeightListener(TerrainGeometryHeightListener* listener)
{
        _listener = listener;
}
//------------------------------------------------------------------------------------------------
bool TerrainGeometry::queryResult(Ogre::MovableObject *obj, Ogre::Real distance)
{
        return false;
}

//------------------------------------------------------------------------------------------------
bool TerrainGeometry::queryResult(Ogre::SceneQuery::WorldFragment *fragment,Ogre::Real distance)
{
        _distance_to_terrain = distance;
        return false;
}
//------------------------------------------------------------------------------------------------
const Ogre::Vector3& TerrainGeometry::getPosition()
{
        return Ogre::Vector3::ZERO;
}

//------------------------------------------------------------------------------------------------
const Ogre::Quaternion& TerrainGeometry::getOrientation()
{
        return Ogre::Quaternion::ZERO;
}

//------------------------------------------------------------------------------------------------
TerrainGeometry::~TerrainGeometry()
{
        _world->getSceneManager()->destroyQuery(_ray_query);
}

//------------------------------------------------------------------------------------------------
std::list<Ogre::Plane>* PlaneBoundedRegionGeometry::_planeCallback(void* data,int x,int z)
{
    PlaneBoundedRegionGeometry* terrain = (PlaneBoundedRegionGeometry*)data;

    if (terrain->_listener)
    {
        return terrain->_listener->planesAt(Vector3((Ogre::Real)x,
            terrain->_max_height,
            (Ogre::Real)z));
    }
    else
    {
        return  terrain->planesAt(Vector3((Ogre::Real)x, 
                terrain->_max_height, 
                (Ogre::Real)z));
    }
    return 0;
}
//------------------------------------------------------------------------------------------------
PlaneBoundedRegionGeometry::PlaneBoundedRegionGeometry (World *world, Space* space,
                                                        const Ogre::AxisAlignedBox &Size) : 
    Geometry(world,space)
{

    _ray = Ogre::Ray (Ogre::Vector3::ZERO, Ogre::Vector3::NEGATIVE_UNIT_Y);
    _ray_query = _world->getSceneManager()->createRayQuery(_ray);

    _ray_query->setQueryTypeMask(Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK );
    _ray_query->setWorldFragmentType(Ogre::SceneQuery::WFT_SINGLE_INTERSECTION); 

}

//------------------------------------------------------------------------------------------------
Ogre::Real PlaneBoundedRegionGeometry::getPointDepth(const Ogre::Vector3& point)
{
    //dGetDepthFn(_geom,(dReal)point.x,(dReal)point.y,(dReal)point.z);
    return (Real) 0.0;
}

//------------------------------------------------------------------------------------------------
void PlaneBoundedRegionGeometry::setPlaneListener(PlaneBoundedRegionGeometryPlaneListener* listener)
{
    _listener = listener;
}
//------------------------------------------------------------------------------------------------
bool PlaneBoundedRegionGeometry::queryResult(Ogre::MovableObject *obj, Ogre::Real distance)
{
    return false;
}

//------------------------------------------------------------------------------------------------
bool PlaneBoundedRegionGeometry::queryResult(Ogre::SceneQuery::WorldFragment *fragment,Ogre::Real distance)
{
    _distance_to_terrain = distance;
    return false;
}
//------------------------------------------------------------------------------------------------
const Ogre::Vector3& PlaneBoundedRegionGeometry::getPosition()
{
    return Ogre::Vector3::ZERO;
}

//------------------------------------------------------------------------------------------------
const Ogre::Quaternion& PlaneBoundedRegionGeometry::getOrientation()
{
    return Ogre::Quaternion::ZERO;
}

//------------------------------------------------------------------------------------------------
PlaneBoundedRegionGeometry::~PlaneBoundedRegionGeometry()
{
    _world->getSceneManager()->destroyQuery(_ray_query);
}