public void GetSearchDirection(Vec2 result) { switch (Count) { case 1: result.Set(m_v1.W).NegateLocal(); return; case 2: e12.Set(m_v2.W).SubLocal(m_v1.W); // use out for a temp variable real quick result.Set(m_v1.W).NegateLocal(); float sgn = Vec2.Cross(e12, result); if (sgn > 0f) { // Origin is left of e12. Vec2.CrossToOutUnsafe(1f, e12, result); return; } else { // Origin is right of e12. Vec2.CrossToOutUnsafe(e12, 1f, result); return; } default: Debug.Assert(false); result.SetZero(); return; } }
public float MotorMass; // effective mass for motor/limit translational constraint. public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def) : base(argWorld, def) { LocalAnchorA = new Vec2(def.LocalAnchorA); LocalAnchorB = new Vec2(def.LocalAnchorB); LocalXAxisA = new Vec2(def.LocalAxisA); LocalXAxisA.Normalize(); LocalYAxisA = new Vec2(); Vec2.CrossToOutUnsafe(1f, LocalXAxisA, LocalYAxisA); ReferenceAngle = def.ReferenceAngle; Impulse = new Vec3(); MotorMass = 0.0f; MotorImpulse = 0.0f; LowerTranslation = def.LowerTranslation; UpperTranslation = def.UpperTranslation; m_maxMotorForce = def.MaxMotorForce; m_motorSpeed = def.MotorSpeed; m_limitEnabled = def.EnableLimit; m_motorEnabled = def.EnableMotor; LimitState = LimitState.Inactive; K = new Mat33(); Axis = new Vec2(); Perp = new Vec2(); }
public void WarmStart() { // Warm start. for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float iA = vc.InvIA; float mB = vc.InvMassB; float iB = vc.InvIB; int pointCount = vc.PointCount; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; Vec2 normal = vc.Normal; Vec2.CrossToOutUnsafe(normal, 1.0f, tangent); for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; //Console.WriteLine("vcp normal impulse is " + vcp.normalImpulse); temp.Set(normal).MulLocal(vcp.NormalImpulse); P.Set(tangent).MulLocal(vcp.TangentImpulse).AddLocal(temp); wA -= iA * Vec2.Cross(vcp.RA, P); vA.SubLocal(temp.Set(P).MulLocal(mA)); //Debug.Assert(vA.x == 0); //Debug.Assert(wA == 0); wB += iB * Vec2.Cross(vcp.RB, P); vB.AddLocal(temp.Set(P).MulLocal(mB)); //Debug.Assert(vB.x == 0); //Debug.Assert(wB == 0); } Velocities[indexA].W = wA; Velocities[indexB].W = wB; //Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " - " + wA); //Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " - " + wB); } }
public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Vec2 vpA = Pool.PopVec2(); Vec2 vpB = Pool.PopVec2(); // Cdot = dot(u, v + cross(w, r)) Vec2.CrossToOutUnsafe(wA, RA, vpA); vpA.AddLocal(vA); Vec2.CrossToOutUnsafe(wB, RB, vpB); vpB.AddLocal(vB); float Cdot = Vec2.Dot(U, vpB.SubLocal(vpA)); float impulse = (-Mass) * (Cdot + Bias + Gamma * Impulse); Impulse += impulse; float Px = impulse * U.X; float Py = impulse * U.Y; vA.X -= InvMassA * Px; vA.Y -= InvMassA * Py; wA -= InvIA * (RA.X * Py - RA.Y * Px); vB.X += InvMassB * Px; vB.Y += InvMassB * Py; wB += InvIB * (RB.X * Py - RB.Y * Px); data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(2); }
public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Vec2 vpA = Pool.PopVec2(); Vec2 vpB = Pool.PopVec2(); Vec2 PA = Pool.PopVec2(); Vec2 PB = Pool.PopVec2(); Vec2.CrossToOutUnsafe(wA, RA, vpA); vpA.AddLocal(vA); Vec2.CrossToOutUnsafe(wB, RB, vpB); vpB.AddLocal(vB); float Cdot = -Vec2.Dot(m_uA, vpA) - m_ratio * Vec2.Dot(m_uB, vpB); float impulse = (-m_mass) * Cdot; m_impulse += impulse; PA.Set(m_uA).MulLocal(-impulse); PB.Set(m_uB).MulLocal((-m_ratio) * impulse); vA.X += InvMassA * PA.X; vA.Y += InvMassA * PA.Y; wA += InvIA * Vec2.Cross(RA, PA); vB.X += InvMassB * PB.X; vB.Y += InvMassB * PB.Y; wB += InvIB * Vec2.Cross(RB, PB); data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(4); }
public override void SolveVelocityConstraints(SolverData data) { Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; // Cdot = v + cross(w, r) Vec2 Cdot = Pool.PopVec2(); Vec2.CrossToOutUnsafe(wB, RB, Cdot); Cdot.AddLocal(vB); Vec2 impulse = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); temp.Set(m_impulse).MulLocal(m_gamma).AddLocal(m_C).AddLocal(Cdot).NegateLocal(); Mat22.MulToOutUnsafe(m_mass, temp, impulse); Vec2 oldImpulse = temp; oldImpulse.Set(m_impulse); m_impulse.AddLocal(impulse); float maxImpulse = data.Step.Dt * m_maxForce; if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) { m_impulse.MulLocal(maxImpulse / m_impulse.Length()); } impulse.Set(m_impulse).SubLocal(oldImpulse); vB.X += InvMassB * m_impulse.X; vB.Y += InvMassB * m_impulse.Y; wB += InvIB * Vec2.Cross(RB, impulse); data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(3); }
public void InitializeVelocityConstraints() { //Console.WriteLine("Initializing velocity constraints for " + m_count + " contacts"); // Warm start. for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; ContactPositionConstraint pc = PositionConstraints[i]; float radiusA = pc.RadiusA; float radiusB = pc.RadiusB; Manifold manifold = Contacts[vc.ContactIndex].Manifold; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float mB = vc.InvMassB; float iA = vc.InvIA; float iB = vc.InvIB; Vec2 localCenterA = pc.LocalCenterA; Vec2 localCenterB = pc.LocalCenterB; Vec2 cA = Positions[indexA].C; float aA = Positions[indexA].A; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 cB = Positions[indexB].C; float aB = Positions[indexB].A; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; Debug.Assert(manifold.PointCount > 0); xfA.Q.Set(aA); xfB.Q.Set(aB); Rot.MulToOutUnsafe(xfA.Q, localCenterA, temp); xfA.P.Set(cA).SubLocal(temp); Rot.MulToOutUnsafe(xfB.Q, localCenterB, temp); xfB.P.Set(cB).SubLocal(temp); worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); vc.Normal.Set(worldManifold.Normal); int pointCount = vc.PointCount; for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; vcp.RA.Set(worldManifold.Points[j]).SubLocal(cA); vcp.RB.Set(worldManifold.Points[j]).SubLocal(cB); float rnA = Vec2.Cross(vcp.RA, vc.Normal); float rnB = Vec2.Cross(vcp.RB, vc.Normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; Vec2.CrossToOutUnsafe(vc.Normal, 1.0f, tangent); float rtA = Vec2.Cross(vcp.RA, tangent); float rtB = Vec2.Cross(vcp.RB, tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp.VelocityBias = 0.0f; Vec2.CrossToOutUnsafe(wB, vcp.RB, temp1); Vec2.CrossToOutUnsafe(wA, vcp.RA, temp2); temp.Set(vB).AddLocal(temp1).SubLocal(vA).SubLocal(temp2); float vRel = Vec2.Dot(vc.Normal, temp); if (vRel < -Settings.VELOCITY_THRESHOLD) { vcp.VelocityBias = (-vc.Restitution) * vRel; } } // If we have two points, then prepare the block solver. if (vc.PointCount == 2) { ContactVelocityConstraint.VelocityConstraintPoint vcp1 = vc.Points[0]; ContactVelocityConstraint.VelocityConstraintPoint vcp2 = vc.Points[1]; float rn1A = Vec2.Cross(vcp1.RA, vc.Normal); float rn1B = Vec2.Cross(vcp1.RB, vc.Normal); float rn2A = Vec2.Cross(vcp2.RA, vc.Normal); float rn2B = Vec2.Cross(vcp2.RB, vc.Normal); float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; if (k11 * k11 < MAX_CONDITION_NUMBER * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.Ex.Set(k11, k12); vc.K.Ey.Set(k12, k22); vc.K.InvertToOut(vc.NormalMass); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.PointCount = 1; } } } }
/// <seealso cref="Joint.solveVelocityConstraints(TimeStep)"></seealso> public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[m_indexA].V; float wA = data.Velocities[m_indexA].W; Vec2 vB = data.Velocities[m_indexB].V; float wB = data.Velocities[m_indexB].W; float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Vec2 Cdot1 = Pool.PopVec2(); Vec2 P = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); if (Frequency > 0.0f) { float Cdot2 = wB - wA; float impulse2 = (-m_mass.Ez.Z) * (Cdot2 + m_bias + m_gamma * m_impulse.Z); m_impulse.Z += impulse2; wA -= iA * impulse2; wB += iB * impulse2; Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1); Vec2.CrossToOutUnsafe(wA, m_rA, temp); Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp); Vec2 impulse1 = P; Mat33.Mul22ToOutUnsafe(m_mass, Cdot1, impulse1); impulse1.NegateLocal(); m_impulse.X += impulse1.X; m_impulse.Y += impulse1.Y; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * Vec2.Cross(m_rA, P); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * Vec2.Cross(m_rB, P); } else { Vec2.CrossToOutUnsafe(wA, m_rA, temp); Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1); Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp); float Cdot2 = wB - wA; Vec3 Cdot = Pool.PopVec3(); Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2); Vec3 impulse = Pool.PopVec3(); Mat33.MulToOutUnsafe(m_mass, Cdot, impulse); impulse.NegateLocal(); m_impulse.AddLocal(impulse); P.Set(impulse.X, impulse.Y); vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * (Vec2.Cross(m_rA, P) + impulse.Z); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * (Vec2.Cross(m_rB, P) + impulse.Z); Pool.PushVec3(2); } data.Velocities[m_indexA].V.Set(vA); data.Velocities[m_indexA].W = wA; data.Velocities[m_indexB].V.Set(vB); data.Velocities[m_indexB].W = wB; Pool.PushVec2(3); }
/// <summary> /// This resets the mass properties to the sum of the mass properties of the fixtures. This /// normally does not need to be called unless you called setMassData to override the mass and you /// later want to reset the mass. /// </summary> public void ResetMassData() { // Compute mass data from shapes. Each shape has its own density. Mass = 0.0f; InvMass = 0.0f; I = 0.0f; InvI = 0.0f; Sweep.LocalCenter.SetZero(); // Static and kinematic bodies have zero mass. if (m_type == BodyType.Static || m_type == BodyType.Kinematic) { // m_sweep.c0 = m_sweep.c = m_xf.position; Sweep.C0.Set(Xf.P); Sweep.C.Set(Xf.P); Sweep.A0 = Sweep.A; return; } Debug.Assert(m_type == BodyType.Dynamic); // Accumulate mass over all fixtures. Vec2 localCenter = World.Pool.PopVec2(); localCenter.SetZero(); Vec2 temp = World.Pool.PopVec2(); MassData massData = pmd; for (Fixture f = FixtureList; f != null; f = f.Next) { if (f.Density == 0.0f) { continue; } f.GetMassData(massData); Mass += massData.Mass; // center += massData.mass * massData.center; temp.Set(massData.Center).MulLocal(massData.Mass); localCenter.AddLocal(temp); I += massData.I; } // Compute center of mass. if (Mass > 0.0f) { InvMass = 1.0f / Mass; localCenter.MulLocal(InvMass); } else { // Force all dynamic bodies to have a positive mass. Mass = 1.0f; InvMass = 1.0f; } if (I > 0.0f && (Flags & TypeFlags.FixedRotation) == 0) { // Center the inertia about the center of mass. I -= Mass * Vec2.Dot(localCenter, localCenter); Debug.Assert(I > 0.0f); InvI = 1.0f / I; } else { I = 0.0f; InvI = 0.0f; } Vec2 oldCenter = World.Pool.PopVec2(); // Move center of mass. oldCenter.Set(Sweep.C); Sweep.LocalCenter.Set(localCenter); // m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter); Transform.MulToOutUnsafe(Xf, Sweep.LocalCenter, Sweep.C0); Sweep.C.Set(Sweep.C0); // Update center of mass velocity. // m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter); temp.Set(Sweep.C).SubLocal(oldCenter); Vec2 temp2 = oldCenter; Vec2.CrossToOutUnsafe(m_angularVelocity, temp, temp2); m_linearVelocity.AddLocal(temp2); World.Pool.PushVec2(3); }
// TODO_ERIN might not need to return the separation public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1) { ProxyA = proxyA; ProxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); SweepA = sweepA; SweepB = sweepB; SweepA.GetTransform(xfa, t1); SweepB.GetTransform(xfb, t1); // log.debug("initializing separation.\n" + // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n" // "distance: "+proxyA. if (count == 1) { Type = Type.Points; /* * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2 * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize(); */ localPointA.Set(ProxyA.GetVertex(cache.IndexA[0])); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); Transform.MulToOutUnsafe(xfb, localPointB, pointB); Axis.Set(pointB).SubLocal(pointA); float s = Axis.Normalize(); return(s); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. Type = Type.FaceB; localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0])); localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1])); temp.Set(localPointB2).SubLocal(localPointB1); Vec2.CrossToOutUnsafe(temp, 1f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfb.Q, Axis, normal); LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f); Transform.MulToOutUnsafe(xfb, LocalPoint, pointB); localPointA.Set(proxyA.GetVertex(cache.IndexA[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); temp.Set(pointA).SubLocal(pointB); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return(s); } else { // Two points on A and one or two points on B. Type = Type.FaceA; localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0])); localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1])); temp.Set(localPointA2).SubLocal(localPointA1); Vec2.CrossToOutUnsafe(temp, 1.0f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfa.Q, Axis, normal); LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f); Transform.MulToOutUnsafe(xfa, LocalPoint, pointA); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfb, localPointB, pointB); temp.Set(pointB).SubLocal(pointA); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return(s); } }
/// <summary> /// Create a convex hull from the given array of points. /// The count must be in the range [3, Settings.maxPolygonVertices]. /// This method takes an arraypool for pooling /// </summary> /// <warning>the points may be re-ordered, even if they form a convex polygon</warning> /// <warning>collinear points are handled but not removed. Collinear points may lead to poor stacking behavior.</warning> public void Set(Vec2[] verts, int num, Vec2Array vecPool, IntArray intPool) { Debug.Assert(3 <= num && num <= Settings.MAX_POLYGON_VERTICES); if (num < 3) { SetAsBox(1.0f, 1.0f); return; } int n = MathUtils.Min(num, Settings.MAX_POLYGON_VERTICES); // Copy the vertices into a local buffer Vec2[] ps = (vecPool != null) ? vecPool.Get(n) : new Vec2[n]; for (int i = 0; i < n; ++i) { ps[i] = verts[i]; } // Create the convex hull using the Gift wrapping algorithm // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm // Find the right most point on the hull int i0 = 0; float x0 = ps[0].X; for (int i = 1; i < num; ++i) { float x = ps[i].X; if (x > x0 || (x == x0 && ps[i].Y < ps[i0].Y)) { i0 = i; x0 = x; } } int[] hull = (intPool != null) ? intPool.Get(Settings.MAX_POLYGON_VERTICES) : new int[Settings.MAX_POLYGON_VERTICES]; int m = 0; int ih = i0; while (true) { hull[m] = ih; int ie = 0; for (int j = 1; j < n; ++j) { if (ie == ih) { ie = j; continue; } Vec2 r = pool1.Set(ps[ie]).SubLocal(ps[hull[m]]); Vec2 v = pool2.Set(ps[j]).SubLocal(ps[hull[m]]); float c = Vec2.Cross(r, v); if (c < 0.0f) { ie = j; } // Collinearity check if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) { ie = j; } } ++m; ih = ie; if (ie == i0) { break; } } VertexCount = m; // Copy vertices. for (int i = 0; i < VertexCount; ++i) { if (Vertices[i] == null) { Vertices[i] = new Vec2(); } Vertices[i].Set(ps[hull[i]]); } Vec2 edge = pool1; // Compute normals. Ensure the edges have non-zero length. for (int i = 0; i < VertexCount; ++i) { int i1 = i; int i2 = i + 1 < VertexCount ? i + 1 : 0; edge.Set(Vertices[i2]).SubLocal(Vertices[i1]); Debug.Assert(edge.LengthSquared() > Settings.EPSILON * Settings.EPSILON); Vec2.CrossToOutUnsafe(edge, 1f, Normals[i]); Normals[i].Normalize(); } // Compute the polygon centroid. ComputeCentroidToOut(Vertices, VertexCount, Centroid); }
public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (m_motorEnabled && LimitState != LimitState.Equal && fixedRotation == false) { float Cdot = wB - wA - m_motorSpeed; float impulse = (-MotorMass) * Cdot; float oldImpulse = MotorImpulse; float maxImpulse = data.Step.Dt * m_maxMotorTorque; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } Vec2 temp = Pool.PopVec2(); // Solve limit constraint. if (m_limitEnabled && LimitState != LimitState.Inactive && fixedRotation == false) { Vec2 Cdot1 = Pool.PopVec2(); Vec3 Cdot = Pool.PopVec3(); // Solve point-to-point constraint Vec2.CrossToOutUnsafe(wA, RA, temp); Vec2.CrossToOutUnsafe(wB, RB, Cdot1); Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp); float Cdot2 = wB - wA; Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2); Vec3 impulse = Pool.PopVec3(); Mass.Solve33ToOut(Cdot, impulse); impulse.NegateLocal(); if (LimitState == LimitState.Equal) { Impulse.AddLocal(impulse); } else if (LimitState == LimitState.AtLower) { float newImpulse = Impulse.Z + impulse.Z; if (newImpulse < 0.0f) { //UPGRADE_NOTE: Final was removed from the declaration of 'rhs '. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1003'" Vec2 rhs = Pool.PopVec2(); rhs.Set(Mass.Ez.X, Mass.Ez.Y).MulLocal(Impulse.Z).SubLocal(Cdot1); Mass.Solve22ToOut(rhs, temp); impulse.X = temp.X; impulse.Y = temp.Y; impulse.Z = -Impulse.Z; Impulse.X += temp.X; Impulse.Y += temp.Y; Impulse.Z = 0.0f; Pool.PushVec2(1); } else { Impulse.AddLocal(impulse); } } else if (LimitState == LimitState.AtUpper) { float newImpulse = Impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vec2 rhs = Pool.PopVec2(); rhs.Set(Mass.Ez.X, Mass.Ez.Y).MulLocal(Impulse.Z).SubLocal(Cdot1); Mass.Solve22ToOut(rhs, temp); impulse.X = temp.X; impulse.Y = temp.Y; impulse.Z = -Impulse.Z; Impulse.X += temp.X; Impulse.Y += temp.Y; Impulse.Z = 0.0f; Pool.PushVec2(1); } else { Impulse.AddLocal(impulse); } } Vec2 P = Pool.PopVec2(); P.Set(impulse.X, impulse.Y); vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * (Vec2.Cross(RA, P) + impulse.Z); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * (Vec2.Cross(RB, P) + impulse.Z); Pool.PushVec2(2); Pool.PushVec3(2); } else { // Solve point-to-point constraint Vec2 Cdot = Pool.PopVec2(); Vec2 impulse = Pool.PopVec2(); Vec2.CrossToOutUnsafe(wA, RA, temp); Vec2.CrossToOutUnsafe(wB, RB, Cdot); Cdot.AddLocal(vB).SubLocal(vA).SubLocal(temp); Mass.Solve22ToOut(Cdot.NegateLocal(), impulse); // just leave negated Impulse.X += impulse.X; Impulse.Y += impulse.Y; vA.X -= mA * impulse.X; vA.Y -= mA * impulse.Y; wA -= iA * Vec2.Cross(RA, impulse); vB.X += mB * impulse.X; vB.Y += mB * impulse.Y; wB += iB * Vec2.Cross(RB, impulse); Pool.PushVec2(2); } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(1); }
/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback to perform a exact /// ray-cast in the case were the proxy contains a shape. The callback also performs the any /// collision filtering. This has performance roughly equal to k * log(n), where k is the number of /// collisions and n is the number of proxies in the tree. /// </summary> /// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> /// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param> public void Raycast(ITreeRayCastCallback callback, RayCastInput input) { Vec2 p1 = input.P1; Vec2 p2 = input.P2; r.Set(p2).SubLocal(p1); Debug.Assert(r.LengthSquared() > 0f); r.Normalize(); // v is perpendicular to the segment. Vec2.CrossToOutUnsafe(1f, r, v); absV.Set(v).AbsLocal(); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segAABB = aabb; // Vec2 t = p1 + maxFraction * (p2 - p1); temp.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1); Vec2.MinToOut(p1, temp, segAABB.LowerBound); Vec2.MaxToOut(p1, temp, segAABB.UpperBound); intStack.Push(m_root); while (intStack.Count > 0) { int nodeId = intStack.Pop(); if (nodeId == TreeNode.NULL_NODE) { continue; } TreeNode node = m_nodes[nodeId]; if (!AABB.TestOverlap(node.AABB, segAABB)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) node.AABB.GetCenterToOut(c); node.AABB.GetExtentsToOut(h); temp.Set(p1).SubLocal(c); float separation = MathUtils.Abs(Vec2.Dot(v, temp)) - Vec2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.Leaf) { subInput.P1.Set(input.P1); subInput.P2.Set(input.P2); subInput.MaxFraction = maxFraction; float value = callback.RaycastCallback(subInput, nodeId); if (value == 0.0f) { // The client has terminated the ray cast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; t.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1); Vec2.MinToOut(p1, t, segAABB.LowerBound); Vec2.MaxToOut(p1, t, segAABB.UpperBound); } } else { intStack.Push(node.Child1); intStack.Push(node.Child2); } } }
public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; float h = data.Step.Dt; // Solve angular friction { float Cdot = wB - wA; float impulse = (-AngularMass) * Cdot; float oldImpulse = m_angularImpulse; float maxImpulse = h * m_maxTorque; m_angularImpulse = MathUtils.Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_angularImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve linear friction { Vec2 Cdot = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); Vec2.CrossToOutUnsafe(wA, RA, temp); Vec2.CrossToOutUnsafe(wB, RB, Cdot); Cdot.AddLocal(vB).SubLocal(vA).SubLocal(temp); Vec2 impulse = Pool.PopVec2(); Mat22.MulToOutUnsafe(LinearMass, Cdot, impulse); impulse.NegateLocal(); Vec2 oldImpulse = Pool.PopVec2(); oldImpulse.Set(m_linearImpulse); m_linearImpulse.AddLocal(impulse); float maxImpulse = h * m_maxForce; if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { m_linearImpulse.Normalize(); m_linearImpulse.MulLocal(maxImpulse); } impulse.Set(m_linearImpulse).SubLocal(oldImpulse); temp.Set(impulse).MulLocal(mA); vA.SubLocal(temp); wA -= iA * Vec2.Cross(RA, impulse); temp.Set(impulse).MulLocal(mB); vB.AddLocal(temp); wB += iB * Vec2.Cross(RB, impulse); } data.Velocities[IndexA].V.Set(vA); if (data.Velocities[IndexA].W != wA) { Debug.Assert(data.Velocities[IndexA].W != wA); } data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(4); }