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