//public static FixedFrictionJoint CreateFixedFrictionJoint(World world, Body body, Vector2 bodyAnchor) //{ // FixedFrictionJoint frictionJoint = new FixedFrictionJoint(body, bodyAnchor); // world.AddJoint(frictionJoint); // return frictionJoint; //} #endregion #region Gear Joint public static GearJoint CreateGearJoint(World world, FarseerJoint jointA, FarseerJoint jointB, float ratio) { GearJoint gearJoint = new GearJoint(jointA, jointB, ratio); world.AddJoint(gearJoint); return(gearJoint); }
public void Solve(ref FSTimeStep step, ref FVector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; FVector2 c = b.Sweep.C; float a = b.Sweep.A; FVector2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.C0 = b.Sweep.C; b.Sweep.A0 = b.Sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. v += h * (b.GravityScale * gravity + b.InvMass * b.Force); w += h * b.InvI * b.Torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; // Initialize velocity constraints. //b2ContactSolverDef contactSolverDef; //contactSolverDef.step = step; //contactSolverDef.contacts = m_contacts; //contactSolverDef.count = m_contactCount; //contactSolverDef.positions = m_positions; //contactSolverDef.velocities = m_velocities; //contactSolverDef.allocator = m_allocator; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (FSSettings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < FSSettings.VelocityIterations; ++i) { #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) { continue; } joint.SolveVelocityConstraints(ref solverData); //TODO: Move up before solve? joint.Validate(step.inv_dt); } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { FVector2 c = _positions[i].c; float a = _positions[i].a; FVector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities FVector2 translation = h * v; if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared) { float ratio = FSSettings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > FSSettings.MaxRotationSquared) { float ratio = FSSettings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solve position constraints bool positionSolved = false; for (int i = 0; i < FSSettings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) { continue; } bool jointOkay = joint.SolvePositionConstraints(ref solverData); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { FSBody body = Bodies[i]; body.Sweep.C = _positions[i].c; body.Sweep.A = _positions[i].a; body.LinearVelocity = _velocities[i].v; body.AngularVelocity = _velocities[i].w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (FSSettings.AllowSleep) { float minSleepTime = FSSettings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || FVector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= FSSettings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; b.Awake = false; } } } }
public void Add(FarseerJoint joint) { Debug.Assert(JointCount < JointCapacity); _joints[JointCount++] = joint; }
private void DrawJoint(FarseerJoint joint) { if (!joint.Enabled) { return; } FSBody b1 = joint.BodyA; FSBody b2 = joint.BodyB; Transform xf1, xf2; b1.GetTransform(out xf1); FVector2 x2 = FVector2.Zero; // WIP David if (!joint.IsFixedType()) { b2.GetTransform(out xf2); x2 = xf2.p; } FVector2 p1 = joint.WorldAnchorA; FVector2 p2 = joint.WorldAnchorB; FVector2 x1 = xf1.p; Color color = new Color(0.5f, 0.8f, 0.8f); switch (joint.JointType) { case JointType.Distance: DrawSegment(p1, p2, color); break; case JointType.Pulley: FSPulleyJoint pulley = (FSPulleyJoint)joint; FVector2 s1 = pulley.GroundAnchorA; FVector2 s2 = pulley.GroundAnchorB; DrawSegment(s1, p1, color); DrawSegment(s2, p2, color); DrawSegment(s1, s2, color); break; case JointType.FixedMouse: DrawPoint(p1, 0.5f, new Color(0.0f, 1.0f, 0.0f)); DrawSegment(p1, p2, new Color(0.8f, 0.8f, 0.8f)); break; case JointType.Revolute: //DrawSegment(x2, p1, color); DrawSegment(p2, p1, color); DrawSolidCircle(p2, 0.1f, FVector2.Zero, Color.red); DrawSolidCircle(p1, 0.1f, FVector2.Zero, Color.blue); break; case JointType.FixedAngle: //Should not draw anything. break; case JointType.FixedRevolute: DrawSegment(x1, p1, color); DrawSolidCircle(p1, 0.1f, FVector2.Zero, new Color(1f, 0.5f, 1f)); break; case JointType.FixedLine: DrawSegment(x1, p1, color); DrawSegment(p1, p2, color); break; case JointType.FixedDistance: DrawSegment(x1, p1, color); DrawSegment(p1, p2, color); break; case JointType.FixedPrismatic: DrawSegment(x1, p1, color); DrawSegment(p1, p2, color); break; case JointType.Gear: DrawSegment(x1, x2, color); break; //case JointType.Weld: // break; default: DrawSegment(x1, p1, color); DrawSegment(p1, p2, color); DrawSegment(x2, p2, color); break; } }