public void update()
{
Ode.dVector3 position = Ode.dGeomGetPosition(geomID);
Ode.dQuaternion rotation = new Ode.dQuaternion();
Ode.dGeomGetQuaternion(geomID, ref rotation);
this.position.set(position.X, position.Y, position.Z);
// get the position & rotation of the sphere
// convert the quaternion to an axis angle so we can put the
// rotation into glRotatef()
float cos_a = rotation.W;
float angle = (float)(Math.Acos(cos_a) * 2.0f);
float sin_a = (float)(Math.Sqrt(1.0f - cos_a * cos_a));
if (Math.Abs(sin_a) < 0.0005f)
{
sin_a = 1.0f;
}
sin_a = 1.0f / sin_a;
angle = RAD_TO_DEG(angle);
float x_axis = rotation.X * sin_a;
float y_axis = rotation.Y * sin_a;
float z_axis = rotation.Z * sin_a;
this.rotationOGL.set(angle, x_axis, y_axis, z_axis);
}
public static void ToODE(Quat value, out Ode.dQuaternion result)
{
result.X = value.X;
result.Y = value.Y;
result.Z = value.Z;
result.W = value.W;
}
