/// <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));
}
