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(); }
public override bool IsStability() { if( jointID == dJointID.Zero ) return true; Ode.dVector3 a1 = new Ode.dVector3(); Ode.dVector3 a2 = new Ode.dVector3(); Ode.dJointGetHingeAnchor( jointID, ref a1 ); Ode.dJointGetHingeAnchor2( 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 odeAxisDirection = new Ode.dVector3(); Ode.dJointGetHingeAnchor( jointID, ref odeAnchor ); Ode.dJointGetHingeAxis( jointID, ref odeAxisDirection ); Vec3 anchor = Convert.ToNet( odeAnchor ); Vec3 axisDirection = Convert.ToNet( odeAxisDirection ); UpdateDataFromLibrary( ref anchor, 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 ); } }