618 lines
16 KiB
C++
618 lines
16 KiB
C++
#include "RigidBodySimulation.h"
|
|
|
|
// local includes
|
|
//#include "scriptsystem.h"
|
|
|
|
// library includes
|
|
#include "Utilities/StringUtilities.h"
|
|
#include "Utilities/Log.h"
|
|
//#include "Utilities/xmlconfig.h"
|
|
|
|
// system includes
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
|
|
namespace BlueCore
|
|
{
|
|
static RigidBodySimulation* gRigidBodySimulation;
|
|
|
|
//------------------------------------------------------------------------------
|
|
RigidBody::RigidBody(RigidBodySimulation* simulation, dWorldID world,
|
|
dSpaceID space) :
|
|
_Body( 0), _Space( 0), _Simulation(simulation)
|
|
{
|
|
_Body = dBodyCreate(world);
|
|
_Space = dHashSpaceCreate(space);
|
|
dHashSpaceSetLevels(_Space, -10, 10);
|
|
dBodySetData(_Body, this);
|
|
clog << ">>> RigidBody constructed..."<< endlog;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
RigidBody::~RigidBody()
|
|
{
|
|
dSpaceDestroy(_Space);
|
|
dBodyDestroy(_Body);
|
|
|
|
std::vector<CollisionMesh>::iterator iter;
|
|
for (iter = _CollisionMeshes.begin(); iter != _CollisionMeshes.end(); iter++)
|
|
{
|
|
dGeomTriMeshDataDestroy((*iter).trimeshdata);
|
|
}
|
|
|
|
clog << ">>> RigidBody destructed..."<< endlog;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
|
|
const Vector3 &RigidBody::getPosition() const
|
|
{
|
|
return _Position;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::setPosition(const Vector3 &v)
|
|
{
|
|
dBodySetPosition(_Body, v.x, v.y, v.z);
|
|
_Position = v;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
const Vector3 &RigidBody::getLinearVelocity() const
|
|
{
|
|
return _LinearVelocity;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::setLinearVelocity(const Vector3 &v)
|
|
{
|
|
dBodySetLinearVel(_Body, v.x, v.y, v.z);
|
|
_LinearVelocity = v;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
const Quaternion &RigidBody::getOrientation() const
|
|
{
|
|
return _Orientation;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::setOrientation(const Quaternion &q)
|
|
{
|
|
|
|
dBodySetQuaternion(_Body, ( const dQuaternion & ) q );
|
|
|
|
_Orientation = q;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
const Vector3 &RigidBody::getAngularVelocity() const
|
|
{
|
|
return _AngularVelocity;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::applyGlobalForce(const Vector3 &force, const Vector3 &point)
|
|
{
|
|
dBodyAddForceAtPos(_Body, force.x, force.y, force.z, point.x, point.y,
|
|
point.z);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::applyGlobalForce(const Vector3 &force)
|
|
{
|
|
dBodyAddForce(_Body, force.x, force.y, force.z);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::applyLocalForce(const Vector3 &force, const Vector3 &point)
|
|
{
|
|
if (point.isZero() )
|
|
{
|
|
dBodyAddRelForce(_Body, force.x, force.y, force.z);
|
|
}
|
|
else
|
|
{
|
|
dBodyAddRelForceAtRelPos(_Body, force.x, force.y, force.z, point.x,
|
|
point.y, point.z);
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::applyLocalForce(const Vector3 &force)
|
|
{
|
|
dBodyAddRelForce(_Body, force.x, force.y, force.z);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
Scalar RigidBody::getMass() const
|
|
{
|
|
dMass m;
|
|
dBodyGetMass(_Body, &m);
|
|
return m.mass;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::saveState()
|
|
{
|
|
// save position
|
|
|
|
const Scalar *p = dBodyGetPosition(_Body);
|
|
_Position.x = p[0];
|
|
_Position.y = p[1];
|
|
_Position.z = p[2];
|
|
|
|
// save linear velocity
|
|
const Scalar *v = dBodyGetLinearVel(_Body);
|
|
_LinearVelocity.x = v[0];
|
|
_LinearVelocity.y = v[1];
|
|
_LinearVelocity.z = v[2];
|
|
|
|
// save orientation
|
|
const Scalar *q = dBodyGetQuaternion(_Body);
|
|
_Orientation.w = q[0];
|
|
_Orientation.x = q[1];
|
|
_Orientation.y = q[2];
|
|
_Orientation.z = q[3];
|
|
|
|
// save angular velocity
|
|
const Scalar *a = dBodyGetAngularVel(_Body);
|
|
_AngularVelocity.x = a[0];
|
|
_AngularVelocity.y = a[1];
|
|
_AngularVelocity.z = a[2];
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
unsigned int RigidBody::addCollisionMesh(const std::string &meshname,
|
|
Scalar density)
|
|
{
|
|
if (_Simulation.valid() == false)
|
|
return 0;
|
|
|
|
const RigidBodySimulation::Trimesh *trimesh =
|
|
_Simulation->getTrimesh(meshname);
|
|
|
|
if (trimesh)
|
|
{
|
|
CollisionMesh collisionmesh;
|
|
|
|
collisionmesh.trimeshdata = dGeomTriMeshDataCreate();
|
|
dGeomTriMeshDataBuildSingle(collisionmesh.trimeshdata,
|
|
trimesh->vertices.const_data(), 3 * sizeof(float),
|
|
trimesh->vertices.count() / 3, trimesh->indices.const_data(),
|
|
trimesh->indices.count(), 3 * sizeof(int));
|
|
|
|
collisionmesh.geom = dCreateTriMesh(_Space, collisionmesh.trimeshdata,
|
|
0, 0, 0);
|
|
dGeomSetBody(collisionmesh.geom, _Body);
|
|
|
|
dMass mass;
|
|
dMassSetTrimesh( &mass, density, collisionmesh.geom);
|
|
//dMassSetBox( &mass, density, 5.0, 5.0, 5.0 );
|
|
dBodySetMass(_Body, &mass);
|
|
|
|
_CollisionMeshes.push_back(collisionmesh);
|
|
return _CollisionMeshes.size();
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::setCollisionMeshPosition(unsigned int geom,
|
|
const Vector3 &position)
|
|
{
|
|
if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
|
|
{
|
|
dGeomSetOffsetPosition(_CollisionMeshes[geom-1].geom, position.x,
|
|
position.y, position.z);
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::setCollisionMeshRotation(unsigned int geom,
|
|
const Quaternion &rotation)
|
|
{
|
|
if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
|
|
{
|
|
dGeomSetOffsetQuaternion(_CollisionMeshes[geom-1].geom,
|
|
( dReal * ) &rotation );
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::getCollisionMeshPosition(unsigned int geom, Vector3 &position)
|
|
{
|
|
if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
|
|
{
|
|
const Scalar*p = dGeomGetOffsetPosition(_CollisionMeshes[geom-1].geom);
|
|
position.x = p[0];
|
|
position.y = p[1];
|
|
position.z = p[2];
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::getCollisionMeshRotation(unsigned int geom,
|
|
Quaternion &rotation)
|
|
{
|
|
if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
|
|
{
|
|
dGeomGetOffsetQuaternion(_CollisionMeshes[geom-1].geom,
|
|
( dReal * ) &rotation );
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::enableCollisionMesh(unsigned int geom)
|
|
{
|
|
if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
|
|
{
|
|
dGeomEnable(_CollisionMeshes[geom-1].geom);
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::disableCollisionMesh(unsigned int geom)
|
|
{
|
|
if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
|
|
{
|
|
dGeomDisable(_CollisionMeshes[geom-1].geom);
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
unsigned int RigidBody::getCollisionMeshId(dGeomID geom)
|
|
{
|
|
for (unsigned int i = 0; i < _CollisionMeshes.size(); i++)
|
|
{
|
|
if (_CollisionMeshes[i].geom == geom)
|
|
return i + 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
HSQOBJECT RigidBody::getCollisionHandler()
|
|
{
|
|
return _CollisionHandler;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBody::setCollisionHandler(HSQOBJECT handler)
|
|
{
|
|
_CollisionHandler = handler;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
RigidBodySimulation::RigidBodySimulation(ScriptSystem *scriptsystem) :
|
|
_world( 0), _space( 0), _contactgroup( 0), _delta( 0.0), _steps( 0),
|
|
_ScriptSystem(scriptsystem)
|
|
{
|
|
dInitODE();
|
|
|
|
gRigidBodySimulation = this;
|
|
//XmlConfig config("config.xml");
|
|
|
|
clog << ">>> initialize RigidBodySimulation..."<< endline;
|
|
|
|
Scalar erp = 1.0, cfm = 0.0;
|
|
_stepSize = 0.02;
|
|
_maxContacts = 5;
|
|
/*
|
|
config.getDouble("RigidBodySimulation", "ERP", erp, 1.0);
|
|
config.getDouble("RigidBodySimulation", "CFM", cfm, 0.0);
|
|
config.getDouble("RigidBodySimulation", "StepSize", _stepSize, 0.02);
|
|
config.getInt("RigidBodySimulation", "MaxContacts", _maxContacts, 5);
|
|
*/
|
|
// sanity check
|
|
|
|
if (erp > 1.0)
|
|
erp = 1.0;
|
|
else if (erp < 0.0001)
|
|
erp = 0.0001;
|
|
|
|
clog << " ERP: "<< erp << endline;
|
|
|
|
// sanity check
|
|
if (cfm > 1.0)
|
|
cfm = 1.0;
|
|
else if (cfm < 0.0)
|
|
cfm = 0.0;
|
|
|
|
clog << " CFM: "<< cfm << endline;
|
|
|
|
// sanity check
|
|
if (_stepSize < 0.0001)
|
|
_stepSize = 0.0001;
|
|
else if (_stepSize > 1.0)
|
|
_stepSize = 1.0;
|
|
|
|
clog << " step size: "<< _stepSize << endline;
|
|
|
|
// sanity check
|
|
if (_maxContacts < 1)
|
|
_maxContacts = 1;
|
|
else if (_maxContacts > 500)
|
|
_maxContacts = 500;
|
|
|
|
clog << " max counts: "<< _maxContacts << endline;
|
|
|
|
_world = dWorldCreate();
|
|
|
|
dWorldSetERP(_world, erp);
|
|
|
|
dWorldSetCFM(_world, cfm);
|
|
|
|
_space = dHashSpaceCreate( 0);
|
|
|
|
dHashSpaceSetLevels(_space, -10, 10);
|
|
|
|
_contactgroup = dJointGroupCreate( 0);
|
|
/*
|
|
// Bullet
|
|
btVector3 worldAabbMin(-100000,-100000,-100000);
|
|
btVector3 worldAabbMax(100000,100000,100000);
|
|
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,32766);
|
|
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
|
|
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
|
|
_World = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
|
|
*/
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
RigidBodySimulation::~RigidBodySimulation()
|
|
{
|
|
|
|
if (_contactgroup)
|
|
dJointGroupDestroy(_contactgroup);
|
|
|
|
if (_space)
|
|
dSpaceDestroy(_space);
|
|
|
|
if (_world)
|
|
dWorldDestroy(_world);
|
|
|
|
std::map<std::string, Trimesh *>::iterator iter;
|
|
for (iter = _Trimeshes.begin(); iter != _Trimeshes.end(); iter++)
|
|
{
|
|
Trimesh *trimesh = iter->second;
|
|
if (trimesh)
|
|
delete trimesh;
|
|
}
|
|
|
|
_Trimeshes.clear();
|
|
|
|
dCloseODE();
|
|
|
|
gRigidBodySimulation = 0;
|
|
|
|
clog << ">>> RigidBodySimulation destructed ..."<< endline;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
RigidBody *RigidBodySimulation::createRigidBody()
|
|
{
|
|
RigidBody *rigidBody = new RigidBody ( this, _world, _space );
|
|
|
|
_bodies.push_back(rigidBody);
|
|
|
|
return rigidBody;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBodySimulation::deleteRigidBody(RigidBody *rigidBody)
|
|
{
|
|
#ifdef DEBUG_PHYSICS
|
|
clog << ">>> delete rigid body" << endline;
|
|
#endif
|
|
_bodies.remove(rigidBody);
|
|
delete rigidBody;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBodySimulation::removeAll()
|
|
{
|
|
std::list<RigidBody *>::iterator i;
|
|
|
|
for (i = _bodies.begin(); i != _bodies.end(); i++)
|
|
delete *i;
|
|
|
|
_bodies.clear();
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBodySimulation::saveStates()
|
|
{
|
|
std::list<RigidBody *>::iterator i;
|
|
|
|
for (i = _bodies.begin(); i != _bodies.end(); i++)
|
|
( *i )->saveState();
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBodySimulation::nearCallback(void *data, dGeomID o1, dGeomID o2)
|
|
{
|
|
if (gRigidBodySimulation == 0)
|
|
return;
|
|
|
|
if (dGeomIsSpace(o1) && dGeomIsSpace(o2) )
|
|
{
|
|
|
|
// colliding a space with something
|
|
dSpaceCollide2(o1, o2, data, &nearCallback);
|
|
/*
|
|
// collide all geoms internal to the space(s)
|
|
if( dGeomIsSpace(o1) )
|
|
dSpaceCollide( (dxSpace *)o1, data, &nearCallback );
|
|
|
|
if( dGeomIsSpace(o2) )
|
|
dSpaceCollide( (dxSpace *)o2, data,&nearCallback );
|
|
*/
|
|
}
|
|
else
|
|
{
|
|
static Buffer<dContact> contacts;
|
|
|
|
if (contacts.count() == 0)
|
|
contacts.create(gRigidBodySimulation->getMaxContacts());
|
|
|
|
dBodyID b1, b2;
|
|
b1 = dGeomGetBody(o1);
|
|
b2 = dGeomGetBody(o2);
|
|
|
|
// exit without doing anything if the two bodies are connected by a joint
|
|
if (b1 && b2 && dAreConnected(b1, b2) )
|
|
return;
|
|
|
|
for (unsigned int i = 0; i < contacts.count(); i++)
|
|
{
|
|
contacts[i].surface.mode = 0; //dContactBounce;
|
|
contacts[i].surface.mu = 1.0;
|
|
contacts[i].surface.mu2 = 0;
|
|
contacts[i].surface.bounce = 0.1;
|
|
contacts[i].surface.bounce_vel = 0.1;
|
|
contacts[i].surface.soft_cfm = 0.01;
|
|
}
|
|
|
|
unsigned int collisions = dCollide(o1, o2, contacts.count(),
|
|
&contacts[0].geom, sizeof(dContact));
|
|
|
|
//clog << "collide" << endline;
|
|
|
|
unsigned int actual_collisions = 0;
|
|
|
|
for (unsigned int i = 0; i < collisions; i++)
|
|
{
|
|
clog << "depth: "<< contacts[i].geom.depth<< endline;
|
|
dJointID joint = dJointCreateContact(gRigidBodySimulation->_world,
|
|
gRigidBodySimulation->_contactgroup, &contacts[i]);
|
|
dJointAttach(joint, b1, b2);
|
|
}
|
|
|
|
#ifdef DEBUG_PHYSICS
|
|
if ( collisions == contacts.count() )
|
|
clog << "!!! Max collisions reached!" << endline;
|
|
|
|
#endif
|
|
|
|
if (collisions > 0)
|
|
{
|
|
//clog << "rbsim handle collision" << endline;
|
|
//ScriptManager::getSingleton()->handleCollision ( o1, o2 );
|
|
}
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
int RigidBodySimulation::getSteps()
|
|
{
|
|
return _steps;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
Scalar RigidBodySimulation::getStepSize()
|
|
{
|
|
return _stepSize;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
int RigidBodySimulation::getMaxContacts()
|
|
{
|
|
return _maxContacts;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void RigidBodySimulation::updateSteps(Scalar delta)
|
|
{
|
|
_delta += delta;
|
|
|
|
if (_delta < 0.0)
|
|
return;
|
|
|
|
_steps = ( int ) floor(_delta / _stepSize);
|
|
|
|
if (_steps > 10)
|
|
_steps = 10;
|
|
|
|
_delta -= _steps * _stepSize;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool RigidBodySimulation::step()
|
|
{
|
|
if (_steps > 0)
|
|
{
|
|
dSpaceCollide(_space, 0, &nearCallback);
|
|
dWorldQuickStep(_world, _stepSize);
|
|
StepSignal(_stepSize);
|
|
dJointGroupEmpty(_contactgroup);
|
|
_steps--;
|
|
}
|
|
|
|
if (_steps > 0)
|
|
return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
const RigidBodySimulation::Trimesh *RigidBodySimulation::getTrimesh(
|
|
const std::string &name)
|
|
{
|
|
Trimesh *trimesh = 0;
|
|
|
|
std::map<std::string, Trimesh *>::const_iterator result;
|
|
result = _Trimeshes.find(name);
|
|
|
|
if (result != _Trimeshes.end() )
|
|
{
|
|
trimesh = result->second;
|
|
}
|
|
/*
|
|
if ( trimesh == 0 )
|
|
{
|
|
MeshLoader loader;
|
|
|
|
clog << ">>> loading collision trimesh: " << name << endline;
|
|
loader.load ( name + ".3ds" );
|
|
|
|
if ( loader.objects().count() > 0 )
|
|
{
|
|
trimesh = new Trimesh();
|
|
|
|
MeshLoader::Object &object = loader.objects().item ( 0 );
|
|
|
|
clog << " " << object.vertices.count() << " Vertices" << endline;
|
|
trimesh->vertices.create ( object.vertices.count() * 3 );
|
|
|
|
clog << " " << object.faces.count() * 3 << " Indices" << endline;
|
|
trimesh->indices.create ( object.faces.count() * 3 );
|
|
|
|
unsigned int i;
|
|
|
|
for ( i = 0; i < object.vertices.count(); i++ )
|
|
{
|
|
trimesh->vertices[i*3] = -object.vertices[i].x;
|
|
trimesh->vertices[i*3+1] = object.vertices[i].y;
|
|
trimesh->vertices[i*3+2] = object.vertices[i].z;
|
|
}
|
|
|
|
for ( i = 0; i < object.faces.count(); i++ )
|
|
{
|
|
trimesh->indices[i*3] = object.faces[i].a;
|
|
trimesh->indices[i*3+2] = object.faces[i].b;
|
|
trimesh->indices[i*3+1] = object.faces[i].c;
|
|
}
|
|
|
|
_Trimeshes[name] = trimesh;
|
|
}
|
|
}
|
|
*/
|
|
return trimesh;
|
|
}
|
|
|
|
} // namespace BlueCore
|