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