I looked at the irrlicht ode wrapper but I don't want to use it if I don't have to. Maybe I'm just being silly but I really don't want two scene nodes for every physics enabled element in my scene. That possibly makes worst case iteration n*2 instead of n.
Below is my physics class I call the poll after the GUI & scene are rendered each frame. please forgive all the cout calls but I have been trying to debug this thing for a while.
Code: Select all
/*
Copyright © 2009 Christopher Joseph Dean Schaefer (disks86)
This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "PhysicsManager.hpp"
namespace Enigma
{
PhysicsManager::PhysicsManager()
{
dInitODE(); //placed here so load and unload can be called for map changes.
this->PreInit();
}
PhysicsManager::~PhysicsManager()
{
if(!this->mIsUnloaded)
{
this->Unload();
}
dCloseODE(); //placed here so load and unload can be called for map changes.
}
void PhysicsManager::PreInit()
{
this->mIsUnloaded=false;
this->mApplication=NULL;
this->mWorldId=0;
this->mSpaceId=0;
this->mJointGroupId=0;
this->mStepSize=0.1f;
//this->mEarthGrativty = irr::core::vector3df(0,-9.81,0);
this->mEarthGrativty = irr::core::vector3df(0,-0.0005,0);
}
void PhysicsManager::Init(IApplication* application)
{
this->mApplication=application;
}
void PhysicsManager::Load()
{
this->mIsUnloaded=false;
this->mWorldId=dWorldCreate();
this->mSpaceId=dSimpleSpaceCreate(0);
this->mJointGroupId=dJointGroupCreate(0);
//so you don't fall forever
dCreatePlane(0, 0, 1.0, 0, 0);
dWorldSetGravity(this->mWorldId,
this->mEarthGrativty.X,
this->mEarthGrativty.Y,
this->mEarthGrativty.Z);
}
void PhysicsManager::Unload()
{
if(!this->mIsUnloaded)
{
this->mIsUnloaded=true;
dJointGroupDestroy(this->mJointGroupId);
dSpaceDestroy(this->mSpaceId);
dWorldDestroy(this->mWorldId);
}
}
void PhysicsManager::Reload()
{
this->Unload();
this->Load();
}
void PhysicsManager::Poll()
{
if(!this->mIsUnloaded)
{
this->UpdateOde(); //so I can move things manually sometimes.
dSpaceCollide(this->mSpaceId,(void*)this,&Enigma::CollisionCallback);
dWorldQuickStep(this->mWorldId,this->mStepSize);
dJointGroupEmpty(this->mJointGroupId);
this->UpdateScene();
}
}
void PhysicsManager::UpdateOde()
{
irr::core::vector3df position;
irr::core::aabbox3d<f32> boundingBox;
irr::core::vector3df extent;
std::list<dGeomID>::iterator i=NULL;
irr::scene::ISceneNode* node=NULL;
for(i=mGeometries.begin();i!=mGeometries.end();i++)
{
node=(irr::scene::ISceneNode*)dGeomGetData(*i);
if(node!=NULL)
{
if(node->getParent()==NULL)
{
position = node->getPosition();
dGeomSetPosition(
*i,
position.X,
position.Y,
position.Z);
//std::cout << "in position: " << position.X << " " << position.Y << " " << position.Z << std::endl;
boundingBox=node->getBoundingBox();
extent=boundingBox.getExtent();
dGeomBoxSetLengths(
*i,
extent.X,
extent.Y,
extent.Z);
//std::cout << "in extent: " << extent.X << " " << extent.Y << " " << extent.Z << std::endl;
}
else
{
position = node->getPosition() + node->getParent()->getPosition();
dGeomSetPosition(
*i,
position.X,
position.Y,
position.Z);
//std::cout << "relative in position: " << position.X << " " << position.Y << " " << position.Z << std::endl;
boundingBox=node->getBoundingBox();
extent=boundingBox.getExtent() + node->getParent()->getPosition();
dGeomBoxSetLengths(
*i,
extent.X,
extent.Y,
extent.Z);
//std::cout << "relative in extent: " << extent.X << " " << extent.Y << " " << extent.Z << std::endl;
}
}
}
}
void PhysicsManager::UpdateScene()
{
irr::core::vector3df position;
irr::core::vector3df relativePosition;
irr::core::vector3df rotation;
dReal* odePosition;
dQuaternion odeRotation;
std::list<dGeomID>::iterator i=NULL;
irr::scene::ISceneNode* node=NULL;
for(i=mGeometries.begin();i!=mGeometries.end();i++)
{
odePosition=(dReal*)dGeomGetPosition(*i);
position.set(
(irr::f32)odePosition[0],
(irr::f32)odePosition[1],
(irr::f32)odePosition[2]);
dGeomGetQuaternion(*i,odeRotation);
QuaternionToEuler(odeRotation,rotation);
node=(irr::scene::ISceneNode*)dGeomGetData(*i);
if(node!=NULL)
{
if(node->getParent()==NULL)
{
node->setPosition(position);
//std::cout << "out position: " << relativePosition.X << " " << relativePosition.Y << " " << relativePosition.Z << std::endl;
}
else
{
relativePosition = position - node->getParent()->getPosition();
node->setPosition(relativePosition);
//std::cout << "relative out position: " << relativePosition.X << " " << relativePosition.Y << " " << relativePosition.Z << std::endl;
}
node->setRotation(rotation);
node->updateAbsolutePosition();
}
}
}
//add scene node to ode world.
void PhysicsManager::AddToWorld(irr::scene::ISceneNode* node)
{
if(node!=NULL)
{
irr::core::aabbox3d<f32> boundingBox;
irr::core::vector3df extent;
irr::core::vector3df center;
irr::core::vector3df position;
irr::core::vector3df scale;
irr::core::vector3df rotation;
dQuaternion quaternion;
boundingBox=node->getBoundingBox();
position=node->getAbsolutePosition();
extent=boundingBox.getExtent();
center=boundingBox.getCenter();
rotation = node->getAbsoluteTransformation().getRotationDegrees();
//scale = node->getParent()->getScale();
EulerToQuaternion(rotation,quaternion);
dGeomID boxGeometry=0;
//Build box shapped geometry for ODE to use from node bounding box.
boxGeometry=dCreateBox(
this->mSpaceId,
(dReal)extent.X,
(dReal)extent.Y,
(dReal)extent.Z);
if(boxGeometry!=NULL)
{
//Set the position of the ODE object using the node's position vector.
dGeomSetPosition(
boxGeometry,
position.X,
position.Y,
position.Z);
dGeomSetQuaternion(
boxGeometry,
quaternion);
//Assign node pointer to geometry for later use.
dGeomSetData(
boxGeometry,
(void*)node);
}
else
{
std::cout << "Failed to create bounding box for scene node." << std::endl;
}
}
else
{
std::cout << "Cannot register a null scene node with Ode." << std::endl;
}
}
void PhysicsManager::AddToWorld(EntitySceneNode* node)
{
if(node!=NULL)
{
irr::core::aabbox3d<f32> boundingBox;
irr::core::vector3df extent;
irr::core::vector3df center;
irr::core::vector3df position;
irr::core::vector3df scale;
irr::core::vector3df rotation;
dQuaternion quaternion;
boundingBox=node->getBoundingBox();
position=node->getAbsolutePosition();
//extent=boundingBox.getExtent();
extent = node->GetInnerSceneNode()->getMesh()->getBoundingBox().getExtent();
center=boundingBox.getCenter();
rotation = node->getAbsoluteTransformation().getRotationDegrees();
scale = node->getParent()->getScale();
EulerToQuaternion(rotation,quaternion);
dGeomID boxGeometry=0;
dBodyID nodeBodyId=0;
dMass bodyMass;
dReal density=1.0f; //guess
//Build box shapped geometry for ODE to use from node bounding box.
boxGeometry=dCreateBox(
this->mSpaceId,
(dReal)(extent.X*scale.X),
(dReal)(extent.Y*scale.Y),
(dReal)(extent.Z*scale.Z));
if(boxGeometry!=NULL)
{
//Set the position of the ODE object using the node's position vector.
dGeomSetPosition(
boxGeometry,
position.X,
position.Y,
position.Z);
dGeomSetQuaternion(
boxGeometry,
quaternion);
//Assign node pointer to geometry for later use.
dGeomSetData(
boxGeometry,
(void*)node);
//Create a new body that represents the node.
nodeBodyId=dBodyCreate(this->mWorldId);
if(nodeBodyId!=NULL)
{
//Assign zero mass to the body.
dMassSetZero(&bodyMass);
//Set the density of the mass.
/*dMassSetBox(
&bodyMass,
density,
(dReal)extent.X,
(dReal)extent.Y,
(dReal)extent.Z);*/
dMassSetBoxTotal(
&bodyMass,
density,
(dReal)extent.X,
(dReal)extent.Y,
(dReal)extent.Z);
//Assign the mass to the body.
/*dBodySetMass(
nodeBodyId,
&bodyMass);*/
//Assign the box geometry to the body.
dGeomSetBody(
boxGeometry,
nodeBodyId);
//Set the body's position just like I did with the geometry.
dBodySetPosition(
nodeBodyId,
position.X,
position.Y,
0.0); //everybody does this for some reason. position.Z
//Assign node pointer to body for later use.
dBodySetData(
nodeBodyId,
(void*)node);
node->SetBodyId(nodeBodyId);
node->SetGeometryId(boxGeometry);
//dBodySetGravityMode(nodeBodyId,0);
}
else
{
std::cout << "Failed to create rigid body for entity scene node." << std::endl;
}
this->mGeometries.push_back(boxGeometry);
}
else
{
std::cout << "Failed to create bounding box for entity scene node." << std::endl;
}
}
else
{
std::cout << "Cannot register a null entity scene node with Ode." << std::endl;
}
}
//add terrian to ode world
void PhysicsManager::AddToWorld(irr::scene::ITerrainSceneNode* node)
{
if(node!=NULL)
{
irr::core::vector3df position;
irr::core::aabbox3d<f32> boundingBox;
irr::core::vector3df extent;
dGeomID geometryId;
position=node->getPosition();
boundingBox=node->getBoundingBox();
extent=boundingBox.getExtent();
geometryId=this->CreateTriMesh(node->getMesh());
if(geometryId>0)
{
dGeomSetPosition(geometryId,position.X,position.Y,position.Z);
//dGeomSetData(geometryId,(void*)node);
std::cout << "added terrain scene node at position " << position.X << ", " << position.Y << ", " << position.Z << std::endl;
std::cout << "set terrain scene node extent to " << extent.X << ", " << extent.Y << ", " << extent.Z << std::endl;
dGeomSetData(geometryId,NULL); //don't need to do anything to terrian.
//this->mGeometries.push_front(geometryId);
}
else
{
std::cout << "failed to create terrain in Ode." << std::endl;
}
}
else
{
std::cout << "terrain must not be null." << std::endl;
}
}
//clean recode of trimesh function.
dGeomID PhysicsManager::CreateTriMesh(irr::scene::IMesh* mesh)
{
if(mesh==NULL)
{
std::cout << "no mesh to terrain" << std::endl;
return 0;
}
dGeomID result=0;
dTriMeshDataID trimeshId=0;
int indexCount=0;
int vertexCount=0;
dVector3* vertices=NULL;
int* indices=NULL;
irr::u16* meshBufferIndices=NULL;
irr::video::S3DVertex* meshBufferS3DVertices=NULL;
irr::video::S3DVertex2TCoords* meshBuffer2TVertices=NULL;
int ci=0; //?
int cif=0; //?
int cv=0; //?
for(int i=0;i<mesh->getMeshBufferCount();i++)
{
indexCount+=mesh->getMeshBuffer(i)->getIndexCount();
vertexCount+=mesh->getMeshBuffer(i)->getVertexCount();
}
vertices = new dVector3[vertexCount];
indices = new int[indexCount];
for(int i=0;i<mesh->getMeshBufferCount();i++)
{
meshBufferIndices = mesh->getMeshBuffer(i)->getIndices();
for(int j=0;j<mesh->getMeshBuffer(i)->getIndexCount();j++)
{
indices[ci]=cif+meshBufferIndices[j];
ci++;
}
cif=cif+mesh->getMeshBuffer(i)->getVertexCount();
switch(mesh->getMeshBuffer(i)->getVertexType())
{
case irr::video::EVT_STANDARD:
meshBufferS3DVertices=(irr::video::S3DVertex*)mesh->getMeshBuffer(i)->getVertices();
for(int j=0;j<mesh->getMeshBuffer(i)->getVertexCount();j++)
{
vertices[cv][0]=meshBufferS3DVertices[j].Pos.X;
vertices[cv][1]=meshBufferS3DVertices[j].Pos.Y;
vertices[cv][2]=meshBufferS3DVertices[j].Pos.Z;
cv++;
}
break;
case irr::video::EVT_2TCOORDS:
meshBuffer2TVertices=(irr::video::S3DVertex2TCoords*)mesh->getMeshBuffer(i)->getVertices();
for(int j=0;j<mesh->getMeshBuffer(i)->getVertexCount();j++)
{
vertices[cv][0]=meshBuffer2TVertices[j].Pos.X;
vertices[cv][1]=meshBuffer2TVertices[j].Pos.Y;
vertices[cv][2]=meshBuffer2TVertices[j].Pos.Z;
cv++;
}
break;
default:
return 0;
break;
}
}
trimeshId=dGeomTriMeshDataCreate();
if(trimeshId>0)
{
dGeomTriMeshDataBuildSimple(
trimeshId,
(dReal*)vertices,
vertexCount,
(dTriIndex*)indices,
indexCount);
result=dCreateTriMesh(this->mSpaceId,trimeshId,0,0,0);
dGeomSetBody(result,0);
}
else
{
result=0;
}
return result;
}
void PhysicsManager::ApplyForce(EntitySceneNode* node, irr::core::vector3df vector)
{
if(node!=NULL && node->GetBodyId()>0)
{
dBodyAddRelForce(
node->GetBodyId(),
(dReal)vector.X,
(dReal)vector.Y,
(dReal)vector.Z);
}
}
void PhysicsManager::ApplyForce(EntitySceneNode* node, f32 x, f32 y, f32 z)
{
if(node!=NULL && node->GetBodyId()>0)
{
dBodyAddRelForce(
node->GetBodyId(),
(dReal)x,
(dReal)y,
(dReal)z);
}
}
//Converts quaternions to euler angles.
void PhysicsManager::QuaternionToEuler(const dQuaternion quaternion, vector3df &euler)
{
dReal w,x,y,z;
w=quaternion[0];
x=quaternion[1];
y=quaternion[2];
z=quaternion[3];
double sqw = w*w;
double sqx = x*x;
double sqy = y*y;
double sqz = z*z;
//euler.Z = (irr::f32) (atan2(2.0 * (x*y + z*w),(sqx - sqy - sqz + sqw)) *irr::core::PI);
euler.Z = (irr::f32) (atan2(2.0 * (x*y + z*w),(sqx - sqy - sqz + sqw)) * GRAD_PI);
//euler.X = (irr::f32) (atan2(2.0 * (y*z + x*w),(-sqx - sqy + sqz + sqw)) *irr::core::PI);
euler.X = (irr::f32) (atan2(2.0 * (y*z + x*w),(-sqx - sqy + sqz + sqw)) * GRAD_PI);
//euler.Y = (irr::f32) (asin(-2.0 * (x*z - y*w)) *irr::core::PI);
euler.Y = (irr::f32) (asin(-2.0 * (x*z - y*w)) * GRAD_PI);
}
//Converts euler angles to quaternions.
void PhysicsManager::EulerToQuaternion (const vector3df v, dQuaternion q)
{
float heading=v.Z*GRAD_PI2/2.0f;
float attitude=v.Y*GRAD_PI2/2.0f;
float bank=v.X*GRAD_PI2/2.0f;
float c1=cos(heading);
float s1=sin(heading);
float c2=cos(attitude);
float s2=sin(attitude);
float c3=cos(bank);
float s3=sin(bank);
q[0]=(dReal)(c1*c2*c3+s1*s2*s3); //w
q[1]=(dReal)(c1*c2*s3-s1*s2*c3); //x
q[2]=(dReal)(c1*s2*c3+s1*c2*s3); //y
q[3]=(dReal)(s1*c2*c3-c1*s2*s3); //z
}
//Callback for collision.
void CollisionCallback(void* me, dGeomID o1, dGeomID o2)
{
std::cout << "I am here!" << std::endl;
int i=0;
int numberOfCollisions=0;
dBodyID body1=0;
dBodyID body2=0;
dJointID contactJoint=0;
dContact contact[MAX_CONTACTS];
PhysicsManager* physicsManager=NULL;
irr::scene::ISceneNode* node1=NULL;
irr::scene::ISceneNode* node2=NULL;
//get bodies from geometry.
body1=dGeomGetBody(o1);
body2=dGeomGetBody(o2);
node1=(irr::scene::ISceneNode*)dGeomGetData(o1);
node2=(irr::scene::ISceneNode*)dGeomGetData(o2);
physicsManager=(PhysicsManager*)me;
//if both geometry objects are spaces then we need to check for space collision.
if(dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
dSpaceCollide2(o1,o2,me,&Enigma::CollisionCallback);
if(dGeomIsSpace(o1))
{
dSpaceCollide((dxSpace*)o1,me,&Enigma::CollisionCallback);
}
if(dGeomIsSpace(o2))
{
dSpaceCollide((dxSpace*)o2,me,&Enigma::CollisionCallback);
}
}
//bodies are connected so we don't need to do anything.
if(body1 && body2 && dAreConnectedExcluding(body1,body2,dJointTypeContact))
{
return;
}
//if there is no body how did this happen. bail out!
if (!body1 && !body2)
{
return;
}
//Prepare contact structures.
for(i=0;i<MAX_CONTACTS;i++)
{
contact[i].surface.mode=dContactSoftCFM; //dContactBounce | dContactSoftCFM;
//Coulomb friction coefficient.
contact[i].surface.mu=100; //friction dInfinity
//
contact[i].surface.mu2=0;
//How bouncy the surface is.
contact[i].surface.bounce=0; //1e-5f
//The minimum velocity a body must have before it bounces.
contact[i].surface.bounce_vel=9.81; //1e-9f;
//The constraint force mixing marameter of the contact normal.
contact[i].surface.soft_cfm=0.01; //1e-6f;
//The coefficient for the force-dependent slip.
//contact[i].surface.slip1=0;
}
//Get the number of collisions and loop through all creating contact joints along the way.
numberOfCollisions=dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,sizeof(dContact));
if(numberOfCollisions>0)
{
for(i=0;i<numberOfCollisions;i++)
{
if(physicsManager!=NULL)
{
contactJoint=dJointCreateContact(
physicsManager->GetWorldId(),
physicsManager->GetJointGroupId(),
&contact[i]);
std::cout << "created contact joint." << std::endl;
}
else
{
std::cout << "physics manager null, unable to create contact joints." << std::endl;
}
dJointAttach(
contactJoint,
body1,
body2);
}
}
else
{
std::cout << "no collisions." << std::endl;
}
}
};