internal void SynchronizeFixtures()
{
Transform xf1 = new Transform(Vector2.Zero, _sweep.A0);
xf1.p = _sweep.C0 - Complex.Multiply(ref _sweep.LocalCenter, ref xf1.q);
IBroadPhase broadPhase = World.ContactManager.BroadPhase;
for (int i = 0; i < FixtureList.Count; i++)
{
FixtureList[i].Synchronize(broadPhase, ref xf1, ref _xf);
}
}
/// <summary>
/// Evaluate this contact with your own manifold and transforms.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="transformA">The first transform.</param>
/// <param name="transformB">The second transform.</param>
private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB)
{
switch (_type)
{
case ContactType.Polygon:
Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.PolygonAndCircle:
Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.EdgeAndCircle:
Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.EdgeAndPolygon:
Collision.Collision.CollideEdgeAndPolygon(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.ChainAndCircle:
ChainShape chain = (ChainShape)FixtureA.Shape;
chain.GetChildEdge(_edge, ChildIndexA);
Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.ChainAndPolygon:
ChainShape loop2 = (ChainShape)FixtureA.Shape;
loop2.GetChildEdge(_edge, ChildIndexA);
Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.Circle:
Collision.Collision.CollideCircles(ref manifold, (CircleShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB);
break;
}
}
/// <summary>
/// For teleporting a body without considering new contacts immediately.
/// Warning: This method is locked during callbacks.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="angle">The angle.</param>
/// <exception cref="System.InvalidOperationException">Thrown when the world is Locked/Stepping.</exception>
public void SetTransformIgnoreContacts(ref Vector2 position, float angle)
{
Debug.Assert(World != null);
if (World.IsLocked)
{
throw new InvalidOperationException("The World is locked.");
}
_xf.q.Phase = angle;
_xf.p = position;
_sweep.C = Transform.Multiply(ref _sweep.LocalCenter, ref _xf);
_sweep.A = angle;
_sweep.C0 = _sweep.C;
_sweep.A0 = angle;
IBroadPhase broadPhase = World.ContactManager.BroadPhase;
for (int i = 0; i < FixtureList.Count; i++)
{
FixtureList[i].Synchronize(broadPhase, ref _xf, ref _xf);
}
}
/// <summary>
/// Get the body transform for the body's origin.
/// </summary>
/// <param name="transform">The transform of the body's origin.</param>
public void GetTransform(out Transform transform)
{
transform = _xf;
}
/// <summary>
/// Gets a local point relative to the body's origin given a world point.
/// Note that the vector only takes the rotation into account, not the position.
/// </summary>
/// <param name="worldPoint">A point in world coordinates.</param>
/// <returns>The corresponding local point relative to the body's origin.</returns>
public Vector2 GetLocalPoint(ref Vector2 worldPoint)
{
return(Transform.Divide(ref worldPoint, ref _xf));
}
/// <summary>
/// Get the world coordinates of a point given the local coordinates.
/// </summary>
/// <param name="localPoint">A point on the body measured relative the the body's origin.</param>
/// <returns>The same point expressed in world coordinates.</returns>
public Vector2 GetWorldPoint(ref Vector2 localPoint)
{
return(Transform.Multiply(ref localPoint, ref _xf));
}
/// <summary>
/// This resets the mass properties to the sum of the mass properties of the fixtures.
/// This normally does not need to be called unless you called SetMassData to override
/// the mass and you later want to reset the mass.
/// </summary>
public void ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
_mass = 0.0f;
_invMass = 0.0f;
_inertia = 0.0f;
_invI = 0.0f;
_sweep.LocalCenter = Vector2.Zero;
// Kinematic bodies have zero mass.
if (BodyType == BodyType.Kinematic)
{
_sweep.C0 = _xf.p;
_sweep.C = _xf.p;
_sweep.A0 = _sweep.A;
return;
}
Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static);
// Accumulate mass over all fixtures.
Vector2 localCenter = Vector2.Zero;
foreach (Fixture f in FixtureList)
{
if (f.Shape._density == 0)
{
continue;
}
MassData massData = f.Shape.MassData;
_mass += massData.Mass;
localCenter += massData.Mass * massData.Centroid;
_inertia += massData.Inertia;
}
//FPE: Static bodies only have mass, they don't have other properties. A little hacky tho...
if (BodyType == BodyType.Static)
{
_sweep.C0 = _sweep.C = _xf.p;
return;
}
// Compute center of mass.
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
localCenter *= _invMass;
}
else
{
// Force all dynamic bodies to have a positive mass.
_mass = 1.0f;
_invMass = 1.0f;
}
if (_inertia > 0.0f && !_fixedRotation)
{
// Center the inertia about the center of mass.
_inertia -= _mass * Vector2.Dot(localCenter, localCenter);
Debug.Assert(_inertia > 0.0f);
_invI = 1.0f / _inertia;
}
else
{
_inertia = 0.0f;
_invI = 0.0f;
}
// Move center of mass.
Vector2 oldCenter = _sweep.C;
_sweep.LocalCenter = localCenter;
_sweep.C0 = _sweep.C = Transform.Multiply(ref _sweep.LocalCenter, ref _xf);
// Update center of mass velocity.
Vector2 a = _sweep.C - oldCenter;
_linearVelocity += new Vector2(-_angularVelocity * a.Y, _angularVelocity * a.X);
}
