/// <summary>
/// Finds the proxy id 1
/// </summary>
/// <param name="proxyId1">The proxy id</param>
/// <param name="proxyId2">The proxy id</param>
/// <param name="hash">The hash</param>
/// <returns>The pair</returns>
private Pair Find(int proxyId1, int proxyId2, uint hash)
{
int index = HashTable[hash];
while (index != NullPair && Equals(Pairs[index], proxyId1, proxyId2) == false)
{
index = Pairs[index].Next;
}
if (index == NullPair)
{
return(null);
}
Box2DxDebug.Assert(index < Settings.MaxPairs);
return(Pairs[index]);
}
/// <summary>
/// Destroys the contact
/// </summary>
/// <param name="contact">The contact</param>
public static void Destroy(ref Contact contact)
{
Box2DxDebug.Assert(SInitialized);
if (contact.Manifold.PointCount > 0)
{
contact.FixtureA.Body.WakeUp();
contact.FixtureB.Body.WakeUp();
}
ShapeType typeA = contact.FixtureA.ShapeType;
ShapeType typeB = contact.FixtureB.ShapeType;
Box2DxDebug.Assert(ShapeType.UnknownShape < typeA && typeA < ShapeType.ShapeTypeCount);
Box2DxDebug.Assert(ShapeType.UnknownShape < typeB && typeB < ShapeType.ShapeTypeCount);
ContactDestroyFcn destroyFcn = SRegisters[(int)typeA][(int)typeB].DestroyFcn;
destroyFcn(ref contact);
}
/// <summary>
/// Initializes a new instance of the <see cref="PulleyJoint" /> class
/// </summary>
/// <param name="def">The def</param>
public PulleyJoint(PulleyJointDef def)
: base(def)
{
Ground = Body1.GetWorld().GetGroundBody();
GroundAnchor1 = def.GroundAnchor1 - Ground.GetXForm().Position;
GroundAnchor2 = def.GroundAnchor2 - Ground.GetXForm().Position;
LocalAnchor1 = def.LocalAnchor1;
LocalAnchor2 = def.LocalAnchor2;
Box2DxDebug.Assert(def.Ratio != 0.0f);
Ratio = def.Ratio;
Constant = def.Length1 + Ratio * def.Length2;
MaxLength1 = Math.Min(def.MaxLength1, Constant - Ratio * MinPulleyLength);
MaxLength2 = Math.Min(def.MaxLength2, (Constant - MinPulleyLength) / Ratio);
Impulse = 0.0f;
LimitImpulse1 = 0.0f;
LimitImpulse2 = 0.0f;
}
/// <summary>
/// Destroy a fixture. This removes the fixture from the broad-phase and
/// therefore destroys any contacts associated with this fixture. All fixtures
/// attached to a body are implicitly destroyed when the body is destroyed.
/// @warning This function is locked during callbacks.
/// </summary>
/// <param name="fixture">The fixture to be removed.</param>
public void DestroyFixture(Fixture fixture)
{
Box2DxDebug.Assert(world.Lock == false);
if (world.Lock)
{
return;
}
Box2DxDebug.Assert(fixture.Body == this);
// Remove the fixture from this body's singly linked list.
Box2DxDebug.Assert(FixtureCount > 0);
Fixture node = FixtureList;
bool found = false;
while (node != null)
{
if (node == fixture)
{
//*node = fixture->m_next;
FixtureList = fixture.Next;
found = true;
break;
}
node = node.Next;
}
// You tried to remove a shape that is not attached to this body.
Box2DxDebug.Assert(found);
BroadPhase broadPhase = world.BroadPhase;
fixture.Destroy(broadPhase);
fixture.Body = null;
fixture.Next = null;
--FixtureCount;
}
/// <summary>
/// Creates a fixture and attach it to this body.
/// @warning This function is locked during callbacks.
/// </summary>
/// <param name="def">The fixture definition.</param>
public Fixture CreateFixture(FixtureDef def)
{
Box2DxDebug.Assert(world.Lock == false);
if (world.Lock)
{
return(null);
}
BroadPhase broadPhase = world.BroadPhase;
Fixture fixture = new Fixture();
fixture.Create(broadPhase, this, Xf, def);
fixture.Next = FixtureList;
FixtureList = fixture;
++FixtureCount;
fixture.Body = this;
return(fixture);
}
/// <summary>
/// Describes whether this instance test overlap
/// </summary>
/// <param name="b">The </param>
/// <param name="p">The </param>
/// <returns>The bool</returns>
internal bool TestOverlap(BoundValues b, Proxy p)
{
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = Bounds[axis];
Box2DxDebug.Assert(p.LowerBounds[axis] < 2 * ProxyCount);
Box2DxDebug.Assert(p.UpperBounds[axis] < 2 * ProxyCount);
if (b.LowerValues[axis] > bounds[p.UpperBounds[axis]].Value)
{
return(false);
}
if (b.UpperValues[axis] < bounds[p.LowerBounds[axis]].Value)
{
return(false);
}
}
return(true);
}
/// <summary>
/// Adds the type using the specified create fcn
/// </summary>
/// <param name="createFcn">The create fcn</param>
/// <param name="contactDestroyFcn">The destory fcn</param>
/// <param name="type1">The type</param>
/// <param name="type2">The type</param>
public static void AddType(ContactCreateFcn createFcn, ContactDestroyFcn contactDestroyFcn,
ShapeType type1, ShapeType type2)
{
Box2DxDebug.Assert(ShapeType.UnknownShape < type1 && type1 < ShapeType.ShapeTypeCount);
Box2DxDebug.Assert(ShapeType.UnknownShape < type2 && type2 < ShapeType.ShapeTypeCount);
if (SRegisters[(int)type1] == null)
{
SRegisters[(int)type1] = new ContactRegister[(int)ShapeType.ShapeTypeCount];
}
SRegisters[(int)type1][(int)type2].CreateFcn = createFcn;
SRegisters[(int)type1][(int)type2].DestroyFcn = contactDestroyFcn;
SRegisters[(int)type1][(int)type2].Primary = true;
if (type1 != type2)
{
SRegisters[(int)type2][(int)type1].CreateFcn = createFcn;
SRegisters[(int)type2][(int)type1].DestroyFcn = contactDestroyFcn;
SRegisters[(int)type2][(int)type1].Primary = false;
}
}
/// <summary>
/// Computes the bounds using the specified lower values
/// </summary>
/// <param name="lowerValues">The lower values</param>
/// <param name="upperValues">The upper values</param>
/// <param name="aabb">The aabb</param>
private void ComputeBounds(out ushort[] lowerValues, out ushort[] upperValues, Aabb aabb)
{
lowerValues = new ushort[2];
upperValues = new ushort[2];
Box2DxDebug.Assert(aabb.UpperBound.X >= aabb.LowerBound.X);
Box2DxDebug.Assert(aabb.UpperBound.Y >= aabb.LowerBound.Y);
Vec2 minVertex = Math.Clamp(aabb.LowerBound, WorldAabb.LowerBound, WorldAabb.UpperBound);
Vec2 maxVertex = Math.Clamp(aabb.UpperBound, WorldAabb.LowerBound, WorldAabb.UpperBound);
// Bump lower bounds downs and upper bounds up. This ensures correct sorting of
// lower/upper bounds that would have equal values.
// TODO_ERIN implement fast float to uint16 conversion.
lowerValues[0] = (ushort)((ushort)(QuantizationFactor.X * (minVertex.X - WorldAabb.LowerBound.X)) &
(BroadphaseMax - 1));
upperValues[0] = (ushort)((ushort)(QuantizationFactor.X * (maxVertex.X - WorldAabb.LowerBound.X)) | 1);
lowerValues[1] = (ushort)((ushort)(QuantizationFactor.Y * (minVertex.Y - WorldAabb.LowerBound.Y)) &
(BroadphaseMax - 1));
upperValues[1] = (ushort)((ushort)(QuantizationFactor.Y * (maxVertex.Y - WorldAabb.LowerBound.Y)) | 1);
}
/// <summary>
/// The value
/// </summary>
public byte this[int index]
{
get
{
#if DEBUG
Box2DxDebug.Assert(index >= 0 && index < 3);
#endif
if (index == 0)
{
return(i0);
}
if (index == 1)
{
return(i1);
}
return(i2);
}
set
{
#if DEBUG
Box2DxDebug.Assert(index >= 0 && index < 3);
#endif
if (index == 0)
{
i0 = value;
}
else if (index == 1)
{
i1 = value;
}
else
{
i2 = value;
}
}
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.Vertices;
Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vec2[] vertices2 = poly2.Vertices;
Box2DxDebug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vec2 normal1World = Math.Mul(xf1.R, normals1[edge1]);
Vec2 normal1 = Math.MulT(xf2.R, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Settings.FltMax;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vec2 v1 = Math.Mul(xf1, vertices1[edge1]);
Vec2 v2 = Math.Mul(xf2, vertices2[index]);
float separation = Vec2.Dot(v2 - v1, normal1World);
return(separation);
}
/// <summary>
/// Initializes a new instance of the <see cref="PairManager" /> class
/// </summary>
public PairManager()
{
Box2DxDebug.Assert(Math.IsPowerOfTwo((uint)TableCapacity));
Box2DxDebug.Assert(TableCapacity >= Settings.MaxPairs);
for (int i = 0; i < TableCapacity; ++i)
{
HashTable[i] = NullPair;
}
FreePair = 0;
for (int i = 0; i < Settings.MaxPairs; ++i)
{
Pairs[i] = new Pair(); //todo: need some pool here
Pairs[i].ProxyId1 = NullProxy;
Pairs[i].ProxyId2 = NullProxy;
Pairs[i].UserData = null;
Pairs[i].Status = 0;
Pairs[i].Next = (ushort)(i + 1U);
}
Pairs[Settings.MaxPairs - 1].Next = NullPair;
PairCount = 0;
PairBufferCount = 0;
}
// This one is only used for validation.
/// <summary>
/// Describes whether this instance test overlap
/// </summary>
/// <param name="p1">The </param>
/// <param name="p2">The </param>
/// <returns>The bool</returns>
internal bool TestOverlap(Proxy p1, Proxy p2)
{
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = Bounds[axis];
Box2DxDebug.Assert(p1.LowerBounds[axis] < 2 * ProxyCount);
Box2DxDebug.Assert(p1.UpperBounds[axis] < 2 * ProxyCount);
Box2DxDebug.Assert(p2.LowerBounds[axis] < 2 * ProxyCount);
Box2DxDebug.Assert(p2.UpperBounds[axis] < 2 * ProxyCount);
if (bounds[p1.LowerBounds[axis]].Value > bounds[p2.UpperBounds[axis]].Value)
{
return(false);
}
if (bounds[p1.UpperBounds[axis]].Value < bounds[p2.LowerBounds[axis]].Value)
{
return(false);
}
}
return(true);
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
public void Initialize(Body body1, Body body2,
Vec2 groundAnchor1, Vec2 groundAnchor2,
Vec2 anchor1, Vec2 anchor2,
float ratio)
{
Body1 = body1;
Body2 = body2;
GroundAnchor1 = groundAnchor1;
GroundAnchor2 = groundAnchor2;
LocalAnchor1 = body1.GetLocalPoint(anchor1);
LocalAnchor2 = body2.GetLocalPoint(anchor2);
Vec2 d1 = anchor1 - groundAnchor1;
Length1 = d1.Length();
Vec2 d2 = anchor2 - groundAnchor2;
Length2 = d2.Length();
Ratio = ratio;
Box2DxDebug.Assert(ratio > Settings.FltEpsilon);
float c = Length1 + ratio * Length2;
MaxLength1 = c - ratio * PulleyJoint.MinPulleyLength;
MaxLength2 = (c - PulleyJoint.MinPulleyLength) / ratio;
}
/// <summary>
/// Get a vertex by index. Used by Distance.
/// </summary>
public override Vec2 GetVertex(int index)
{
Box2DxDebug.Assert(index == 0);
return(Position);
}
// TODO_ERIN adjust linear velocity and torque to account for movement of center.
/// <summary>
/// Compute the mass properties from the attached shapes. You typically call this
/// after adding all the shapes. If you add or remove shapes later, you may want
/// to call this again. Note that this changes the center of mass position.
/// </summary>
public void SetMassFromShapes()
{
Box2DxDebug.Assert(world.Lock == false);
if (world.Lock)
{
return;
}
// Compute mass data from shapes. Each shape has its own density.
Mass = 0.0f;
InvMass = 0.0f;
I = 0.0f;
InvI = 0.0f;
Vec2 center = Vec2.Zero;
for (Fixture f = FixtureList; f != null; f = f.Next)
{
MassData massData;
f.ComputeMass(out massData);
Mass += massData.Mass;
center += massData.Mass * massData.Center;
I += massData.I;
}
// Compute center of mass, and shift the origin to the COM.
if (Mass > 0.0f)
{
InvMass = 1.0f / Mass;
center *= InvMass;
}
if (I > 0.0f && (Flags & BodyFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
I -= Mass * Vec2.Dot(center, center);
Box2DxDebug.Assert(I > 0.0f);
InvI = 1.0f / I;
}
else
{
I = 0.0f;
InvI = 0.0f;
}
// Move center of mass.
Sweep.LocalCenter = 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>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body1 = Body1;
Body body2 = Body2;
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body1SweepC = body1.Sweep.C + mulR1;
Vec2 body2SweepC = body2.Sweep.C + mulR2;
Vec2 groundAnchor1 = Ground.GetXForm().Position + GroundAnchor1;
Vec2 groundAnchor2 = Ground.GetXForm().Position + GroundAnchor2;
// Get the pulley axes.
U1 = body1SweepC - groundAnchor1;
U2 = body2SweepC - groundAnchor2;
float length1 = U1.Length();
float length2 = U2.Length();
if (length1 > Settings.LinearSlop)
{
U1 *= 1.0f / length1;
}
else
{
U1.SetZero();
}
if (length2 > Settings.LinearSlop)
{
U2 *= 1.0f / length2;
}
else
{
U2.SetZero();
}
float c = Constant - length1 - Ratio * length2;
if (c > 0.0f)
{
State = LimitState.InactiveLimit;
Impulse = 0.0f;
}
else
{
State = LimitState.AtUpperLimit;
}
if (length1 < MaxLength1)
{
LimitState1 = LimitState.InactiveLimit;
LimitImpulse1 = 0.0f;
}
else
{
LimitState1 = LimitState.AtUpperLimit;
}
if (length2 < MaxLength2)
{
LimitState2 = LimitState.InactiveLimit;
LimitImpulse2 = 0.0f;
}
else
{
LimitState2 = LimitState.AtUpperLimit;
}
// Compute effective mass.
float cr1U1 = Vec2.Cross(mulR1, U1);
float cr2U2 = Vec2.Cross(mulR2, U2);
LimitMass1 = body1.InvMass + body1.InvI * cr1U1 * cr1U1;
LimitMass2 = body2.InvMass + body2.InvI * cr2U2 * cr2U2;
PulleyMass = LimitMass1 + Ratio * Ratio * LimitMass2;
Box2DxDebug.Assert(LimitMass1 > Settings.FltEpsilon);
Box2DxDebug.Assert(LimitMass2 > Settings.FltEpsilon);
Box2DxDebug.Assert(PulleyMass > Settings.FltEpsilon);
LimitMass1 = 1.0f / LimitMass1;
LimitMass2 = 1.0f / LimitMass2;
PulleyMass = 1.0f / PulleyMass;
if (step.WarmStarting)
{
// Scale impulses to support variable time steps.
Impulse *= step.DtRatio;
LimitImpulse1 *= step.DtRatio;
LimitImpulse2 *= step.DtRatio;
// Warm starting.
Vec2 p1 = -(Impulse + LimitImpulse1) * U1;
Vec2 p2 = (-Ratio * Impulse - LimitImpulse2) * U2;
body1.LinearVelocity += body1.InvMass * p1;
body1.AngularVelocity += body1.InvI * Vec2.Cross(mulR1, p1);
body2.LinearVelocity += body2.InvMass * p2;
body2.AngularVelocity += body2.InvI * Vec2.Cross(mulR2, p2);
}
else
{
Impulse = 0.0f;
LimitImpulse1 = 0.0f;
LimitImpulse2 = 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 b1 = Body1;
Body b2 = Body2;
// You cannot create a prismatic joint between bodies that
// both have fixed rotation.
Box2DxDebug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);
LocalCenter1 = b1.GetLocalCenter();
LocalCenter2 = b2.GetLocalCenter();
XForm xf1 = b1.GetXForm();
XForm xf2 = b2.GetXForm();
// Compute the effective masses.
Vec2 r1 = Box2DXMath.Mul(xf1.R, LocalAnchor1 - LocalCenter1);
Vec2 r2 = Box2DXMath.Mul(xf2.R, LocalAnchor2 - LocalCenter2);
Vec2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
InvMass1 = b1.InvMass;
InvI1 = b1.InvI;
InvMass2 = b2.InvMass;
InvI2 = b2.InvI;
// Compute motor Jacobian and effective mass.
{
Axis = Box2DXMath.Mul(xf1.R, LocalXAxis1);
a1 = Vec2.Cross(d + r1, Axis);
A2 = Vec2.Cross(r2, Axis);
MotorMass = InvMass1 + InvMass2 + InvI1 * a1 * a1 + InvI2 * A2 * A2;
Box2DxDebug.Assert(MotorMass > Settings.FltEpsilon);
MotorMass = 1.0f / MotorMass;
}
// Prismatic constraint.
{
Perp = Box2DXMath.Mul(xf1.R, LocalYAxis1);
s1 = Vec2.Cross(d + r1, Perp);
s2 = Vec2.Cross(r2, Perp);
float m1 = InvMass1, m2 = InvMass2;
float i1 = InvI1, i2 = InvI2;
float k11 = m1 + m2 + i1 * s1 * s1 + i2 * s2 * s2;
float k12 = i1 * s1 + i2 * s2;
float k13 = i1 * s1 * a1 + i2 * s2 * A2;
float k22 = i1 + i2;
float k23 = i1 * a1 + i2 * A2;
float k33 = m1 + m2 + i1 * a1 * a1 + i2 * A2 * A2;
K.Col1.Set(k11, k12, k13);
K.Col2.Set(k12, k22, k23);
K.Col3.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (IsLimitEnabled)
{
float jointTranslation = Vec2.Dot(Axis, d);
if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
{
LimitState = LimitState.EqualLimits;
}
else if (jointTranslation <= LowerLimit)
{
if (LimitState != LimitState.AtLowerLimit)
{
LimitState = LimitState.AtLowerLimit;
Impulse.Z = 0.0f;
}
}
else if (jointTranslation >= UpperLimit)
{
if (LimitState != LimitState.AtUpperLimit)
{
LimitState = LimitState.AtUpperLimit;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.InactiveLimit;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.InactiveLimit;
}
if (IsMotorEnabled == false)
{
MotorForce = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
Impulse *= step.DtRatio;
MotorForce *= step.DtRatio;
Vec2 p = Impulse.X * Perp + (MotorForce + Impulse.Z) * Axis;
float l1 = Impulse.X * s1 + Impulse.Y + (MotorForce + Impulse.Z) * a1;
float l2 = Impulse.X * s2 + Impulse.Y + (MotorForce + Impulse.Z) * A2;
b1.LinearVelocity -= InvMass1 * p;
b1.AngularVelocity -= InvI1 * l1;
b2.LinearVelocity += InvMass2 * p;
b2.AngularVelocity += InvI2 * l2;
}
else
{
Impulse.SetZero();
MotorForce = 0.0f;
}
}
/// <summary>
/// Compute the closest points between two shapes. Supports any combination of:
/// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output.
/// On the first call set SimplexCache.Count to zero.
/// </summary>
public static unsafe void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input,
Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
XForm transformA = input.TransformA;
XForm transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
// Get simplex vertices as an array.
SimplexVertex *vertices = &simplex.V1;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
int lastCount;
// Main iteration loop.
int iter = 0;
const int kMaxIterationCount = 20;
while (iter < kMaxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex.Count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].IndexA;
lastB[i] = vertices[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DxDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Compute closest point.
Vec2 p = simplex.GetClosestPoint();
float distanceSqr = p.LengthSquared();
// Ensure the search direction is numerically fit.
if (distanceSqr < Settings.FltEpsilonSquared)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex *vertex = vertices + simplex.Count;
vertex->IndexA = shapeA.GetSupport(Math.MulT(transformA.R, p));
vertex->Wa = Math.Mul(transformA, shapeA.GetVertex(vertex->IndexA));
//Vec2 wBLocal;
vertex->IndexB = shapeB.GetSupport(Math.MulT(transformB.R, -p));
vertex->Wb = Math.Mul(transformB, shapeB.GetVertex(vertex->IndexB));
vertex->W = vertex->Wb - vertex->Wa;
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
float lowerBound = Vec2.Dot(p, vertex->W);
float upperBound = distanceSqr;
const float kRelativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= kRelativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
if (vertex->IndexA == lastA[i] && vertex->IndexB == lastB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vec2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA.Radius;
float rB = shapeB.Radius;
if (output.Distance > rA + rB && output.Distance > Settings.FltEpsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vec2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vec2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 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 b1 = Body1;
Body b2 = Body2;
LocalCenter1 = b1.GetLocalCenter();
LocalCenter2 = b2.GetLocalCenter();
XForm xf1 = b1.GetXForm();
XForm xf2 = b2.GetXForm();
// Compute the effective masses.
Vec2 r1 = Math.Mul(xf1.R, LocalAnchor1 - LocalCenter1);
Vec2 r2 = Math.Mul(xf2.R, LocalAnchor2 - LocalCenter2);
Vec2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
InvMass1 = b1.InvMass;
InvI1 = b1.InvI;
InvMass2 = b2.InvMass;
InvI2 = b2.InvI;
// Compute motor Jacobian and effective mass.
{
Axis = Math.Mul(xf1.R, LocalXAxis1);
A1 = Vec2.Cross(d + r1, Axis);
A2 = Vec2.Cross(r2, Axis);
MotorMass = InvMass1 + InvMass2 + InvI1 * A1 * A1 + InvI2 * A2 * A2;
Box2DxDebug.Assert(MotorMass > Settings.FltEpsilon);
MotorMass = 1.0f / MotorMass;
}
// Prismatic constraint.
{
Perp = Math.Mul(xf1.R, LocalYAxis1);
S1 = Vec2.Cross(d + r1, Perp);
s2 = Vec2.Cross(r2, Perp);
float m1 = InvMass1, m2 = InvMass2;
float i1 = InvI1, i2 = InvI2;
float k11 = m1 + m2 + i1 * S1 * S1 + i2 * s2 * s2;
float k12 = i1 * S1 * A1 + i2 * s2 * A2;
float k22 = m1 + m2 + i1 * A1 * A1 + i2 * A2 * A2;
K.Col1.Set(k11, k12);
K.Col2.Set(k12, k22);
}
// Compute motor and limit terms.
if (EnableLimitx)
{
float jointTranslation = Vec2.Dot(Axis, d);
if (Math.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LinearSlop)
{
LimitState = LimitState.EqualLimits;
}
else if (jointTranslation <= LowerTranslation)
{
if (LimitState != LimitState.AtLowerLimit)
{
LimitState = LimitState.AtLowerLimit;
Impulse.Y = 0.0f;
}
}
else if (jointTranslation >= UpperTranslation)
{
if (LimitState != LimitState.AtUpperLimit)
{
LimitState = LimitState.AtUpperLimit;
Impulse.Y = 0.0f;
}
}
else
{
LimitState = LimitState.InactiveLimit;
Impulse.Y = 0.0f;
}
}
else
{
LimitState = LimitState.InactiveLimit;
}
if (EnableMotorx == false)
{
MotorImpulse = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
Impulse *= step.DtRatio;
MotorImpulse *= step.DtRatio;
Vec2 p = Impulse.X * Perp + (MotorImpulse + Impulse.Y) * Axis;
float l1 = Impulse.X * S1 + (MotorImpulse + Impulse.Y) * A1;
float l2 = Impulse.X * s2 + (MotorImpulse + Impulse.Y) * A2;
b1.LinearVelocity -= InvMass1 * p;
b1.AngularVelocity -= InvI1 * l1;
b2.LinearVelocity += InvMass2 * p;
b2.AngularVelocity += InvI2 * l2;
}
else
{
Impulse.SetZero();
MotorImpulse = 0.0f;
}
}
// CCD via the secant method.
/// <summary>
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// Warning the sweeps must have the same time interval.
/// </summary>
/// <returns>
/// The fraction between [0,1] in which the shapes first touch.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </returns>
public static float TimeOfImpact(ToiInput input, Shape shapeA, Shape shapeB)
{
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DxDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DxDebug.Assert(1.0f - sweepA.T0 > Settings.FltEpsilon);
float radius = shapeA.Radius + shapeB.Radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int kMaxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput;
distanceInput.UseRadii = false;
for (;;)
{
XForm xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
unsafe
{
fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
}
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
}
break;
}
#if _FALSE
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N + 1];
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body1 = Body1;
Body body2 = Body2;
if (IsMotorEnabled || IsLimitEnabled)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DxDebug.Assert(body1.InvI > 0.0f || body2.InvI > 0.0f);
}
// Compute the effective mass matrix.
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = body1.InvMass, m2 = body2.InvMass;
float i1 = body1.InvI, i2 = body2.InvI;
float col1X = m1 + m2 + mulR1.Y * mulR1.Y * i1 + mulR2.Y * mulR2.Y * i2;
float col2X = -mulR1.Y * mulR1.X * i1 - mulR2.Y * mulR2.X * i2;
float col3X = -mulR1.Y * i1 - mulR2.Y * i2;
float col1Y = Mass.Col2.X;
float col2Y = m1 + m2 + mulR1.X * mulR1.X * i1 + mulR2.X * mulR2.X * i2;
float col3Y = mulR1.X * i1 + mulR2.X * i2;
float col1Z = Mass.Col3.X;
float col2Z = Mass.Col3.Y;
float col3Z = i1 + i2;
Mass = new Mat33(new Vec3(col1X, col1Y, col1Z), new Vec3(col2X, col2Y, col2Z),
new Vec3(col3X, col3Y, col3Z));
/*
* _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
* _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
* _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
* _mass.Col1.Y = _mass.Col2.X;
* _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
* _mass.Col3.Y = r1.X * i1 + r2.X * i2;
* _mass.Col1.Z = _mass.Col3.X;
* _mass.Col2.Z = _mass.Col3.Y;
* _mass.Col3.Z = i1 + i2;
*/
MotorMass = 1.0f / (i1 + i2);
if (IsMotorEnabled == false)
{
MotorTorque = 0.0f;
}
if (IsLimitEnabled)
{
float jointAngle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.AngularSlop)
{
State = LimitState.EqualLimits;
}
else if (jointAngle <= LowerLimit)
{
if (State != LimitState.AtLowerLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtLowerLimit;
}
else if (jointAngle >= UpperLimit)
{
if (State != LimitState.AtUpperLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtUpperLimit;
}
else
{
State = LimitState.InactiveLimit;
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
}
else
{
State = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
Impulse *= step.DtRatio;
MotorTorque *= step.DtRatio;
Vec2 p = new Vec2(Impulse.X, Impulse.Y);
body1.LinearVelocity -= m1 * p;
body1.AngularVelocity -= i1 * (Vec2.Cross(mulR1, p) + MotorTorque + Impulse.Z);
body2.LinearVelocity += m2 * p;
body2.AngularVelocity += i2 * (Vec2.Cross(mulR2, p) + MotorTorque + Impulse.Z);
}
else
{
Impulse.SetZero();
MotorTorque = 0.0f;
}
}
// Call MoveProxy as many times as you like, then when you are done
// call Commit to finalized the proxy pairs (for your time step).
/// <summary>
/// Moves the proxy using the specified proxy id
/// </summary>
/// <param name="proxyId">The proxy id</param>
/// <param name="aabb">The aabb</param>
public void MoveProxy(int proxyId, Aabb aabb)
{
if (proxyId == PairManager.NullProxy || Settings.MaxProxies <= proxyId)
{
Box2DxDebug.Assert(false);
return;
}
if (aabb.IsValid == false)
{
Box2DxDebug.Assert(false);
return;
}
int boundCount = 2 * ProxyCount;
Proxy proxy1 = ProxyPool[proxyId];
// Get new bound values
BoundValues newValues = new BoundValues();
ComputeBounds(out newValues.LowerValues, out newValues.UpperValues, aabb);
// Get old bound values
BoundValues oldValues = new BoundValues();
for (int axis = 0; axis < 2; ++axis)
{
oldValues.LowerValues[axis] = Bounds[axis][proxy1.LowerBounds[axis]].Value;
oldValues.UpperValues[axis] = Bounds[axis][proxy1.UpperBounds[axis]].Value;
}
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = Bounds[axis];
int lowerIndex = proxy1.LowerBounds[axis];
int upperIndex = proxy1.UpperBounds[axis];
ushort lowerValue = newValues.LowerValues[axis];
ushort upperValue = newValues.UpperValues[axis];
int deltaLower = lowerValue - bounds[lowerIndex].Value;
int deltaUpper = upperValue - bounds[upperIndex].Value;
bounds[lowerIndex].Value = lowerValue;
bounds[upperIndex].Value = upperValue;
//
// Expanding adds overlaps
//
// Should we move the lower bound down?
if (deltaLower < 0)
{
int index = lowerIndex;
while (index > 0 && lowerValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = ProxyPool[prevBound.ProxyId];
++prevBound.StabbingCount;
if (prevBound.IsUpper)
{
if (TestOverlap(newValues, prevProxy))
{
PairManager.AddBufferedPair(proxyId, prevProxyId);
}
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
else
{
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
--proxy1.LowerBounds[axis];
Math.Swap(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
// Should we move the upper bound up?
if (deltaUpper > 0)
{
int index = upperIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= upperValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = ProxyPool[nextProxyId];
++nextBound.StabbingCount;
if (nextBound.IsLower)
{
if (TestOverlap(newValues, nextProxy))
{
PairManager.AddBufferedPair(proxyId, nextProxyId);
}
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
else
{
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
++proxy1.UpperBounds[axis];
Math.Swap(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
//
// Shrinking removes overlaps
//
// Should we move the lower bound up?
if (deltaLower > 0)
{
int index = lowerIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= lowerValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = ProxyPool[nextProxyId];
--nextBound.StabbingCount;
if (nextBound.IsUpper)
{
if (TestOverlap(oldValues, nextProxy))
{
PairManager.RemoveBufferedPair(proxyId, nextProxyId);
}
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
else
{
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
++proxy1.LowerBounds[axis];
Math.Swap(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
// Should we move the upper bound down?
if (deltaUpper < 0)
{
int index = upperIndex;
while (index > 0 && upperValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = ProxyPool[prevProxyId];
--prevBound.StabbingCount;
if (prevBound.IsLower)
{
if (TestOverlap(oldValues, prevProxy))
{
PairManager.RemoveBufferedPair(proxyId, prevProxyId);
}
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
else
{
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
--proxy1.UpperBounds[axis];
Math.Swap(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
}
if (IsValidate)
{
Validate();
}
}
// Create and destroy proxies. These call Flush first.
/// <summary>
/// Creates the proxy using the specified aabb
/// </summary>
/// <param name="aabb">The aabb</param>
/// <param name="userData">The user data</param>
/// <returns>The proxy id</returns>
public ushort CreateProxy(Aabb aabb, object userData)
{
Box2DxDebug.Assert(ProxyCount < Settings.MaxProxies);
Box2DxDebug.Assert(freeProxy != PairManager.NullProxy);
ushort proxyId = freeProxy;
Proxy proxy1 = ProxyPool[proxyId];
freeProxy = proxy1.Next;
proxy1.OverlapCount = 0;
proxy1.UserData = userData;
int boundCount = 2 * ProxyCount;
ushort[] lowerValues;
ushort[] upperValues;
ComputeBounds(out lowerValues, out upperValues, aabb);
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = Bounds[axis];
int lowerIndex, upperIndex;
Query(out lowerIndex, out upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
Bound[] tmp = new Bound[boundCount - upperIndex];
for (int i = 0; i < boundCount - upperIndex; i++)
{
tmp[i] = bounds[upperIndex + i].Clone();
}
for (int i = 0; i < boundCount - upperIndex; i++)
{
bounds[upperIndex + 2 + i] = tmp[i];
}
tmp = new Bound[upperIndex - lowerIndex];
for (int i = 0; i < upperIndex - lowerIndex; i++)
{
tmp[i] = bounds[lowerIndex + i].Clone();
}
for (int i = 0; i < upperIndex - lowerIndex; i++)
{
bounds[lowerIndex + 1 + i] = tmp[i];
}
// The upper index has increased because of the lower bound insertion.
++upperIndex;
// Copy in the new bounds.
bounds[lowerIndex].Value = lowerValues[axis];
bounds[lowerIndex].ProxyId = proxyId;
bounds[upperIndex].Value = upperValues[axis];
bounds[upperIndex].ProxyId = proxyId;
bounds[lowerIndex].StabbingCount = lowerIndex == 0 ? (ushort)0 : bounds[lowerIndex - 1].StabbingCount;
bounds[upperIndex].StabbingCount = bounds[upperIndex - 1].StabbingCount;
// Adjust the stabbing count between the new bounds.
for (int index = lowerIndex; index < upperIndex; ++index)
{
++bounds[index].StabbingCount;
}
// Adjust the all the affected bound indices.
for (int index = lowerIndex; index < boundCount + 2; ++index)
{
Proxy proxy = ProxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy.LowerBounds[axis] = (ushort)index;
}
else
{
proxy.UpperBounds[axis] = (ushort)index;
}
}
}
++ProxyCount;
Box2DxDebug.Assert(QueryResultCount < Settings.MaxProxies);
// Create pairs if the AABB is in range.
for (int i = 0; i < QueryResultCount; ++i)
{
Box2DxDebug.Assert(QueryResults[i] < Settings.MaxProxies);
Box2DxDebug.Assert(ProxyPool[QueryResults[i]].IsValid);
PairManager.AddBufferedPair(proxyId, QueryResults[i]);
}
PairManager.Commit();
if (IsValidate)
{
Validate();
}
// Prepare for next query.
QueryResultCount = 0;
IncrementTimeStamp();
return(proxyId);
}
/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body body1 = Body1;
Body body2 = Body2;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (IsLimitEnabled && State != LimitState.InactiveLimit)
{
float angle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (State == LimitState.EqualLimits)
{
// Prevent large angular corrections
float c = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -MotorMass * c;
angularError = Box2DXMath.Abs(c);
}
else if (State == LimitState.AtLowerLimit)
{
float c = angle - LowerLimit;
angularError = -c;
// Prevent large angular corrections and allow some slop.
c = Box2DXMath.Clamp(c + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -MotorMass * c;
}
else if (State == LimitState.AtUpperLimit)
{
float c = angle - UpperLimit;
angularError = c;
// Prevent large angular corrections and allow some slop.
c = Box2DXMath.Clamp(c - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -MotorMass * c;
}
body1.Sweep.A -= body1.InvI * limitImpulse;
body2.Sweep.A += body2.InvI * limitImpulse;
body1.SynchronizeTransform();
body2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body2SweepC = body2.Sweep.C + mulR2 - body1.Sweep.C - mulR1;
positionError = body2SweepC.Length();
float invMass1 = body1.InvMass, invMass2 = body2.InvMass;
float invI1 = body1.InvI, invI2 = body2.InvI;
// Handle large detachment.
float kAllowedStretch = 10.0f * Settings.LinearSlop;
if (body2SweepC.LengthSquared() > kAllowedStretch * kAllowedStretch)
{
// Use a particle solution (no rotation).
Vec2 sweepC = body2SweepC;
sweepC.Normalize();
float mass12 = invMass1 + invMass2;
Box2DxDebug.Assert(mass12 > Settings.FltEpsilon);
float divideMass12 = 1.0f / mass12;
Vec2 impulseLocal = divideMass12 * -body2SweepC;
float kBeta = 0.5f;
body1.Sweep.C -= kBeta * invMass1 * impulseLocal;
body2.Sweep.C += kBeta * invMass2 * impulseLocal;
body2SweepC = body2.Sweep.C + mulR2 - body1.Sweep.C - mulR1;
}
Mat22 k1 = new Mat22
{
Col1 = new Vec2(invMass1 + invMass2, 0.0f),
Col2 = new Vec2(0.0f, invMass1 + invMass2)
};
Mat22 k2 = new Mat22();
k2.Col1.X = invI1 * mulR1.Y * mulR1.Y;
k2.Col2.X = -invI1 * mulR1.X * mulR1.Y;
k2.Col1.Y = -invI1 * mulR1.X * mulR1.Y;
k2.Col2.Y = invI1 * mulR1.X * mulR1.X;
Mat22 k3 = new Mat22();
k3.Col1.X = invI2 * mulR2.Y * mulR2.Y;
k3.Col2.X = -invI2 * mulR2.X * mulR2.Y;
k3.Col1.Y = -invI2 * mulR2.X * mulR2.Y;
k3.Col2.Y = invI2 * mulR2.X * mulR2.X;
Mat22 k = k1 + k2 + k3;
Vec2 impulse = k.Solve(-body2SweepC);
body1.Sweep.C -= body1.InvMass * impulse;
body1.Sweep.A -= body1.InvI * Vec2.Cross(mulR1, impulse);
body2.Sweep.C += body2.InvMass * impulse;
body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, impulse);
body1.SynchronizeTransform();
body2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Tests the segment using the specified xf
/// </summary>
/// <param name="xf">The xf</param>
/// <param name="lambda">The lambda</param>
/// <param name="normal">The normal</param>
/// <param name="segment">The segment</param>
/// <param name="maxLambda">The max lambda</param>
/// <returns>The segment collide</returns>
public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment,
float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
float lower = 0.0f, upper = maxLambda;
Vec2 p1 = Math.MulT(xf.R, segment.P1 - xf.Position);
Vec2 p2 = Math.MulT(xf.R, segment.P2 - xf.Position);
Vec2 d = p2 - p1;
int index = -1;
for (int i = 0; i < VertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vec2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vec2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(SegmentCollide.MissCollide);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return(SegmentCollide.MissCollide);
}
}
Box2DxDebug.Assert(0.0f <= lower && lower <= maxLambda);
if (index >= 0)
{
lambda = lower;
normal = Math.Mul(xf.R, Normals[index]);
return(SegmentCollide.HitCollide);
}
lambda = 0f;
return(SegmentCollide.StartInsideCollide);
}
/// <summary>
/// Gets the vertex using the specified index
/// </summary>
/// <param name="index">The index</param>
/// <returns>The vec</returns>
public override Vec2 GetVertex(int index)
{
Box2DxDebug.Assert(0 <= index && index < 2);
return(index == 0 ? Vertex1 : Vertex2);
}
/// <summary>
/// Destroys the proxy using the specified proxy id
/// </summary>
/// <param name="proxyId">The proxy id</param>
public void DestroyProxy(int proxyId)
{
Box2DxDebug.Assert(0 < ProxyCount && ProxyCount <= Settings.MaxProxies);
Proxy proxy1 = ProxyPool[proxyId];
Box2DxDebug.Assert(proxy1.IsValid);
int boundCount = 2 * ProxyCount;
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = Bounds[axis];
int lowerIndex = proxy1.LowerBounds[axis];
int upperIndex = proxy1.UpperBounds[axis];
ushort lowerValue = bounds[lowerIndex].Value;
ushort upperValue = bounds[upperIndex].Value;
Bound[] tmp = new Bound[upperIndex - lowerIndex - 1];
for (int i = 0; i < upperIndex - lowerIndex - 1; i++)
{
tmp[i] = bounds[lowerIndex + 1 + i].Clone();
}
for (int i = 0; i < upperIndex - lowerIndex - 1; i++)
{
bounds[lowerIndex + i] = tmp[i];
}
tmp = new Bound[boundCount - upperIndex - 1];
for (int i = 0; i < boundCount - upperIndex - 1; i++)
{
tmp[i] = bounds[upperIndex + 1 + i].Clone();
}
for (int i = 0; i < boundCount - upperIndex - 1; i++)
{
bounds[upperIndex - 1 + i] = tmp[i];
}
// Fix bound indices.
for (int index = lowerIndex; index < boundCount - 2; ++index)
{
Proxy proxy = ProxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy.LowerBounds[axis] = (ushort)index;
}
else
{
proxy.UpperBounds[axis] = (ushort)index;
}
}
// Fix stabbing count.
for (int index = lowerIndex; index < upperIndex - 1; ++index)
{
--bounds[index].StabbingCount;
}
// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
Query(out lowerIndex, out upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
}
Box2DxDebug.Assert(QueryResultCount < Settings.MaxProxies);
for (int i = 0; i < QueryResultCount; ++i)
{
Box2DxDebug.Assert(ProxyPool[QueryResults[i]].IsValid);
PairManager.RemoveBufferedPair(proxyId, QueryResults[i]);
}
PairManager.Commit();
// Prepare for next query.
QueryResultCount = 0;
IncrementTimeStamp();
// Return the proxy to the pool.
proxy1.UserData = null;
proxy1.OverlapCount = Invalid;
proxy1.LowerBounds[0] = Invalid;
proxy1.LowerBounds[1] = Invalid;
proxy1.UpperBounds[0] = Invalid;
proxy1.UpperBounds[1] = Invalid;
proxy1.Next = freeProxy;
freeProxy = (ushort)proxyId;
--ProxyCount;
if (IsValidate)
{
Validate();
}
}
/// <summary>
/// Computes the mass using the specified mass data
/// </summary>
/// <param name="massData">The mass data</param>
/// <param name="denstity">The denstity</param>
public override void ComputeMass(out MassData massData, float denstity)
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
Box2DxDebug.Assert(VertexCount >= 3);
Vec2 center = new Vec2(0);
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = new Vec2(0);
#if O
// This code would put the reference point inside the polygon.
for (int i = 0; i < vCount; ++i)
{
pRef += _vertices[i];
}
pRef *= 1.0f / count;
#endif
const float kInv3 = 1.0f / 3.0f;
for (int i = 0; i < VertexCount; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = Vertices[i];
Vec2 p3 = i + 1 < VertexCount ? Vertices[i + 1] : Vertices[0];
Vec2 e1 = p2 - p1;
Vec2 e2 = p3 - p1;
float d = Vec2.Cross(e1, e2);
float triangleArea = 0.5f * d;
area += triangleArea;
// Area weighted centroid
center += triangleArea * kInv3 * (p1 + p2 + p3);
float px = p1.X, py = p1.Y;
float ex1 = e1.X, ey1 = e1.Y;
float ex2 = e2.X, ey2 = e2.Y;
float intx2 = kInv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) +
0.5f * px * px;
float inty2 = kInv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) +
0.5f * py * py;
I += d * (intx2 + inty2);
}
// Total mass
massData.Mass = denstity * area;
// Center of mass
Box2DxDebug.Assert(area > Settings.FltEpsilon);
center *= 1.0f / area;
massData.Center = center;
// Inertia tensor relative to the local origin.
massData.I = denstity * I;
}
int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
{
float maxLambda = 1;
float dx = (segment.P2.X - segment.P1.X) * QuantizationFactor.X;
float dy = (segment.P2.Y - segment.P1.Y) * QuantizationFactor.Y;
int sx = dx <-Settings.FltEpsilon ? -1 : dx> Settings.FltEpsilon ? 1 : 0;
int sy = dy <-Settings.FltEpsilon ? -1 : dy> Settings.FltEpsilon ? 1 : 0;
Box2DxDebug.Assert(sx != 0 || sy != 0);
float p1X = (segment.P1.X - WorldAabb.LowerBound.X) * QuantizationFactor.X;
float p1Y = (segment.P1.Y - WorldAabb.LowerBound.Y) * QuantizationFactor.Y;
#if ALLOWUNSAFE
ushort *startValues = stackalloc ushort[2];
ushort *startValues2 = stackalloc ushort[2];
#else
ushort[] startValues = new ushort[2];
ushort[] startValues2 = new ushort[2];
#endif
int xIndex;
int yIndex;
ushort proxyId;
// TODO_ERIN implement fast float to ushort conversion.
startValues[0] = (ushort)((ushort)p1X & (BroadphaseMax - 1));
startValues2[0] = (ushort)((ushort)p1X | 1);
startValues[1] = (ushort)((ushort)p1Y & (BroadphaseMax - 1));
startValues2[1] = (ushort)((ushort)p1Y | 1);
//First deal with all the proxies that contain segment.p1
int lowerIndex;
int upperIndex;
Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], Bounds[0], 2 * ProxyCount, 0);
if (sx >= 0)
{
xIndex = upperIndex - 1;
}
else
{
xIndex = lowerIndex;
}
Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], Bounds[1], 2 * ProxyCount, 1);
if (sy >= 0)
{
yIndex = upperIndex - 1;
}
else
{
yIndex = lowerIndex;
}
//If we are using sortKey, then sort what we have so far, filtering negative keys
if (sortKey != null)
{
//Fill keys
for (int j = 0; j < QueryResultCount; j++)
{
QuerySortKeys[j] = sortKey(ProxyPool[QueryResults[j]].UserData);
}
//Bubble sort keys
//Sorting negative values to the top, so we can easily remove them
int i = 0;
while (i < QueryResultCount - 1)
{
float a = QuerySortKeys[i];
float b = QuerySortKeys[i + 1];
if (a < 0 ? b >= 0 : a > b && b >= 0)
{
QuerySortKeys[i + 1] = a;
QuerySortKeys[i] = b;
ushort tempValue = QueryResults[i + 1];
QueryResults[i + 1] = QueryResults[i];
QueryResults[i] = tempValue;
i--;
if (i == -1)
{
i = 1;
}
}
else
{
i++;
}
}
//Skim off negative values
while (QueryResultCount > 0 && QuerySortKeys[QueryResultCount - 1] < 0)
{
QueryResultCount--;
}
}
//Now work through the rest of the segment
for (;;)
{
float xProgress = 0;
float yProgress = 0;
if (xIndex < 0 || xIndex >= ProxyCount * 2)
{
break;
}
if (yIndex < 0 || yIndex >= ProxyCount * 2)
{
break;
}
if (sx != 0)
{
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == ProxyCount * 2)
{
break;
}
}
else
{
xIndex--;
if (xIndex < 0)
{
break;
}
}
xProgress = (Bounds[0][xIndex].Value - p1X) / dx;
}
if (sy != 0)
{
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == ProxyCount * 2)
{
break;
}
}
else
{
yIndex--;
if (yIndex < 0)
{
break;
}
}
yProgress = (Bounds[1][yIndex].Value - p1Y) / dy;
}
for (;;)
{
Proxy proxy;
if (sy == 0 || (sx != 0 && xProgress < yProgress))
{
if (xProgress > maxLambda)
{
break;
}
//Check that we are entering a proxy, not leaving
if (sx > 0 ? Bounds[0][xIndex].IsLower : Bounds[0][xIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = Bounds[0][xIndex].ProxyId;
proxy = ProxyPool[proxyId];
if (sy >= 0)
{
if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
QueryResults[QueryResultCount] = proxyId;
++QueryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
QueryResults[QueryResultCount] = proxyId;
++QueryResultCount;
}
}
}
}
//Early out
if (sortKey != null && QueryResultCount == maxCount && QueryResultCount > 0 &&
xProgress > QuerySortKeys[QueryResultCount - 1])
{
break;
}
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == ProxyCount * 2)
{
break;
}
}
else
{
xIndex--;
if (xIndex < 0)
{
break;
}
}
xProgress = (Bounds[0][xIndex].Value - p1X) / dx;
}
else
{
if (yProgress > maxLambda)
{
break;
}
//Check that we are entering a proxy, not leaving
if (sy > 0 ? Bounds[1][yIndex].IsLower : Bounds[1][yIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = Bounds[1][yIndex].ProxyId;
proxy = ProxyPool[proxyId];
if (sx >= 0)
{
if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
QueryResults[QueryResultCount] = proxyId;
++QueryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
QueryResults[QueryResultCount] = proxyId;
++QueryResultCount;
}
}
}
}
//Early out
if (sortKey != null && QueryResultCount == maxCount && QueryResultCount > 0 &&
yProgress > QuerySortKeys[QueryResultCount - 1])
{
break;
}
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == ProxyCount * 2)
{
break;
}
}
else
{
yIndex--;
if (yIndex < 0)
{
break;
}
}
yProgress = (Bounds[1][yIndex].Value - p1Y) / dy;
}
}
break;
}
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>
/// Gets the vertex using the specified index
/// </summary>
/// <param name="index">The index</param>
/// <returns>The vec</returns>
public override Vec2 GetVertex(int index)
{
Box2DxDebug.Assert(0 <= index && index < VertexCount);
return(Vertices[index]);
}