source: code/branches/physics/src/ogreode/OgreOdeCollision.cpp @ 1932

#include "OgreOdePrecompiledHeaders.h"
#include "OgreOdeCollision.h"

#include "OgreOdeSpace.h"
#include "OgreOdeGeometry.h"

using namespace OgreOde;
using namespace Ogre;

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

//------------------------------------------------------------------------------------------------
CollisionCallback::CollisionCallback()
{
}

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

//-----------------------------------------------------------------------
void CollisionCallback::collisionCallback(Space* spaceFirst, Space* spaceSecond)
{
        spaceFirst->collide(static_cast<CollisionCallback*>(this), spaceSecond);
}
        
//-----------------------------------------------------------------------
void CollisionCallback::collisionCallback(Space* space, Geometry* geometry)
{
        space->collide(static_cast<CollisionCallback*>(this), geometry);
}
        
//-----------------------------------------------------------------------
void CollisionCallback::collisionCallback(Space* space)
{
        space->collide(static_cast<CollisionCallback*>(this));
}
        
//-----------------------------------------------------------------------
void CollisionCallback::collisionCallback(Geometry* geometryFirst, Geometry* geometrySecond)
{
        geometryFirst->collide(geometrySecond, static_cast<CollisionListener*>(this));
}

//-----------------------------------------------------------------------
void CollisionCallback::collisionCallback(void *data, dGeomID geom_a, dGeomID geom_b)
{
        CollisionCallback* colCallback = (CollisionCallback*) data;

        const bool a_space = (dGeomIsSpace(geom_a))?true:false;
        const bool b_space = (dGeomIsSpace(geom_b))?true:false;
        
        void* const ptr_a = dGeomGetData(geom_a);
        void* const ptr_b = dGeomGetData(geom_b);

        if(a_space  || b_space )
        {
                // Collide a space with a space
                if(a_space && b_space) 
                        colCallback->collisionCallback(static_cast<Space*>(ptr_a), static_cast<Space*>(ptr_b));
                        //static_cast<Space*>(ptr_a)->collide(static_cast<Space*>(ptr_b), colCallback);
                else if(a_space) 
                        colCallback->collisionCallback(static_cast<Space*>(ptr_a), static_cast<Geometry*>(ptr_b));
                        //static_cast<Space*>(ptr_a)->collide(static_cast<Geometry*>(ptr_b), colCallback);
                else 
                        colCallback->collisionCallback(static_cast<Space*>(ptr_b), static_cast<Geometry*>(ptr_a));
                        //static_cast<Space*>(ptr_b)->collide(static_cast<Geometry*>(ptr_a), colCallback);

                // Collide geometries internal to the spaces
                if(a_space) 
            static_cast<Space*>(ptr_a)->collide(colCallback);

                if(b_space) 
            static_cast<Space*>(ptr_b)->collide(colCallback);
        }
    else
        {
                // Collide a geom with a geom, i.e. generate contacts
                colCallback->collisionCallback(static_cast<Geometry*>(ptr_a), static_cast<Geometry*>(ptr_b));
                //static_cast<Geometry*>(ptr_a)->collide(static_cast<Geometry*>(ptr_b), static_cast<CollisionListener*>(colCallback));
        }
}


//------------------------------------------------------------------------------------------------
Contact::Contact()
{
}
//------------------------------------------------------------------------------------------------
Contact::~Contact()
{
}
//------------------------------------------------------------------------------------------------
const Ogre::Vector3& Contact::getPosition()
{
        _position.x = (Real)(_contact->geom.pos[0]);
        _position.y = (Real)(_contact->geom.pos[1]);
        _position.z = (Real)(_contact->geom.pos[2]);
        return _position;
}
//------------------------------------------------------------------------------------------------
const Ogre::Vector3& Contact::getNormal()
{
        _normal.x = (Real)(_contact->geom.normal[0]);
        _normal.y = (Real)(_contact->geom.normal[1]);
        _normal.z = (Real)(_contact->geom.normal[2]);
        return _normal;
}
//------------------------------------------------------------------------------------------------
Real Contact::getPenetrationDepth()
{
        return (Real)(_contact->geom.depth);
}
//------------------------------------------------------------------------------------------------
Geometry* Contact::getFirstGeometry()
{
        dGeomID g = _contact->geom.g1;
        if(!g) return 0;

        return (Geometry*)dGeomGetData(g);
}
//------------------------------------------------------------------------------------------------
Geometry* Contact::getSecondGeometry()
{
        dGeomID g = _contact->geom.g2;
        if(!g) return 0;

        return (Geometry*)dGeomGetData(g);
}
//------------------------------------------------------------------------------------------------
int Contact::getFirstSide()
{
   return _contact->geom.side1;
}
//------------------------------------------------------------------------------------------------
int Contact::getSecondSide()
{
   return _contact->geom.side2;
} 
//------------------------------------------------------------------------------------------------
void Contact::setFirstFrictionDirection(const Ogre::Vector3& vector)
{
        _contact->fdir1[0] = (dReal)vector.x;
        _contact->fdir1[1] = (dReal)vector.y;
        _contact->fdir1[2] = (dReal)vector.z;

        _contact->surface.mode |= (int)Flag_UseFirstFrictionDirection;
}
//------------------------------------------------------------------------------------------------
void Contact::setCoulombFriction(Real mu,Real additional_mu)
{
        _contact->surface.mu = mu;
        
        if(additional_mu >= 0.0)
        {
                _contact->surface.mu2 = additional_mu;
                _contact->surface.mode |= (int)Flag_UseAdditionalFriction;
        }
}
//------------------------------------------------------------------------------------------------
void Contact::setBouncyness(Real bouncyness,Real velocity_threshold)
{
        _contact->surface.bounce = bouncyness;
        if(velocity_threshold >= 0.0)
        {
                _contact->surface.bounce_vel = velocity_threshold;
        }
        _contact->surface.mode |= (int)Flag_SurfaceIsBouncy;
}
//------------------------------------------------------------------------------------------------
void Contact::setSoftness(Real ERP,Real CFM)
{
        _contact->surface.soft_erp = ERP;
        _contact->surface.soft_cfm = CFM;

        _contact->surface.mode |= (int)Flag_UseERP;
        _contact->surface.mode |= (int)Flag_UseCFM;
}
//------------------------------------------------------------------------------------------------
void Contact::setIndependentMotion(Real velocity,Real additional_velocity)
{
        _contact->surface.motion1 = velocity;
        _contact->surface.mode |= (int)Flag_IndependentMotion; 

        if(additional_velocity >= 0.0)
        {
                _contact->surface.motion2 = additional_velocity;
                _contact->surface.mode |= (int)Flag_AdditionalIndependentMotion;
        }
}
//------------------------------------------------------------------------------------------------
void Contact::setForceDependentSlip(Real FDS)
{
        _contact->surface.slip1 = FDS;
        _contact->surface.mode |= (int)Flag_UseFDS;
}
//------------------------------------------------------------------------------------------------
void Contact::setAdditionalFDS(Real FDS)
{
        _contact->surface.slip2 = FDS;
        _contact->surface.mode |= (int)Flag_UseAdditionalFDS;
}
//------------------------------------------------------------------------------------------------
void Contact::setFrictionMode(Contact::Flag flag)
{
        assert((flag == Flag_FrictionPyramid)||(flag == Flag_BothFrictionPyramids)||(flag == Flag_AdditionalFrictionPyramid));
        _contact->surface.mode |= (int)flag;                    
}
//------------------------------------------------------------------------------------------------
ContactMapCollisionListener::ContactMapCollisionListener()
{
}
//------------------------------------------------------------------------------------------------
bool ContactMapCollisionListener::collision(Contact* contact)
{
        return false;
        /*
        size_t mt1 = contact->getFirstGeometry()->getMaterialType();
        if(!mt1) return false;
        
        size_t mt2 = contact->getSecondGeometry()->getMaterialType();
        if(!mt2) return false;

        Contact *c = getContactPtr(mt1,mt2);
        if(!c) return false;

        contact->_contact->fdir1[0] = c->_contact->fdir1[0];
        contact->_contact->fdir1[1] = c->_contact->fdir1[1];
        contact->_contact->fdir1[2] = c->_contact->fdir1[2];

        contact->_contact->surface.mu = c->_contact->surface.mu;
        contact->_contact->surface.mu2 = c->_contact->surface.mu2;

        contact->_contact->surface.bounce = c->_contact->surface.bounce;
        contact->_contact->surface.bounce_vel = c->_contact->surface.bounce_vel;

        contact->_contact->surface.soft_erp = c->_contact->surface.soft_erp;
        contact->_contact->surface.soft_cfm = c->_contact->surface.soft_cfm;

        contact->_contact->surface.motion1 = c->_contact->surface.motion1;
        contact->_contact->surface.motion2 = c->_contact->surface.motion2;

        contact->_contact->surface.slip1 = c->_contact->surface.slip1;
        contact->_contact->surface.slip2 = c->_contact->surface.slip2;

        contact->_contact->surface.mode = c->_contact->surface.mode;

        return true;
        */
}
//------------------------------------------------------------------------------------------------
void ContactMapCollisionListener::createContact(MaterialID materialA,MaterialID materialB)
{
        std::map<MaterialID,MaterialMap* >::iterator i = _map.find(materialA);
        if(i == _map.end())
        {
                _map.insert(MaterialMapPair(materialA,new MaterialMap()));
                i = _map.find(materialA);
        }

        MaterialMap::iterator j = i->second->find(materialB);
        if(j == i->second->end())
        {
                Contact *c = new Contact();
                memset(c->_contact,0,sizeof(dContact));

                i->second->insert(std::pair<MaterialID,Contact*>(materialB,c));
        }
}
//------------------------------------------------------------------------------------------------
Contact *ContactMapCollisionListener::getContactPtr(MaterialID materialA,MaterialID materialB)
{
        Contact *c = 0;
        MaterialID A = materialA;
        MaterialID B = materialB;

        std::map<MaterialID,MaterialMap* >::iterator i = _map.find(A);
        if(i == _map.end())
        {
                B = materialA;
                A = materialB;

                i = _map.find(A);
        }

        if(i != _map.end())
        {
                MaterialMap::iterator j = i->second->find(B);
                if(j != i->second->end())
                {
                        c = j->second;
                }
        }

        return c;
}
//------------------------------------------------------------------------------------------------
ContactMapCollisionListener::~ContactMapCollisionListener()
{
        std::map<MaterialID,MaterialMap* >::iterator i = _map.begin();
        std::map<MaterialID,MaterialMap* >::iterator i_end = _map.end();

        for(;i != i_end;++i)
        {
                MaterialMap::iterator j = i->second->begin();
                MaterialMap::iterator j_end = i->second->end();

                for(;j != j_end;++j) 
                        delete j->second;

                delete i->second;
        }
}
//------------------------------------------------------------------------------------------------