void drawPhysicsBodyLinearVelocity(const PhysicsBodyUnrecPtr body, Real32 Length)
{
if(body->getEnable())
{
Pnt3f origin(0.0f,0.0f,0.0f);
Vec3f vel;
body->getRelPointVel(Vec3f(0.0f,0.0f,0.0f),vel);
vel.normalize();
vel *= Length;
//Transform by the bodies position and rotation
Matrix m(body->getTransformation());
m.mult(origin,origin);
Pnt3f p2(origin + vel);
glBegin(GL_LINES);
//Velocity Direction
glColor3f(1.0,0.0,1.0);
glVertex3fv(origin.getValues());
glVertex3fv(p2.getValues());
glEnd();
}
}
OSG_BEGIN_NAMESPACE
void drawPhysicsBodyCoordinateSystem(const PhysicsBodyUnrecPtr body, Real32 Length)
{
Pnt3f origin(0.0f,0.0f,0.0f);
Pnt3f x_axis(Length,0.0f,0.0f),
y_axis(0.0f,Length,0.0f),
z_axis(0.0f,0.0f,Length);
//Transform by the bodies position and rotation
Matrix m(body->getTransformation());
m.mult(origin,origin);
m.mult(x_axis,x_axis);
m.mult(y_axis,y_axis);
m.mult(z_axis,z_axis);
glBegin(GL_LINES);
//X Axis
glColor3f(1.0,0.0,0.0);
glVertex3fv(origin.getValues());
glVertex3fv(x_axis.getValues());
//Y Axis
glColor3f(0.0,1.0,0.0);
glVertex3fv(origin.getValues());
glVertex3fv(y_axis.getValues());
//Z Axis
glColor3f(0.0,0.0,1.0);
glVertex3fv(origin.getValues());
glVertex3fv(z_axis.getValues());
glEnd();
}
OSG_BEGIN_NAMESPACE
// Documentation for this class is emitted in the
// OSGPhysicsHandlerBase.cpp file.
// To modify it, please change the .fcd file (OSGPhysicsHandler.fcd) and
// regenerate the base file.
//////////////////////////////////////////////////////////////////////////
//! this action traverses the graph to match the OpenSG representation
//! to ODE
//////////////////////////////////////////////////////////////////////////
Action::ResultE updateOsgOde(Node* const node)
{
//SLOG << "entering " << node << endLog;
TransformUnrecPtr t = dynamic_cast<Transform*>(node->getCore());
if(t!=NULL)
{
//SLOG << "found a TransformNode " << endLog;
AttachmentUnrecPtr a = node->findAttachment(PhysicsBody::getClassType());
if(a!=NULL)
{
Matrix m;
PhysicsBodyUnrecPtr body = dynamic_pointer_cast<PhysicsBody>(a);
body->updateToODEState();
//update the position and rotation, but keep the scaling
Vec3f Translation, Scale;
Quaternion Rotation, ScaleOrientation;
t->getMatrix().getTransform(Translation,Rotation,Scale,ScaleOrientation);
Translation = body->getPosition();
Rotation = body->getQuaternion();
m.setTransform(Translation,Rotation,Scale,ScaleOrientation);
t->setMatrix(m);
//update BB
node->updateVolume();
}
}
return Action::Continue;
}
PhysicsBodyUnrecPtr buildBox(const Pnt3f& Position,
const Vec3f& Dimensions,
const Color3f& Color,
Node* const spaceGroupNode,
PhysicsWorld* const physicsWorld,
PhysicsHashSpace* const physicsSpace)
{
Matrix m;
//create OpenSG mesh
GeometryUnrecPtr box;
NodeUnrecPtr boxNode = makeBox(Dimensions.x(), Dimensions.y(), Dimensions.z(), 1, 1, 1);
box = dynamic_cast<Geometry*>(boxNode->getCore());
SimpleMaterialUnrecPtr box_mat = SimpleMaterial::create();
box_mat->setAmbient(Color3f(0.0,0.0,0.0));
box_mat->setDiffuse(Color);
box->setMaterial(box_mat);
TransformUnrecPtr boxTrans;
NodeUnrecPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
m.setTranslate(Position);
boxTrans->setMatrix(m);
//create ODE data
PhysicsBodyUnrecPtr boxBody = PhysicsBody::create(physicsWorld);
boxBody->setPosition(Vec3f(Position));
boxBody->setBoxMass(1.0,Dimensions.x(), Dimensions.y(), Dimensions.z());
boxBody->setLinearDamping(0.0001);
boxBody->setAngularDamping(0.0001);
PhysicsBoxGeomUnrecPtr boxGeom = PhysicsBoxGeom::create();
boxGeom->setBody(boxBody);
boxGeom->setSpace(physicsSpace);
boxGeom->setLengths(Dimensions);
//add attachments
boxNode->addAttachment(boxGeom);
boxTransNode->addAttachment(boxBody);
boxTransNode->addChild(boxNode);
//add to SceneGraph
spaceGroupNode->addChild(boxTransNode);
return boxBody;
}
//! create a new instance of the class
PhysicsBodyUnrecPtr PhysicsBody::create(PhysicsWorldUnrecPtr World)
{
PhysicsBodyUnrecPtr fc;
if(getClassType().getPrototype() != NULL)
{
fc = dynamic_pointer_cast<PhysicsBody>(
getClassType().getPrototype()-> shallowCopy());
}
if(fc != NULL)
{
fc->setWorld(World);
commitChanges();
fc->initDefaults();
}
return fc;
}
//////////////////////////////////////////////////////////////////////////
//! build a sphere
//////////////////////////////////////////////////////////////////////////
PhysicsBodyUnrecPtr buildSphere(void)
{
Real32 Radius((Real32)(rand()%2)*0.5+0.5);
Matrix m;
//create OpenSG mesh
GeometryUnrecPtr sphere;
NodeUnrecPtr sphereNode = makeSphere(2, Radius);
sphere = dynamic_cast<Geometry*>(sphereNode->getCore());
SimpleMaterialUnrecPtr sphere_mat = SimpleMaterial::create();
sphere_mat->setAmbient(Color3f(0.0,0.0,0.0));
sphere_mat->setDiffuse(Color3f(0.0,0.0,1.0));
sphere->setMaterial(sphere_mat);
TransformUnrecPtr sphereTrans;
NodeUnrecPtr sphereTransNode = makeCoredNode<Transform>(&sphereTrans);
m.setIdentity();
Real32 randX = (Real32)(rand()%10)-5.0;
Real32 randY = (Real32)(rand()%10)-5.0;
m.setTranslate(randX, randY, 10.0);
sphereTrans->setMatrix(m);
//create ODE data
PhysicsBodyUnrecPtr sphereBody = PhysicsBody::create(physicsWorld);
sphereBody->setPosition(Vec3f(randX, randY, 10.0));
sphereBody->setLinearDamping(0.0001);
sphereBody->setAngularDamping(0.0001);
sphereBody->setSphereMass(1.0,Radius);
PhysicsSphereGeomUnrecPtr sphereGeom = PhysicsSphereGeom::create();
sphereGeom->setBody(sphereBody);
sphereGeom->setSpace(physicsSpace);
sphereGeom->setRadius(Radius);
//add attachments
sphereNode->addAttachment(sphereGeom);
sphereTransNode->addAttachment(sphereBody);
sphereTransNode->addChild(sphereNode);
//add to SceneGraph
spaceGroupNode->addChild(sphereTransNode);
commitChanges();
return sphereBody;
}
//////////////////////////////////////////////////////////////////////////
//! build a box
//////////////////////////////////////////////////////////////////////////
PhysicsBodyUnrecPtr buildBox(void)
{
Vec3f Lengths((Real32)(rand()%2)+1.0, (Real32)(rand()%2)+1.0, (Real32)(rand()%2)+1.0);
Matrix m;
//create OpenSG mesh
GeometryUnrecPtr box;
NodeUnrecPtr boxNode = makeBox(Lengths.x(), Lengths.y(), Lengths.z(), 1, 1, 1);
box = dynamic_cast<Geometry*>(boxNode->getCore());
SimpleMaterialUnrecPtr box_mat = SimpleMaterial::create();
box_mat->setAmbient(Color3f(0.0,0.0,0.0));
box_mat->setDiffuse(Color3f(0.0,1.0 ,0.0));
box->setMaterial(box_mat);
TransformUnrecPtr boxTrans;
NodeUnrecPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
m.setIdentity();
Real32 randX = (Real32)(rand()%10)-5.0;
Real32 randY = (Real32)(rand()%10)-5.0;
m.setTranslate(randX, randY, 10.0);
boxTrans->setMatrix(m);
//create ODE data
PhysicsBodyUnrecPtr boxBody = PhysicsBody::create(physicsWorld);
boxBody->setPosition(Vec3f(randX, randY, 10.0));
boxBody->setBoxMass(1.0, Lengths.x(), Lengths.y(), Lengths.z());
PhysicsBoxGeomUnrecPtr boxGeom = PhysicsBoxGeom::create();
boxGeom->setBody(boxBody);
boxGeom->setSpace(physicsSpace);
boxGeom->setLengths(Lengths);
//add attachments
boxNode->addAttachment(boxGeom);
boxTransNode->addAttachment(boxBody);
boxTransNode->addChild(boxNode);
//add to SceneGraph
spaceGroupNode->addChild(boxTransNode);
commitChanges();
return boxBody;
}
