void UpdateODEJoint() { bool needCreate = PushedToWorld && !Broken; bool created = jointID != dJointID.Zero; if( needCreate == created ) return; if( needCreate ) { ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; jointID = Ode.dJointCreateFixed( ( (ODEPhysicsScene)Scene ).worldID, IntPtr.Zero ); Ode.SetJointContactsEnabled( jointID, false );//ContactsEnabled ); Ode.dJointAttach( jointID, odeBody1.bodyID, odeBody2.bodyID ); Ode.dJointSetFixed( jointID ); Ode.BodyDataAddJoint( odeBody1.bodyData, jointID ); Ode.BodyDataAddJoint( odeBody2.bodyData, jointID ); } else { DestroyODEJoint(); } }
void UpdateODEJoint() { bool needCreate = PushedToWorld && !Broken; bool created = jointID != dJointID.Zero; if (needCreate == created) { return; } if (needCreate) { ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; jointID = Ode.dJointCreateFixed(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero); Ode.SetJointContactsEnabled(jointID, false); //ContactsEnabled ); Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID); Ode.dJointSetFixed(jointID); Ode.BodyDataAddJoint(odeBody1.bodyData, jointID); Ode.BodyDataAddJoint(odeBody2.bodyData, jointID); } else { DestroyODEJoint(); } }
void UpdateODEJoint() { bool needCreate = PushedToWorld && !Broken; bool created = jointID != dJointID.Zero; if (needCreate == created) { return; } if (needCreate) { ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; jointID = Ode.dJointCreateSlider(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero); Ode.SetJointContactsEnabled(jointID, ContactsEnabled); Ode.dJointSetSliderParam(jointID, Ode.dJointParams.dParamFudgeFactor, Defines.jointFudgeFactor); Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID); axis.UpdateToLibrary(true); Ode.BodyDataAddJoint(odeBody1.bodyData, jointID); Ode.BodyDataAddJoint(odeBody2.bodyData, jointID); } else { DestroyODEJoint(); } }
void nearCallback(IntPtr data, dGeomID o1, dGeomID o2) { dBodyID b1 = ode.dGeomGetBody(o1); dBodyID b2 = ode.dGeomGetBody(o2); const int MAX_CONTACTS = 5; ode.dContact[] contact = new ode.dContact[MAX_CONTACTS]; for (int i = 0; i < MAX_CONTACTS; i++) { contact[i].surface.mode = (int)(ode.dContactType.Bounce | ode.dContactType.SoftCFM); contact[i].surface.mu = Mathf.Infinity; contact[i].surface.mu2 = 0; contact[i].surface.bounce = 0.01f; contact[i].surface.bounce_vel = 0.1f; contact[i].surface.soft_cfm = 0.01f; } int numc = ode.dCollide(o1, o2, MAX_CONTACTS, ref contact[0].geom, System.Runtime.InteropServices.Marshal.SizeOf(typeof(ode.dContact))); if (numc > 0) { for (int i = 0; i < numc; i++) { dJointID c = ode.dJointCreateContact(world, contactgroup, ref contact[i]); ode.dJointAttach(c, b1, b2); } } }
public Joint(World world, dJointID ptr, OdeJoint.JointType type) { this.world = world; this.type = type; this.ptr = ptr; id = InstanceMap.Add(this); ode.dJointSetData(ptr, (IntPtr)id); }
void UpdateODEJoint() { bool needCreate = PushedToWorld && !Broken; bool created = jointID != dJointID.Zero; if (needCreate == created) { return; } if (needCreate) { if (Math.Abs(Vec3.Dot(axis1.Direction, axis2.Direction)) > .095f) { Log.Warning("UniversalJoint: Invalid axes."); return; } if (Axis1.LimitsEnabled && Axis1.LimitLow > Axis1.LimitHigh) { Log.Warning("UniversalJoint: Invalid axis1 limits (low > high)."); return; } if (Axis2.LimitsEnabled && Axis2.LimitLow > Axis2.LimitHigh) { Log.Warning("UniversalJoint: Invalid axis2 limits (low > high)."); return; } ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; jointID = Ode.dJointCreateUniversal(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero); Ode.SetJointContactsEnabled(jointID, ContactsEnabled); Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor, Defines.jointFudgeFactor); Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor2, Defines.jointFudgeFactor); Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID); Ode.dJointSetUniversalAnchor(jointID, Anchor.X, Anchor.Y, Anchor.Z); axis1.UpdateToLibrary(true); axis2.UpdateToLibrary(true); axis1LocalAxis = Body1.Rotation.GetInverse() * axis1.Direction; axis2LocalAxis = Body1.Rotation.GetInverse() * axis2.Direction; Ode.BodyDataAddJoint(odeBody1.bodyData, jointID); Ode.BodyDataAddJoint(odeBody2.bodyData, jointID); } else { DestroyODEJoint(); } }
internal static void FreeJointFeedback( dJointID jointID ) { IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr( jointID ); if( jointFeedback != IntPtr.Zero ) { Ode.dFree( jointFeedback, (uint)Marshal.SizeOf( typeof( Ode.dJointFeedback ) ) ); jointFeedback = IntPtr.Zero; } }
internal static void FreeJointFeedback(dJointID jointID) { IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr(jointID); if (jointFeedback != IntPtr.Zero) { Ode.dFree(jointFeedback, (uint)Marshal.SizeOf(typeof(Ode.dJointFeedback))); jointFeedback = IntPtr.Zero; } }
internal Joint(dJointID joint, World world, JointGroup group) { id = joint; handle = GCHandle.Alloc(this); NativeMethods.dJointSetData(id, GCHandle.ToIntPtr(handle)); id.Owner = world; if (group != null) { group.Add(this); } }
internal void DestroyODEJoint() { if (jointID != dJointID.Zero) { ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; Ode.BodyDataRemoveJoint(odeBody1.bodyData, jointID); Ode.BodyDataRemoveJoint(odeBody2.bodyData, jointID); Utils.FreeJointFeedback(jointID); Ode.dJointDestroy(jointID); jointID = dJointID.Zero; } }
internal void DestroyODEJoint() { if( jointID != dJointID.Zero ) { ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; Ode.BodyDataRemoveJoint( odeBody1.bodyData, jointID ); Ode.BodyDataRemoveJoint( odeBody2.bodyData, jointID ); Utils.FreeJointFeedback( jointID ); Ode.dJointDestroy( jointID ); jointID = dJointID.Zero; } }
internal static bool UpdateJointBreakState( Joint joint, dJointID jointID ) { if( jointID == dJointID.Zero ) return false; float force; float torque; CalculateJointStress( jointID, out force, out torque ); if( joint.BreakMaxForce != 0 && force >= joint.BreakMaxForce ) return true; if( joint.BreakMaxTorque != 0 && torque >= joint.BreakMaxTorque ) return true; return false; }
void UpdateODEJoint() { bool needCreate = PushedToWorld && !Broken; bool created = jointID != dJointID.Zero; if (needCreate == created) { return; } if (needCreate) { if (Axis.LimitsEnabled && Axis.LimitLow > Axis.LimitHigh) { Log.Warning("HingeJoint: Invalid axis limits (low > high)."); return; } ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; jointID = Ode.dJointCreateHinge(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero); Ode.SetJointContactsEnabled(jointID, ContactsEnabled); Ode.dJointSetHingeParam(jointID, Ode.dJointParams.dParamFudgeFactor, Defines.jointFudgeFactor); Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID); Ode.dJointSetHingeAnchor(jointID, Anchor.X, Anchor.Y, Anchor.Z); axis.UpdateToLibrary(true); Ode.BodyDataAddJoint(odeBody1.bodyData, jointID); Ode.BodyDataAddJoint(odeBody2.bodyData, jointID); } else { DestroyODEJoint(); } }
internal static bool UpdateJointBreakState(Joint joint, dJointID jointID) { if (jointID == dJointID.Zero) { return(false); } float force; float torque; CalculateJointStress(jointID, out force, out torque); if (joint.BreakMaxForce != 0 && force >= joint.BreakMaxForce) { return(true); } if (joint.BreakMaxTorque != 0 && torque >= joint.BreakMaxTorque) { return(true); } return(false); }
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(); }
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 extern static int dJointGetAMotorAxisRel( dJointID joint, int anum );
public extern static void dJointSetAMotorAxis( dJointID joint, int anum, int rel, dReal x, dReal y, dReal z );
public extern static void dJointSetAMotorNumAxes( dJointID joint, int num );
public extern static void dJointSetAMotorMode( dJointID joint, int mode );
public extern static dReal dJointGetUniversalAngle2Rate( dJointID joint );
public extern static dReal dJointGetUniversalParam( dJointID joint, dJointParams parameter );
public extern static void BodyDataRemoveJoint( IntPtr bodyData, dJointID jointID );
internal override dReal GetParam(dJointID id, dJointParam parameter) { return(NativeMethods.dJointGetPistonParam(id, parameter)); }
internal abstract dReal GetParam(dJointID id, dJointParam parameter);
internal abstract void SetParam(dJointID id, dJointParam parameter, dReal value);
public UniversalJoint(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.Universal) { }
public AMotor(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.AMotor) { }
public BallJoint(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.Ball) { }
public extern static dReal dJointGetAMotorAngleRate( dJointID joint, int anum );
public extern static dReal dJointGetAMotorParam( dJointID joint, dJointParams parameter );
internal override void SetParam(dJointID id, dJointParam parameter, dReal value) { NativeMethods.dJointSetPistonParam(id, parameter, value); }
public HingeJoint(World world, dJointID ptr) : base(world, ptr, OdeJoint.JointType.Hinge) { }
internal JointLimitMotor(Joint joint, int axis) { id = joint.id; index = axis; }
public extern static void dJointAddUniversalTorques( dJointID joint, dReal torque1, dReal torque2 );
void UpdateODEJoint() { bool needCreate = PushedToWorld && !Broken; bool created = jointID != dJointID.Zero; ODEBody odeBody1 = (ODEBody)Body1; ODEBody odeBody2 = (ODEBody)Body2; if (needCreate && (odeBody1.bodyID == dBodyID.Zero || odeBody2.bodyID == dBodyID.Zero)) { Log.Warning("ODEHinge2Joint: It is necessary that both bodies were not static."); needCreate = false; } if (needCreate == created) { return; } if (needCreate) { if (Math.Abs(Vec3.Dot(axis1.Direction, axis2.Direction)) > .095f) { Log.Warning("Hinge2Joint: Invalid axes."); return; } if (Axis1.LimitsEnabled && Axis1.LimitLow > Axis1.LimitHigh) { Log.Warning("Hinge2Joint: Invalid axis1 limits (low > high)."); return; } if (Axis2.LimitsEnabled && Axis2.LimitLow > Axis2.LimitHigh) { Log.Warning("Hinge2Joint: Invalid axis2 limits (low > high)."); return; } jointID = Ode.dJointCreateHinge2(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero); Ode.SetJointContactsEnabled(jointID, ContactsEnabled); Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamFudgeFactor, Defines.jointFudgeFactor); Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamFudgeFactor2, Defines.jointFudgeFactor); Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID); Ode.dJointSetHinge2Anchor(jointID, Anchor.X, Anchor.Y, Anchor.Z); axis1.UpdateToLibrary(true); axis2.UpdateToLibrary(true); UpdateSuspension(); Ode.BodyDataAddJoint(odeBody1.bodyData, jointID); Ode.BodyDataAddJoint(odeBody2.bodyData, jointID); } else { DestroyODEJoint(); } }
public extern static void dJointSetFixed( dJointID joint );
public extern static void dJointGetHinge2Axis2( dJointID joint, ref dVector3 result );
public extern static int dJointGetAMotorMode( dJointID joint );
public extern static dReal dJointGetHinge2Angle2Rate( dJointID joint );
public extern static int dJointGetAMotorNumAxes( dJointID joint );
public extern static dReal dJointGetHinge2Param( dJointID joint, dJointParams parameter );
public extern static void dJointGetAMotorAxis( dJointID joint, int anum, ref dVector3 result );
public extern static void dJointAddHinge2Torques( dJointID joint, dReal torque1, dReal torque2 );
public extern static void dJointSetAMotorAngle( dJointID joint, int anum, dReal angle );
public extern static void dJointSetUniversalAxis2( dJointID joint, dReal x, dReal y, dReal z );
public extern static void dJointSetAMotorParam( dJointID joint, dJointParams parameter, dReal value );
public extern static void dJointGetUniversalAxis2( dJointID joint, ref dVector3 result );
public extern static void dJointAddAMotorTorques( dJointID joint, dReal torque1, dReal torque2, dReal torque3 );
public extern static void dJointSetUniversalParam( dJointID joint, dJointParams parameter, dReal value );
public extern static void SetJointContactsEnabled( dJointID jointID, [MarshalAs( UnmanagedType.U1 )] bool contactsEnabled );
void OnNewContact(ref ode.dContact contact, dBodyID body1, dBodyID body2) { dJointID c = ode.dJointCreateContact(world.ptr, world.contactgroup, ref contact); ode.dJointAttach(c, body1, body2); }