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);
}
internal void UpdateDataFromLibrary()
{
if (jointID == dJointID.Zero)
{
return;
}
Ode.dVector3 odeAxisDirection = new Ode.dVector3();
Ode.dJointGetSliderAxis(jointID, ref odeAxisDirection);
Vec3 axisDirection = Convert.ToNet(odeAxisDirection);
UpdateDataFromLibrary(ref axisDirection);
}
internal void UpdateDataFromLibrary()
{
if (jointID == dJointID.Zero)
{
return;
}
Ode.dVector3 odeAnchor = new Ode.dVector3();
Ode.dJointGetUniversalAnchor(jointID, ref odeAnchor);
Vec3 anchor = Convert.ToNet(odeAnchor);
Vec3 axis1Direction = Body1.Rotation * axis1LocalAxis;
Vec3 axis2Direction = Body1.Rotation * axis2LocalAxis;
UpdateDataFromLibrary(ref anchor, ref axis1Direction, ref axis2Direction);
}
internal void DoDamping()
{
if (bodyID == dBodyID.Zero)
{
return;
}
Ode.dMass mass = new Ode.dMass();
Ode.dBodyGetMass(bodyID, ref mass);
// Linear damping
if (LinearDamping != 0)
{
// The damping force depends on the damping amount, mass, and velocity
// (i.e. damping amount and momentum).
float factor = -LinearDamping * mass.mass;
Vec3 force = LinearVelocity * factor;
// Add a global force opposite to the global linear velocity.
Ode.dBodyAddForce(bodyID, force.X, force.Y, force.Z);
}
// Angular damping
if (AngularDamping != 0)
{
Vec3 localVelocity;
{
Ode.dVector3 aVelLocal = new Ode.dVector3();
Ode.dBodyVectorFromWorld(bodyID, AngularVelocity.X, AngularVelocity.Y,
AngularVelocity.Z, ref aVelLocal);
localVelocity = Convert.ToNet(aVelLocal);
}
// The damping force depends on the damping amount, mass, and velocity
// (i.e. damping amount and momentum).
float factor = -AngularDamping;
Vec3 momentum = new Vec3(
Vec3.Dot(new Vec3(mass.I.M00, mass.I.M01, mass.I.M02), localVelocity),
Vec3.Dot(new Vec3(mass.I.M10, mass.I.M11, mass.I.M12), localVelocity),
Vec3.Dot(new Vec3(mass.I.M20, mass.I.M21, mass.I.M22), localVelocity));
Vec3 torque = momentum * factor;
// Add a local torque opposite to the local angular velocity.
Ode.dBodyAddRelTorque(bodyID, torque.X, torque.Y, torque.Z);
}
}
public override bool IsStability()
{
if (jointID == dJointID.Zero)
{
return(true);
}
Ode.dVector3 a1 = new Ode.dVector3();
Ode.dVector3 a2 = new Ode.dVector3();
Ode.dJointGetUniversalAnchor(jointID, ref a1);
Ode.dJointGetUniversalAnchor2(jointID, ref a2);
const float limit = .01f; // .001f;
return(Math.Abs(a1.X - a2.X) < limit &&
Math.Abs(a1.Y - a2.Y) < limit &&
Math.Abs(a1.Z - a2.Z) < limit);
}
internal void UpdateDataFromLibrary()
{
if (jointID == dJointID.Zero)
{
return;
}
Ode.dVector3 odeAnchor = new Ode.dVector3();
Ode.dVector3 odeAxis1Direction = new Ode.dVector3();
Ode.dVector3 odeAxis2Direction = new Ode.dVector3();
Ode.dJointGetHinge2Anchor(jointID, ref odeAnchor);
Ode.dJointGetHinge2Axis1(jointID, ref odeAxis1Direction);
Ode.dJointGetHinge2Axis2(jointID, ref odeAxis2Direction);
Vec3 anchor = Convert.ToNet(odeAnchor);
Vec3 axis1Direction = Convert.ToNet(odeAxis1Direction);
Vec3 axis2Direction = Convert.ToNet(odeAxis2Direction);
UpdateDataFromLibrary(ref anchor, ref axis1Direction, ref axis2Direction);
}
unsafe internal static void CalculateJointStress(dJointID jointID,
out float force, out float torque)
{
IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr(jointID);
if (jointFeedback == IntPtr.Zero)
{
//create jointFeedback and begin use on next simulation step
jointFeedback = Ode.dAlloc((uint)Marshal.SizeOf(typeof(Ode.dJointFeedback)));
//zero memory
unsafe
{
Ode.dJointFeedback *p = (Ode.dJointFeedback *)jointFeedback;
p->f1 = new Ode.dVector3();
p->t1 = new Ode.dVector3();
p->f2 = new Ode.dVector3();
p->t2 = new Ode.dVector3();
}
Ode.dJointSetFeedbackAsIntPtr(jointID, jointFeedback);
force = 0;
torque = 0;
return;
}
Ode.dJointFeedback *ptr = (Ode.dJointFeedback *)jointFeedback;
Vec3 f1 = Convert.ToNet(ptr->f1);
Vec3 t1 = Convert.ToNet(ptr->t1);
Vec3 f2 = Convert.ToNet(ptr->f2);
Vec3 t2 = Convert.ToNet(ptr->t2);
// This is a simplification, but it should still work.
force = (f1 - f2).Length();
torque = (t1 - t2).Length();
}
/// <summary>
/// Application's entry point.
/// </summary>
public static void Main()
{
// Create a world
IntPtr world = Ode.dWorldCreate();
// Add gravity to the world (pull down on the Y axis 9.81 meters/second
Ode.dWorldSetGravity(world, 0, -9.81f, 0);
// Create a rigid body (in the world)
IntPtr body = Ode.dBodyCreate(world);
// Create some mass, we're creating a sphere with a radius of 0.05 centimeters
// and a constant density of 2500 (about that of glass)
Ode.dMass mass = new Ode.dMass();
Ode.dMassSetSphere(ref mass, 2500, 0.05f);
// If you printed the values of mass now, you'd see the mass would be about 1.3 kilograms
// We'll change that to 1 kilogram here
mass.mass = 1;
// If you printed the values of mass now, you'd notice that the inertia tensions values
// were also updated
// We'll set the body's mass to the mass we just created
Ode.dBodySetMass(body, ref mass);
// Set the body's position (in the world) to 2 meters above the 'ground'
Ode.dBodySetPosition(body, 0, 2, 0);
// Apply a force to the body, we're sending it vertical, up the Y axis
// This force is only applied for the first step, forces will be reset to zero
// at the end of each step
Ode.dBodyAddForce(body, 0, 200, 0);
// The simulation loop's 'time', in seconds
float time = 0;
// The 'time' increment, in seconds
float deltaTime = 0.04f;
// Run the simulation loop for 2 seconds
while (time < 2)
{
// Get the body's current position
Ode.dVector3 position = Ode.dBodyGetPosition(body);
// Get the body's current linear velocity
Ode.dVector3 velocity = Ode.dBodyGetLinearVel(body);
// Print out the 'time', the body's position, and its velocity
Console.WriteLine("{0:0.00} sec: pos=({1:0.00}, {2:0.00}, {3:0.00}) vel={4:0.00}, {5:0.00}, {6:0.00})",
time, position[0], position[1], position[2], velocity[0], velocity[1], velocity[2]);
// Move the bodies in the world
Ode.dWorldStep(world, deltaTime);
// Increment the time
time += deltaTime;
}
Console.WriteLine();
Console.WriteLine("Press Enter to exit...");
Console.ReadLine();
}
public static void ToNet(ref Ode.dVector3 value, out Vec3 result)
{
result = new Vec3(value.X, value.Y, value.Z);
}
public static Vec3 ToNet(Ode.dVector3 value)
{
return(new Vec3(value.X, value.Y, value.Z));
}
