/// <summary> /// Inits the velocity constraints using the specified step /// </summary> /// <param name="step">The step</param> internal override void InitVelocityConstraints(TimeStep step) { Body body2 = Body2; float body2Mass = body2.GetMass(); // Frequency float omega = 2.0f * Settings.Pi * FrequencyHz; // Damping coefficient float coefficient = 2.0f * body2Mass * DampingRatio * omega; // Spring stiffness float stiffness = body2Mass * (omega * omega); // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. Box2DxDebug.Assert(coefficient + step.Dt * stiffness > Settings.FltEpsilon); Gamma = 1.0f / (step.Dt * (coefficient + step.Dt * stiffness)); Beta = step.Dt * stiffness * Gamma; // Compute the effective mass matrix. Vec2 effectiveMass = Math.Mul(body2.GetXForm().R, LocalAnchor - body2.GetLocalCenter()); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] float invMass = body2.InvMass; float invI = body2.InvI; Mat22 k1 = new Mat22 { Col1 = new Vec2(invMass, 0.0f), Col2 = new Vec2(0.0f, invMass) }; Mat22 k2 = new Mat22 { Col1 = new Vec2(invI * effectiveMass.Y * effectiveMass.Y, -invI * effectiveMass.X * effectiveMass.Y), Col2 = new Vec2(-invI * effectiveMass.X * effectiveMass.Y, invI * effectiveMass.X * effectiveMass.X) }; Mat22 k = k1 + k2; k.Col1.X += Gamma; k.Col2.Y += Gamma; Mass = k.GetInverse(); C = body2.Sweep.C + effectiveMass - Target; // Cheat with some damping body2.AngularVelocity *= 0.98f; // Warm starting. Impulse *= step.DtRatio; body2.LinearVelocity += invMass * Impulse; body2.AngularVelocity += invI * Vec2.Cross(effectiveMass, Impulse); }
// Query an AABB for overlapping proxies, returns the user data and // the count, up to the supplied maximum count. /// <summary> /// Queries the aabb /// </summary> /// <param name="aabb">The aabb</param> /// <param name="userData">The user data</param> /// <param name="maxCount">The max count</param> /// <returns>The count</returns> public int Query(Aabb aabb, object[] userData, int maxCount) { ushort[] lowerValues; ushort[] upperValues; ComputeBounds(out lowerValues, out upperValues, aabb); int lowerIndex, upperIndex; Query(out lowerIndex, out upperIndex, lowerValues[0], upperValues[0], Bounds[0], 2 * ProxyCount, 0); Query(out lowerIndex, out upperIndex, lowerValues[1], upperValues[1], Bounds[1], 2 * ProxyCount, 1); Box2DxDebug.Assert(QueryResultCount < Settings.MaxProxies); int count = 0; for (int i = 0; i < QueryResultCount && count < maxCount; ++i, ++count) { Box2DxDebug.Assert(QueryResults[i] < Settings.MaxProxies); Proxy proxy = ProxyPool[QueryResults[i]]; Box2DxDebug.Assert(proxy.IsValid); userData[i] = proxy.UserData; } // Prepare for next query. QueryResultCount = 0; IncrementTimeStamp(); return(count); }
/// <summary> /// Creates the fixture a /// </summary> /// <param name="fixtureA">The fixture</param> /// <param name="fixtureB">The fixture</param> /// <returns>The contact</returns> public static Contact Create(Fixture fixtureA, Fixture fixtureB) { if (SInitialized == false) { InitializeRegisters(); SInitialized = true; } ShapeType type1 = fixtureA.ShapeType; ShapeType type2 = fixtureB.ShapeType; Box2DxDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount); Box2DxDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount); ContactCreateFcn createFcn = SRegisters[(int)type1][(int)type2].CreateFcn; if (createFcn != null) { if (SRegisters[(int)type1][(int)type2].Primary) { return(createFcn(fixtureA, fixtureB)); } return(createFcn(fixtureB, fixtureA)); } return(null); }
/// <summary> /// Gets the closest point /// </summary> /// <returns>The vec</returns> internal Vec2 GetClosestPoint() { switch (Count) { case 0: #if DEBUG Box2DxDebug.Assert(false); #endif return(Vec2.Zero); case 1: return(V1.W); case 2: return(V1.A * V1.W + V2.A * V2.W); case 3: return(Vec2.Zero); default: #if DEBUG Box2DxDebug.Assert(false); #endif return(Vec2.Zero); } }
/// <summary> /// Initializes a new instance of the <see cref="PolyAndEdgeContact" /> class /// </summary> /// <param name="fixtureA">The fixture</param> /// <param name="fixtureB">The fixture</param> public PolyAndEdgeContact(Fixture fixtureA, Fixture fixtureB) : base(fixtureA, fixtureB) { Box2DxDebug.Assert(fixtureA.ShapeType == ShapeType.PolygonShape); Box2DxDebug.Assert(fixtureB.ShapeType == ShapeType.EdgeShape); CollideShapeFunction = CollidePolyAndEdgeContact; }
/// <summary> /// Gets the metric /// </summary> /// <returns>The float</returns> internal float GetMetric() { switch (Count) { case 0: #if DEBUG Box2DxDebug.Assert(false); #endif return(0.0f); case 1: return(0.0f); case 2: return(Vec2.Distance(V1.W, V2.W)); case 3: return(Vec2.Cross(V2.W - V1.W, V3.W - V1.W)); default: #if DEBUG Box2DxDebug.Assert(false); #endif return(0.0f); } }
/// <summary> /// Gets the witness points using the specified p a /// </summary> /// <param name="pA">The </param> /// <param name="pB">The </param> internal unsafe void GetWitnessPoints(Vec2 *pA, Vec2 *pB) { switch (Count) { case 0: Box2DxDebug.Assert(false); break; case 1: *pA = V1.Wa; *pB = V1.Wb; break; case 2: *pA = V1.A * V1.Wa + V2.A * V2.Wa; *pB = V1.A * V1.Wb + V2.A * V2.Wb; break; case 3: *pA = V1.A * V1.Wa + V2.A * V2.Wa + V3.A * V3.Wa; *pB = *pA; break; default: Box2DxDebug.Assert(false); break; } }
/// <summary> /// Validates this instance /// </summary> public void Validate() { for (int axis = 0; axis < 2; ++axis) { Bound[] bounds = Bounds[axis]; int boundCount = 2 * ProxyCount; ushort stabbingCount = 0; for (int i = 0; i < boundCount; ++i) { Bound bound = bounds[i]; Box2DxDebug.Assert(i == 0 || bounds[i - 1].Value <= bound.Value); Box2DxDebug.Assert(bound.ProxyId != PairManager.NullProxy); Box2DxDebug.Assert(ProxyPool[bound.ProxyId].IsValid); if (bound.IsLower) { Box2DxDebug.Assert(ProxyPool[bound.ProxyId].LowerBounds[axis] == i); ++stabbingCount; } else { Box2DxDebug.Assert(ProxyPool[bound.ProxyId].UpperBounds[axis] == i); --stabbingCount; } Box2DxDebug.Assert(bound.StabbingCount == stabbingCount); } } }
/// <summary> /// Validates the buffer /// </summary> private void ValidateBuffer() { #if DEBUG Box2DxDebug.Assert(PairBufferCount <= PairCount); //std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount); BufferedPair[] tmp = new BufferedPair[PairBufferCount]; Array.Copy(PairBuffer, 0, tmp, 0, PairBufferCount); Array.Sort(tmp, BufferedPairSortPredicate); Array.Copy(tmp, 0, PairBuffer, 0, PairBufferCount); for (int i = 0; i < PairBufferCount; ++i) { if (i > 0) { Box2DxDebug.Assert(Equals(PairBuffer[i], PairBuffer[i - 1]) == false); } Pair pair = Find(PairBuffer[i].ProxyId1, PairBuffer[i].ProxyId2); Box2DxDebug.Assert(pair.IsBuffered()); Box2DxDebug.Assert(pair.ProxyId1 != pair.ProxyId2); Box2DxDebug.Assert(pair.ProxyId1 < Settings.MaxProxies); Box2DxDebug.Assert(pair.ProxyId2 < Settings.MaxProxies); Proxy proxy1 = BroadPhase.ProxyPool[pair.ProxyId1]; Proxy proxy2 = BroadPhase.ProxyPool[pair.ProxyId2]; Box2DxDebug.Assert(proxy1.IsValid); Box2DxDebug.Assert(proxy2.IsValid); } #endif }
/// <summary> /// Validates the table /// </summary> private void ValidateTable() { #if DEBUG for (int i = 0; i < TableCapacity; ++i) { ushort index = HashTable[i]; while (index != NullPair) { Pair pair = Pairs[index]; Box2DxDebug.Assert(pair.IsBuffered() == false); Box2DxDebug.Assert(pair.IsFinal()); Box2DxDebug.Assert(pair.IsRemoved() == false); Box2DxDebug.Assert(pair.ProxyId1 != pair.ProxyId2); Box2DxDebug.Assert(pair.ProxyId1 < Settings.MaxProxies); Box2DxDebug.Assert(pair.ProxyId2 < Settings.MaxProxies); Proxy proxy1 = BroadPhase.ProxyPool[pair.ProxyId1]; Proxy proxy2 = BroadPhase.ProxyPool[pair.ProxyId2]; Box2DxDebug.Assert(proxy1.IsValid); Box2DxDebug.Assert(proxy2.IsValid); Box2DxDebug.Assert(BroadPhase.TestOverlap(proxy1, proxy2)); index = pair.Next; } } #endif }
// Returns existing pair or creates a new one. /// <summary> /// Adds the pair using the specified proxy id 1 /// </summary> /// <param name="proxyId1">The proxy id</param> /// <param name="proxyId2">The proxy id</param> /// <returns>The pair</returns> private Pair AddPair(int proxyId1, int proxyId2) { if (proxyId1 > proxyId2) { Math.Swap(ref proxyId1, ref proxyId2); } int hash = (int)(Hash((uint)proxyId1, (uint)proxyId2) & TableMask); Pair pair = Find(proxyId1, proxyId2, (uint)hash); if (pair != null) { return(pair); } Box2DxDebug.Assert(PairCount < Settings.MaxPairs && FreePair != NullPair); ushort pairIndex = FreePair; pair = Pairs[pairIndex]; FreePair = pair.Next; pair.ProxyId1 = (ushort)proxyId1; pair.ProxyId2 = (ushort)proxyId2; pair.Status = 0; pair.UserData = null; pair.Next = HashTable[hash]; HashTable[hash] = pairIndex; ++PairCount; return(pair); }
// Buffer a pair for removal. /// <summary> /// Removes the buffered pair using the specified id 1 /// </summary> /// <param name="id1">The id</param> /// <param name="id2">The id</param> public void RemoveBufferedPair(int id1, int id2) { Box2DxDebug.Assert(id1 != NullProxy && id2 != NullProxy); Box2DxDebug.Assert(PairBufferCount < Settings.MaxPairs); Pair pair = Find(id1, id2); if (pair == null) { // The pair never existed. This is legal (due to collision filtering). return; } // If this pair is not in the pair buffer ... if (pair.IsBuffered() == false) { // This must be an old pair. Box2DxDebug.Assert(pair.IsFinal()); pair.SetBuffered(); PairBuffer[PairBufferCount].ProxyId1 = pair.ProxyId1; PairBuffer[PairBufferCount].ProxyId2 = pair.ProxyId2; ++PairBufferCount; Box2DxDebug.Assert(PairBufferCount <= PairCount); } pair.SetRemoved(); if (BroadPhase.IsValidate) { ValidateBuffer(); } }
/* * As proxies are created and moved, many pairs are created and destroyed. Even worse, the same * pair may be added and removed multiple times in a single time step of the physics engine. To reduce * traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the * end of the physics step. This is done by buffering all the RemovePair requests. AddPair * requests are processed immediately because we need the hash table entry for quick lookup. * * All user user callbacks are delayed until the buffered pairs are confirmed in Commit. * This is very important because the user callbacks may be very expensive and client logic * may be harmed if pairs are added and removed within the same time step. * * Buffer a pair for addition. * We may add a pair that is not in the pair manager or pair buffer. * We may add a pair that is already in the pair manager and pair buffer. * If the added pair is not a new pair, then it must be in the pair buffer (because RemovePair was called). */ /// <summary> /// Adds the buffered pair using the specified id 1 /// </summary> /// <param name="id1">The id</param> /// <param name="id2">The id</param> public void AddBufferedPair(int id1, int id2) { Box2DxDebug.Assert(id1 != NullProxy && id2 != NullProxy); Box2DxDebug.Assert(PairBufferCount < Settings.MaxPairs); Pair pair = AddPair(id1, id2); // If this pair is not in the pair buffer ... if (pair.IsBuffered() == false) { // This must be a newly added pair. Box2DxDebug.Assert(pair.IsFinal() == false); // Add it to the pair buffer. pair.SetBuffered(); PairBuffer[PairBufferCount].ProxyId1 = pair.ProxyId1; PairBuffer[PairBufferCount].ProxyId2 = pair.ProxyId2; ++PairBufferCount; Box2DxDebug.Assert(PairBufferCount <= PairCount); } // Confirm this pair for the subsequent call to Commit. pair.ClearRemoved(); if (BroadPhase.IsValidate) { ValidateBuffer(); } }
/// <summary> /// The assert /// </summary> public float this[int i] { get { if (i == 0) { return(X); } if (i == 1) { return(Y); } Box2DxDebug.Assert(false, "Incorrect Vec2 element!"); return(0); } set { if (i == 0) { X = value; } else if (i == 1) { Y = value; } else { Box2DxDebug.Assert(false, "Incorrect Vec2 element!"); } } }
/// <summary> /// Commits this instance /// </summary> public void Commit() { int removeCount = 0; Proxy[] proxies = BroadPhase.ProxyPool; for (int i = 0; i < PairBufferCount; ++i) { Pair pair = Find(PairBuffer[i].ProxyId1, PairBuffer[i].ProxyId2); Box2DxDebug.Assert(pair.IsBuffered()); pair.ClearBuffered(); Box2DxDebug.Assert(pair.ProxyId1 < Settings.MaxProxies && pair.ProxyId2 < Settings.MaxProxies); Proxy proxy1 = proxies[pair.ProxyId1]; Proxy proxy2 = proxies[pair.ProxyId2]; Box2DxDebug.Assert(proxy1.IsValid); Box2DxDebug.Assert(proxy2.IsValid); if (pair.IsRemoved()) { // It is possible a pair was added then removed before a commit. Therefore, // we should be careful not to tell the user the pair was removed when the // the user didn't receive a matching add. if (pair.IsFinal()) { Callback.PairRemoved(proxy1.UserData, proxy2.UserData, pair.UserData); } // Store the ids so we can actually remove the pair below. PairBuffer[removeCount].ProxyId1 = pair.ProxyId1; PairBuffer[removeCount].ProxyId2 = pair.ProxyId2; ++removeCount; } else { Box2DxDebug.Assert(BroadPhase.TestOverlap(proxy1, proxy2)); if (pair.IsFinal() == false) { pair.UserData = Callback.PairAdded(proxy1.UserData, proxy2.UserData); pair.SetFinal(); } } } for (int i = 0; i < removeCount; ++i) { RemovePair(PairBuffer[i].ProxyId1, PairBuffer[i].ProxyId2); } PairBufferCount = 0; if (BroadPhase.IsValidate) { ValidateTable(); } }
/// <summary> /// Set the joint limits, usually in meters. /// </summary> public void SetLimits(float lower, float upper) { Box2DxDebug.Assert(lower <= upper); Body1.WakeUp(); Body2.WakeUp(); LowerLimit = lower; UpperLimit = upper; }
// Removes a pair. The pair must exist. /// <summary> /// Removes the pair using the specified proxy id 1 /// </summary> /// <param name="proxyId1">The proxy id</param> /// <param name="proxyId2">The proxy id</param> /// <returns>The object</returns> private object RemovePair(int proxyId1, int proxyId2) { Box2DxDebug.Assert(PairCount > 0); if (proxyId1 > proxyId2) { Math.Swap(ref proxyId1, ref proxyId2); } int hash = (int)(Hash((uint)proxyId1, (uint)proxyId2) & TableMask); //uint16* node = &m_hashTable[hash]; ushort node = HashTable[hash]; bool ion = false; int ni = 0; while (node != NullPair) { if (Equals(Pairs[node], proxyId1, proxyId2)) { //uint16 index = *node; //*node = m_pairs[*node].next; ushort index = node; node = Pairs[node].Next; if (ion) { Pairs[ni].Next = node; } else { HashTable[hash] = node; } Pair pair = Pairs[index]; object userData = pair.UserData; // Scrub pair.Next = FreePair; pair.ProxyId1 = NullProxy; pair.ProxyId2 = NullProxy; pair.UserData = null; pair.Status = 0; FreePair = index; --PairCount; return(userData); } //node = &m_pairs[*node].next; ni = node; node = Pairs[ni].Next; ion = true; } Box2DxDebug.Assert(false); return(null); }
/// <summary> /// Initializes a new instance of the <see cref="GearJoint" /> class /// </summary> /// <param name="def">The def</param> public GearJoint(GearJointDef def) : base(def) { JointType type1 = def.Joint1.GetType(); JointType type2 = def.Joint2.GetType(); Box2DxDebug.Assert(type1 == JointType.RevoluteJoint || type1 == JointType.PrismaticJoint); Box2DxDebug.Assert(type2 == JointType.RevoluteJoint || type2 == JointType.PrismaticJoint); Box2DxDebug.Assert(def.Joint1.GetBody1().IsStatic()); Box2DxDebug.Assert(def.Joint2.GetBody1().IsStatic()); Revolute1 = null; Prismatic1 = null; Revolute2 = null; Prismatic2 = null; float coordinate1, coordinate2; Ground1 = def.Joint1.GetBody1(); Body1 = def.Joint1.GetBody2(); if (type1 == JointType.RevoluteJoint) { Revolute1 = (RevoluteJoint)def.Joint1; GroundAnchor1 = Revolute1.LocalAnchor1; LocalAnchor1 = Revolute1.LocalAnchor2; coordinate1 = Revolute1.JointAngleX; } else { Prismatic1 = (PrismaticJoint)def.Joint1; GroundAnchor1 = Prismatic1.LocalAnchor1; LocalAnchor1 = Prismatic1.LocalAnchor2; coordinate1 = Prismatic1.JointTranslation; } Ground2 = def.Joint2.GetBody1(); Body2 = def.Joint2.GetBody2(); if (type2 == JointType.RevoluteJoint) { Revolute2 = (RevoluteJoint)def.Joint2; GroundAnchor2 = Revolute2.LocalAnchor1; LocalAnchor2 = Revolute2.LocalAnchor2; coordinate2 = Revolute2.JointAngleX; } else { Prismatic2 = (PrismaticJoint)def.Joint2; GroundAnchor2 = Prismatic2.LocalAnchor1; LocalAnchor2 = Prismatic2.LocalAnchor2; coordinate2 = Prismatic2.JointTranslation; } Ratio = def.Ratio; Constant = coordinate1 + Ratio * coordinate2; Impulse = 0.0f; }
/// <summary> /// Inits the velocity constraints using the specified step /// </summary> /// <param name="step">The step</param> internal override void InitVelocityConstraints(TimeStep step) { Body g1 = Ground1; Body g2 = Ground2; Body b1 = Body1; Body b2 = Body2; float k = 0.0f; jacobian.SetZero(); if (Revolute1 != null) { jacobian.Angular1 = -1.0f; k += b1.InvI; } else { Vec2 ug = Math.Mul(g1.GetXForm().R, Prismatic1.LocalXAxis1); Vec2 r = Math.Mul(b1.GetXForm().R, LocalAnchor1 - b1.GetLocalCenter()); float crug = Vec2.Cross(r, ug); jacobian.Linear1 = -ug; jacobian.Angular1 = -crug; k += b1.InvMass + b1.InvI * crug * crug; } if (Revolute2 != null) { jacobian.Angular2 = -Ratio; k += Ratio * Ratio * b2.InvI; } else { Vec2 ug = Math.Mul(g2.GetXForm().R, Prismatic2.LocalXAxis1); Vec2 r = Math.Mul(b2.GetXForm().R, LocalAnchor2 - b2.GetLocalCenter()); float crug = Vec2.Cross(r, ug); jacobian.Linear2 = -Ratio * ug; jacobian.Angular2 = -Ratio * crug; k += Ratio * Ratio * (b2.InvMass + b2.InvI * crug * crug); } // Compute effective mass. Box2DxDebug.Assert(k > 0.0f); Mass = 1.0f / k; if (step.WarmStarting) { // Warm starting. b1.LinearVelocity += b1.InvMass * Impulse * jacobian.Linear1; b1.AngularVelocity += b1.InvI * Impulse * jacobian.Angular1; b2.LinearVelocity += b2.InvMass * Impulse * jacobian.Linear2; b2.AngularVelocity += b2.InvI * Impulse * jacobian.Angular2; } else { Impulse = 0.0f; } }
/// <summary> /// Evaluates this instance /// </summary> public void Evaluate() { Body bodyA = FixtureA.Body; Body bodyB = FixtureB.Body; Box2DxDebug.Assert(CollideShapeFunction != null); CollideShapeFunction(ref manifold, FixtureA.Shape, bodyA.GetXForm(), FixtureB.Shape, bodyB.GetXForm()); }
/// <summary> /// Initializes the cc /// </summary> /// <param name="cc">The cc</param> internal void Initialize(ContactConstraint cc) { Box2DxDebug.Assert(cc.PointCount > 0); switch (cc.Type) { case ManifoldType.Circles: { Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint); Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint); if (Vec2.DistanceSquared(pointA, pointB) > Settings.FltEpsilonSquared) { Normal = pointB - pointA; Normal.Normalize(); } else { Normal.Set(1.0f, 0.0f); } Points[0] = 0.5f * (pointA + pointB); Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius; } break; case ManifoldType.FaceA: { Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } } break; case ManifoldType.FaceB: { Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal); Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint); for (int i = 0; i < cc.PointCount; ++i) { Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint); Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Points[i] = clipPoint; } // Ensure normal points from A to B Normal = -Normal; } break; } }
/// <summary> /// Initializes a new instance of the <see cref="EdgeAndCircleContact" /> class /// </summary> /// <param name="fixtureA">The fixture</param> /// <param name="fixtureB">The fixture</param> public EdgeAndCircleContact(Fixture fixtureA, Fixture fixtureB) : base(fixtureA, fixtureB) { Box2DxDebug.Assert(fixtureA.ShapeType == ShapeType.EdgeShape); Box2DxDebug.Assert(fixtureB.ShapeType == ShapeType.CircleShape); Manifold.PointCount = 0; Manifold.Points[0].NormalImpulse = 0.0f; Manifold.Points[0].TangentImpulse = 0.0f; CollideShapeFunction = CollideEdgeAndCircle; }
/// <summary> /// Reads the cache using the specified cache /// </summary> /// <param name="cache">The cache</param> /// <param name="shapeA">The shape</param> /// <param name="transformA">The transform</param> /// <param name="shapeB">The shape</param> /// <param name="transformB">The transform</param> internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, XForm transformA, Shape shapeB, XForm transformB) { Box2DxDebug.Assert(0 <= cache->Count && cache->Count <= 3); // Copy data from cache. Count = cache->Count; SimplexVertex **vertices = stackalloc SimplexVertex *[3]; fixed(SimplexVertex *v1Ptr = &V1, v2Ptr = &V2, v3Ptr = &V3) { vertices[0] = v1Ptr; vertices[1] = v2Ptr; vertices[2] = v3Ptr; for (int i = 0; i < Count; ++i) { SimplexVertex *v = vertices[i]; v->IndexA = cache->IndexA[i]; v->IndexB = cache->IndexB[i]; Vec2 wALocal = shapeA.GetVertex(v->IndexA); Vec2 wBLocal = shapeB.GetVertex(v->IndexB); v->Wa = Math.Mul(transformA, wALocal); v->Wb = Math.Mul(transformB, wBLocal); v->W = v->Wb - v->Wa; v->A = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (Count > 1) { float metric1 = cache->Metric; float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FltEpsilon) { // Reset the simplex. Count = 0; } } // If the cache is empty or invalid ... if (Count == 0) { SimplexVertex *v = vertices[0]; v->IndexA = 0; v->IndexB = 0; Vec2 wALocal = shapeA.GetVertex(0); Vec2 wBLocal = shapeB.GetVertex(0); v->Wa = Math.Mul(transformA, wALocal); v->Wb = Math.Mul(transformB, wBLocal); v->W = v->Wb - v->Wa; Count = 1; } } }
/// <summary> /// Creates the def /// </summary> /// <param name="def">The def</param> /// <returns>The joint</returns> internal static Joint Create(JointDef def) { Joint joint = null; switch (def.Type) { case JointType.DistanceJoint: { joint = new DistanceJoint((DistanceJointDef)def); } break; case JointType.MouseJoint: { joint = new MouseJoint((MouseJointDef)def); } break; case JointType.PrismaticJoint: { joint = new PrismaticJoint((PrismaticJointDef)def); } break; case JointType.RevoluteJoint: { joint = new RevoluteJoint((RevoluteJointDef)def); } break; case JointType.PulleyJoint: { joint = new PulleyJoint((PulleyJointDef)def); } break; case JointType.GearJoint: { joint = new GearJoint((GearJointDef)def); } break; case JointType.LineJoint: { joint = new LineJoint((LineJointDef)def); } break; default: Box2DxDebug.Assert(false); break; } return(joint); }
/// <summary> /// Copy vertices. This assumes the vertices define a convex polygon. /// It is assumed that the exterior is the the right of each edge. /// </summary> public void Set(Vec2[] vertices, int count) { Box2DxDebug.Assert(3 <= count && count <= Settings.MaxPolygonVertices); VertexCount = count; int i; // Copy vertices. for (i = 0; i < VertexCount; ++i) { Vertices[i] = vertices[i]; } // Compute normals. Ensure the edges have non-zero length. for (i = 0; i < VertexCount; ++i) { int i1 = i; int i2 = i + 1 < count ? i + 1 : 0; Vec2 edge = Vertices[i2] - Vertices[i1]; Box2DxDebug.Assert(edge.LengthSquared() > Settings.FltEpsilonSquared); Normals[i] = Vec2.Cross(edge, 1.0f); Normals[i].Normalize(); } #if DEBUG // Ensure the polygon is convex and the interior // is to the left of each edge. for (i = 0; i < VertexCount; ++i) { int i1 = i; int i2 = i + 1 < count ? i + 1 : 0; Vec2 edge = Vertices[i2] - Vertices[i1]; for (int j = 0; j < VertexCount; ++j) { // Don't check vertices on the current edge. if (j == i1 || j == i2) { continue; } Vec2 r = Vertices[j] - Vertices[i1]; // Your polygon is non-convex (it has an indentation) or // has colinear edges. float s = Vec2.Cross(edge, r); Box2DxDebug.Assert(s > 0.0f); } } #endif // Compute the polygon centroid. Centroid = ComputeCentroid(Vertices, VertexCount); }
/// <summary> /// Removes a body from the controller list. /// </summary> public void RemoveBody(Body body) { //Assert that the controller is not empty Box2DxDebug.Assert(BodyCount > 0); //Find the corresponding edge ControllerEdge edge = BodyList; while (edge != null && edge.Body != body) { edge = edge.NextBody; } //Assert that we are removing a body that is currently attached to the controller Box2DxDebug.Assert(edge != null); //Remove edge from controller list if (edge.PrevBody != null) { edge.PrevBody.NextBody = edge.NextBody; } if (edge.NextBody != null) { edge.NextBody.PrevBody = edge.PrevBody; } if (edge == BodyList) { BodyList = edge.NextBody; } --BodyCount; //Remove edge from body list if (edge.PrevController != null) { edge.PrevController.NextController = edge.NextController; } if (edge.NextController != null) { edge.NextController.PrevController = edge.PrevController; } if (edge == body.ControllerList) { body.ControllerList = edge.NextController; } //Free the edge edge = null; }
/// <summary> /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. /// </summary> public Vec3 Solve33(Vec3 b) { float det = Vec3.Dot(Col1, Vec3.Cross(Col2, Col3)); Box2DxDebug.Assert(det != 0.0f); det = 1.0f / det; Vec3 x = new Vec3(); x.X = det * Vec3.Dot(b, Vec3.Cross(Col2, Col3)); x.Y = det * Vec3.Dot(Col1, Vec3.Cross(b, Col3)); x.Z = det * Vec3.Dot(Col1, Vec3.Cross(Col2, b)); return(x); }
/// <summary> /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. Solve only the upper /// 2-by-2 matrix equation. /// </summary> public Vec2 Solve22(Vec2 b) { float a11 = Col1.X, a12 = Col2.X, a21 = Col1.Y, a22 = Col2.Y; float det = a11 * a22 - a12 * a21; Box2DxDebug.Assert(det != 0.0f); det = 1.0f / det; Vec2 x = new Vec2(); x.X = det * (a22 * b.X - a12 * b.Y); x.Y = det * (a11 * b.Y - a21 * b.X); return(x); }
// TODO_ERIN adjust linear velocity and torque to account for movement of center. /// <summary> /// Set the mass properties. Note that this changes the center of mass position. /// If you are not sure how to compute mass properties, use SetMassFromShapes. /// The inertia tensor is assumed to be relative to the center of mass. /// </summary> /// <param name="massData">The mass properties.</param> public void SetMass(MassData massData) { Box2DxDebug.Assert(world.Lock == false); if (world.Lock) { return; } InvMass = 0.0f; I = 0.0f; InvI = 0.0f; Mass = massData.Mass; if (Mass > 0.0f) { InvMass = 1.0f / Mass; } I = massData.I; if (I > 0.0f && (Flags & BodyFlags.FixedRotation) == 0) { InvI = 1.0f / I; } // Move center of mass. Sweep.LocalCenter = massData.Center; Sweep.C0 = Sweep.C = Math.Mul(Xf, Sweep.LocalCenter); BodyType oldType = type; if (InvMass == 0.0f && InvI == 0.0f) { type = BodyType.Static; } else { type = BodyType.Dynamic; } // If the body type changed, we need to refilter the broad-phase proxies. if (oldType != type) { for (Fixture f = FixtureList; f != null; f = f.Next) { f.RefilterProxy(world.BroadPhase, Xf); } } }
/// <summary> /// Computes the sweep radius using the specified pivot /// </summary> /// <param name="pivot">The pivot</param> /// <returns>The float</returns> public override float ComputeSweepRadius(Vec2 pivot) { int vCount = VertexCount; Box2DxDebug.Assert(vCount > 0); float sr = Vec2.DistanceSquared(Vertices[0], pivot); for (int i = 1; i < vCount; ++i) { sr = Math.Max(sr, Vec2.DistanceSquared(Vertices[i], pivot)); } return(Math.Sqrt(sr)); }