// Passes in the controller stick positions public void SetStickPositions(Microsoft.Xna.Framework.Vector2 leftStick, Microsoft.Xna.Framework.Vector2 rightStick) { // move the center dot _leftStickEllipse.Margin = new Thickness(leftStick.X * 100, leftStick.Y * -100, 0, 0); int dialIndex, labelIndex; dialIndex = labelIndex = -1; // if the user hasn't pushed the left stick very far, don't do anything if (leftStick.Length() > .8) { // Calculate the angle for the stick, then the index of the corresponding dial double lAngle = Math.Atan2(-leftStick.Y, leftStick.X) + angleOffset + Math.PI * 2 + Math.PI / KeyboardWork.DIAL_AMT; dialIndex = (int)(lAngle / (2 * Math.PI / KeyboardWork.DIAL_AMT)); dialIndex %= KeyboardWork.DIAL_AMT; // if the user hasn't pushed the right stick very far, don't push a letter if (rightStick.Length() > .8) { // Calculate the angle for the stick, then the corresponding label double rAngle = Math.Atan2(-rightStick.Y, rightStick.X) + angleOffset + Math.PI * 2 + Math.PI / KeyboardWork.KEY_AMT; labelIndex = (int)(rAngle / (2 * Math.PI / KeyboardWork.KEY_AMT)); labelIndex %= KeyboardWork.KEY_AMT; if (_prevRStickMag < .8) { char key = (_keyLabels[labelIndex + KeyboardWork.DIAL_AMT * dialIndex].Content as String)[0]; InputRobot.KeyboardRobot.TypeCharacter(key); } } } // Reset the previously selected dial if it has changed if (_prevDialIndex != dialIndex || _prevLabelIndex != labelIndex) { if (_prevDialIndex != -1) { SetDialSize(_prevDialIndex, 1.0, _prevLabelIndex, 1.0); } // Apply the dial size if we're pointing at a dial if (dialIndex != -1) { SetDialSize(dialIndex, 1.1, labelIndex, 1.3); } // store the new dial data _prevDialIndex = dialIndex; _prevLabelIndex = labelIndex; } // store the previous length of the value _prevRStickMag = rightStick.Length(); }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 u = cB + rB - cA - rA; float length = u.Length(); u.Normalize(); float C = length - MaxLength; C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection); float impulse = -_mass * C; Vector2 P = impulse * u; cA -= _invMassA * P; aA -= _invIA * MathUtils.Cross(ref rA, ref P); cB += _invMassB * P; aB += _invIB * MathUtils.Cross(ref rB, ref P); data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(length - MaxLength < Settings.LinearSlop); }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; // Cdot = v + cross(w, r) Vector2 Cdot = vA + MathUtils.Cross(wA, ref _rA); Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); Vector2 oldImpulse = _impulse; _impulse += impulse; float maxImpulse = data.step.dt * MaxForce; if (_impulse.LengthSquared() > maxImpulse * maxImpulse) { _impulse *= maxImpulse / _impulse.Length(); } impulse = _impulse - oldImpulse; vA += _invMassA * impulse; wA += _invIA * MathUtils.Cross(ref _rA, ref impulse); data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
public void Update(float timeStep, float kSpring, float influenceRadius) { if (!Active) { return; } Vector2 dir = P1.Position - P0.Position; float distance = dir.Length(); dir.Normalize(); // This is to avoid imploding simulation with really springy fluids if (distance < 0.5f * influenceRadius) { Active = false; return; } if (RestLength > influenceRadius) { Active = false; return; } //Algorithm 3 float displacement = timeStep * timeStep * kSpring * (1.0f - RestLength / influenceRadius) * (RestLength - distance) * 0.5f; dir *= displacement; P0.Position -= dir; P1.Position += dir; }
public List <Microsoft.Xna.Framework.Vector3> DoEdges(List <Microsoft.Xna.Framework.Point> Nodes) { List <Geometry.Point> pts = new List <Geometry.Point>(); foreach (Microsoft.Xna.Framework.Point p in Nodes) { pts.Add(new Geometry.Point(p.X, p.Y)); } List <Geometry.Triangle> tris = Triangulate(pts); List <Microsoft.Xna.Framework.Vector3> Edges = new List <Microsoft.Xna.Framework.Vector3>(); Microsoft.Xna.Framework.Vector2 dist = new Microsoft.Xna.Framework.Vector2(); Microsoft.Xna.Framework.Vector3 curr = new Microsoft.Xna.Framework.Vector3(); foreach (Geometry.Triangle t in tris) { dist = Nodes[t.p1].ToVector2() - Nodes[t.p2].ToVector2(); curr = new Microsoft.Xna.Framework.Vector3(t.p1, t.p2, dist.Length()); if (!Edges.Contains(curr)) { Edges.Add(curr); } dist = Nodes[t.p2].ToVector2() - Nodes[t.p3].ToVector2(); curr = new Microsoft.Xna.Framework.Vector3(t.p2, t.p3, dist.Length()); if (!Edges.Contains(curr)) { Edges.Add(curr); } dist = Nodes[t.p3].ToVector2() - Nodes[t.p1].ToVector2(); curr = new Microsoft.Xna.Framework.Vector3(t.p3, t.p1, dist.Length()); if (!Edges.Contains(curr)) { Edges.Add(curr); } } return(Edges); }
// Find the smallest correction we can apply to the first rectangle that // makes it so the rectangles are no longer colliding. public static PhysicalVector2 GetMinCorrection( Rectangle rectangle1, PhysicalVector2 position1, Rectangle rectangle2, PhysicalVector2 position2 ) { var posRect1 = new PosRectangle(position1, rectangle1); var posRect2 = new PosRectangle(position2, rectangle2); var xCorrection = new PhysicalVector2(0, 0); var yCorrection = new PhysicalVector2(0, 0); // Check how far we have to push the rectangle out on the x direction. if (position1.X <= position2.X) { xCorrection.X = posRect2.Left - posRect1.Right; } else { xCorrection.X = posRect2.Right - posRect1.Left; } // Check how far we have to push the rectangle out on the y direction. if (position1.Y <= position2.Y) { yCorrection.Y = posRect2.Top - posRect1.Bottom; } else { yCorrection.Y = posRect2.Bottom - posRect1.Top; } // See which correction pushes the rectangle out the least and use that as our correction. if (xCorrection.Length() < yCorrection.Length()) { return(xCorrection); } else { return(yCorrection); } }
/// <summary> /// tests if ray intersects AABB /// </summary> /// <param name="aabb"></param> /// <returns></returns> public static bool RayCastAABB(AABB aabb, Vector2 p1, Vector2 p2) { AABB segmentAABB = new AABB(); { Vector2.Min(ref p1, ref p2, out segmentAABB.LowerBound); Vector2.Max(ref p1, ref p2, out segmentAABB.UpperBound); } if (!AABB.TestOverlap(ref aabb, ref segmentAABB)) { return(false); } Vector2 rayDir = p2 - p1; Vector2 rayPos = p1; Vector2 norm = new Vector2(-rayDir.Y, rayDir.X); //normal to ray if (norm.Length() == 0.0f) { return(true); //if ray is just a point, return true (iff point is within aabb, as tested earlier) } norm.Normalize(); float dPos = Vector2.Dot(rayPos, norm); var verts = aabb.Vertices; float d0 = Vector2.Dot(verts[0], norm) - dPos; for (int i = 1; i < 4; i++) { float d = Vector2.Dot(verts[i], norm) - dPos; if (Math.Sign(d) != Math.Sign(d0)) { //return true if the ray splits the vertices (ie: sign of dot products with normal are not all same) return(true); } } return(false); }
/// <summary> /// Constructor for RopeJoint. /// </summary> /// <param name="bodyA">The first body</param> /// <param name="bodyB">The second body</param> /// <param name="anchorA">The anchor on the first body</param> /// <param name="anchorB">The anchor on the second body</param> /// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param> public RopeJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, bool useWorldCoordinates = false) : base(bodyA, bodyB) { JointType = JointType.Rope; if (useWorldCoordinates) { LocalAnchorA = bodyA.GetLocalPoint(anchorA); LocalAnchorB = bodyB.GetLocalPoint(anchorB); } else { LocalAnchorA = anchorA; LocalAnchorB = anchorB; } //FPE feature: Setting default MaxLength Vector2 d = WorldAnchorB - WorldAnchorA; MaxLength = d.Length(); }
/// <summary> /// Constructor for PulleyJoint. /// </summary> /// <param name="bodyA">The first body.</param> /// <param name="bodyB">The second body.</param> /// <param name="anchorA">The anchor on the first body.</param> /// <param name="anchorB">The anchor on the second body.</param> /// <param name="worldAnchorA">The world anchor for the first body.</param> /// <param name="worldAnchorB">The world anchor for the second body.</param> /// <param name="ratio">The ratio.</param> /// <param name="useWorldCoordinates">Set to true if you are using world coordinates as anchors.</param> public PulleyJoint(Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, Vector2 worldAnchorA, Vector2 worldAnchorB, float ratio, bool useWorldCoordinates = false) : base(bodyA, bodyB) { JointType = JointType.Pulley; WorldAnchorA = worldAnchorA; WorldAnchorB = worldAnchorB; if (useWorldCoordinates) { LocalAnchorA = BodyA.GetLocalPoint(anchorA); LocalAnchorB = BodyB.GetLocalPoint(anchorB); Vector2 dA = anchorA - worldAnchorA; LengthA = dA.Length(); Vector2 dB = anchorB - worldAnchorB; LengthB = dB.Length(); } else { LocalAnchorA = anchorA; LocalAnchorB = anchorB; Vector2 dA = anchorA - BodyA.GetLocalPoint(worldAnchorA); LengthA = dA.Length(); Vector2 dB = anchorB - BodyB.GetLocalPoint(worldAnchorB); LengthB = dB.Length(); } Debug.Assert(ratio != 0.0f); Debug.Assert(ratio > Settings.Epsilon); Ratio = ratio; Constant = LengthA + ratio * LengthB; _impulse = 0.0f; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); _u = cB + _rB - cA - _rA; _length = _u.Length(); float C = _length - MaxLength; if (C > 0.0f) { State = LimitState.AtUpper; } else { State = LimitState.Inactive; } if (_length > Settings.LinearSlop) { _u *= 1.0f / _length; } else { _u = Vector2.Zero; _mass = 0.0f; _impulse = 0.0f; return; } // Compute effective mass. float crA = MathUtils.Cross(ref _rA, ref _u); float crB = MathUtils.Cross(ref _rB, ref _u); float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB; _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (data.step.warmStarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(ref _rA, ref P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(ref _rB, ref P); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public float Length() { return(_vector.Length()); }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // Get the pulley axes. Vector2 uA = cA + rA - WorldAnchorA; Vector2 uB = cB + rB - WorldAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { uA *= 1.0f / lengthA; } else { uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { uB *= 1.0f / lengthB; } else { uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(ref rA, ref uA); float ruB = MathUtils.Cross(ref rB, ref uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; float mass = mA + Ratio * Ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = Constant - lengthA - Ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; Vector2 PA = -impulse * uA; Vector2 PB = -Ratio * impulse * uB; cA += _invMassA * PA; aA += _invIA * MathUtils.Cross(ref rA, ref PA); cB += _invMassB * PB; aB += _invIB * MathUtils.Cross(ref rB, ref PB); data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(linearError < Settings.LinearSlop); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // Get the pulley axes. _uA = cA + _rA - WorldAnchorA; _uB = cB + _rB - WorldAnchorB; float lengthA = _uA.Length(); float lengthB = _uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { _uA *= 1.0f / lengthA; } else { _uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { _uB *= 1.0f / lengthB; } else { _uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(ref _rA, ref _uA); float ruB = MathUtils.Cross(ref _rB, ref _uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; _mass = mA + Ratio * Ratio * mB; if (_mass > 0.0f) { _mass = 1.0f / _mass; } if (data.step.warmStarting) { // Scale impulses to support variable time steps. _impulse *= data.step.dtRatio; // Warm starting. Vector2 PA = -(_impulse) * _uA; Vector2 PB = (-Ratio * _impulse) * _uB; vA += _invMassA * PA; wA += _invIA * MathUtils.Cross(ref _rA, ref PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(ref _rB, ref PB); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; float angularError = 0.0f; float positionError; bool fixedRotation = (_invIA + _invIB == 0.0f); // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { float angle = aB - aA - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } aA -= _invIA * limitImpulse; aB += _invIB * limitImpulse; } // Solve point-to-point constraint. { Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Mat22 K = new Mat22(); K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y; K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y; K.ey.X = K.ex.Y; K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X; Vector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(ref rA, ref impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(ref rB, ref impulse); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); _u = cB + _rB - cA - _rA; // Handle singularity. float length = _u.Length(); if (length > Settings.LinearSlop) { _u *= 1.0f / length; } else { _u = Vector2.Zero; } float crAu = MathUtils.Cross(ref _rA, ref _u); float crBu = MathUtils.Cross(ref _rB, ref _u); float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu; // Compute the effective mass matrix. _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (Frequency > 0.0f) { float C = length - Length; // Frequency float omega = Constant.Tau * Frequency; // Damping coefficient float d = 2.0f * _mass * DampingRatio * omega; // Spring stiffness float k = _mass * omega * omega; // magic formulas float h = data.step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invMass += _gamma; _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; } else { _gamma = 0.0f; _bias = 0.0f; } if (data.step.warmStarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(ref _rA, ref P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(ref _rB, ref P); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal void Solve(ref TimeStep step, ref Vector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Vector2 c = b._sweep.C; float a = b._sweep.A; Vector2 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. // FPE: Only apply gravity if the body wants it. if (b.IgnoreGravity) { v += h * (b._invMass * b._force); } else { v += h * (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; solverData.locks = _locks; _contactSolver.Reset(ref step, ContactCount, _contacts, _positions, _velocities, _locks, _contactManager.VelocityConstraintsMultithreadThreshold, _contactManager.PositionConstraintsMultithreadThreshold); _contactSolver.InitializeVelocityConstraints(); if (step.warmStarting) { _contactSolver.WarmStart(); } if (Settings.EnableDiagnostics) { _watch.Start(); } for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } if (Settings.EnableDiagnostics) { _watch.Stop(); } // Solve velocity constraints. for (int i = 0; i < step.velocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } joint.SolveVelocityConstraints(ref solverData); joint.Validate(step.inv_dt); if (Settings.EnableDiagnostics) { _watch.Stop(); } } _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.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 < step.positionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { Joint joint = _joints[j]; if (!joint.Enabled) { continue; } if (Settings.EnableDiagnostics) { _watch.Start(); } bool jointOkay = joint.SolvePositionConstraints(ref solverData); if (Settings.EnableDiagnostics) { _watch.Stop(); } jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } if (Settings.EnableDiagnostics) { JointUpdateTime = TimeSpan.FromTicks(_watch.ElapsedTicks); _watch.Reset(); } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body 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 (Settings.AllowSleep) { float minSleepTime = Settings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += h; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; _positions[i].c = b._sweep.C; _positions[i].a = b._sweep.A; _velocities[i].v = b._linearVelocity; _velocities[i].w = b._angularVelocity; } _contactSolver.Reset(ref subStep, ContactCount, _contacts, _positions, _velocities, _locks, _contactManager.VelocityConstraintsMultithreadThreshold, _contactManager.PositionConstraintsMultithreadThreshold); // Solve position constraints. for (int i = 0; i < subStep.positionIterations; ++i) { bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // Leap of faith to new safe state. Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c; Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a; Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c; Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. _contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < subStep.velocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. float h = subStep.dt; // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Vector2 c = _positions[i].c; float a = _positions[i].a; Vector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities Vector2 translation = h * v; if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared) { float ratio = Settings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.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; // Sync bodies Body body = Bodies[i]; body._sweep.C = c; body._sweep.A = a; body._linearVelocity = v; body._angularVelocity = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); float positionError, angularError; Mat33 K = new Mat33(); K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; K.ez.X = -rA.Y * iA - rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.ez.Y = rA.X * iA + rB.X * iB; K.ex.Z = K.ez.X; K.ey.Z = K.ez.Y; K.ez.Z = iA + iB; if (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse; if (K.ez.Z <= 0.0f) { Vector2 impulse2 = -K.Solve22(C1); impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); } else { impulse = -K.Solve33(C); } Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(ref rB, ref P) + impulse.Z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }