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();
}
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();
}
}