/// <summary>
/// Creates a revolute joint and adds it to the world
/// </summary>
/// <param name="world"></param>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="anchorB"></param>
/// <returns></returns>
public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchorB)
{
Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchorB));
RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, anchorB);
world.AddJoint(joint);
return joint;
}
private void DrawFixture(Graphic g, Body body, Fixture fixture, Color color)
{
switch (fixture.Shape.ShapeType)
{
case ShapeType.Circle:
{
var circle = (CircleShape)fixture.Shape;
Vector2 center = body.GetWorldPoint(circle.Position);
if (DrawSolidShape)
{
g.DrawSolidCircle(center, circle.Radius, color);
}
else
{
g.DrawCircle(center, circle.Radius, color);
}
break;
}
case ShapeType.Polygon:
{
TempVertices.Clear();
var poly = fixture.Shape as PolygonShape;
int vertexCount = poly.Vertices.Count;
for (int i = 0; i < vertexCount; ++i)
{
var p = body.GetWorldPoint(poly.Vertices[i]);
TempVertices.Add(p);
}
if (DrawSolidShape)
{
g.DrawSolidPolygon(TempVertices.ToArray(), color);
}
else
{
g.DrawPolygon(TempVertices.ToArray(), color);
}
break;
}
}
}
/// <summary>
/// Creates a revolute joint.
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localAnchorB">The anchor of bodyB in local coordinates</param>
/// <returns></returns>
public static RevoluteJoint CreateRevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorB)
{
Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localAnchorB));
RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, localAnchorB);
return joint;
}
/// <summary>
/// Creates a prsimatic joint
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localanchorB"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static PrismaticJoint CreatePrismaticJoint(Body bodyA, Body bodyB, Vector2 localanchorB, Vector2 axis)
{
Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localanchorB));
PrismaticJoint joint = new PrismaticJoint(bodyA, bodyB, localanchorA, localanchorB, axis);
return joint;
}
/// <summary>
/// Creates a weld joint
/// </summary>
/// <param name="world"></param>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localanchorB"></param>
/// <returns></returns>
public static WeldJoint CreateWeldJoint(Body bodyA, Body bodyB, Vector2 localanchorB)
{
Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localanchorB));
WeldJoint joint = new WeldJoint(bodyA, bodyB, localanchorA, localanchorB);
return joint;
}
public override void Evaluate(ContactListener listener)
{
Body b1 = _shape1.GetBody();
Body b2 = _shape2.GetBody();
//memcpy(&m0, &m_manifold, sizeof(b2Manifold));
Manifold m0 = _manifold.Clone();
Collision.Collision.CollidePolygonAndCircle(ref _manifold, (PolygonShape)_shape1, b1.GetXForm(),
(CircleShape)_shape2, b2.GetXForm());
bool[] persisted = new[] { false, false };
ContactPoint cp = new ContactPoint();
cp.Shape1 = _shape1;
cp.Shape2 = _shape2;
cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction);
cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution);
// Match contact ids to facilitate warm starting.
if (_manifold.PointCount > 0)
{
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < _manifold.PointCount; ++i)
{
ManifoldPoint mp = _manifold.Points[i];
mp.NormalImpulse = 0.0f;
mp.TangentImpulse = 0.0f;
bool found = false;
ContactID id = mp.ID;
for (int j = 0; j < m0.PointCount; ++j)
{
if (persisted[j])
{
continue;
}
ManifoldPoint mp0 = m0.Points[j];
if (mp0.ID.Key == id.Key)
{
persisted[j] = true;
mp.NormalImpulse = mp0.NormalImpulse;
mp.TangentImpulse = mp0.TangentImpulse;
// A persistent point.
found = true;
// Report persistent point.
if (listener != null)
{
cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
Vector2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
Vector2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Normal = _manifold.Normal;
cp.Separation = mp.Separation;
cp.ID = id;
listener.Persist(cp);
}
break;
}
}
// Report added point.
if (found == false && listener != null)
{
cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
Vector2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
Vector2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Normal = _manifold.Normal;
cp.Separation = mp.Separation;
cp.ID = id;
listener.Add(cp);
}
}
_manifoldCount = 1;
}
else
{
_manifoldCount = 0;
}
if (listener == null)
{
return;
}
// Report removed points.
for (int i = 0; i < m0.PointCount; ++i)
{
if (persisted[i])
{
continue;
}
ManifoldPoint mp0 = m0.Points[i];
cp.Position = b1.GetWorldPoint(mp0.LocalPoint1);
Vector2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1);
Vector2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Normal = m0.Normal;
cp.Separation = mp0.Separation;
cp.ID = mp0.ID;
listener.Remove(cp);
}
}
public override void Evaluate(ContactListener listener)
{
Body b1 = _shape1.GetBody();
Body b2 = _shape2.GetBody();
//memcpy(&m0, &m_manifold, sizeof(b2Manifold));
Manifold m0 = _manifold.Clone();
Collision.Collision.CollideCircles(ref _manifold, (CircleShape)_shape1, b1.GetXForm(),
(CircleShape)_shape2, b2.GetXForm());
ContactPoint cp = new ContactPoint();
cp.Shape1 = _shape1;
cp.Shape2 = _shape2;
cp.Friction = Settings.MixFriction(_shape1.Friction, _shape2.Friction);
cp.Restitution = Settings.MixRestitution(_shape1.Restitution, _shape2.Restitution);
if (_manifold.PointCount > 0)
{
_manifoldCount = 1;
ManifoldPoint mp = _manifold.Points[0];
if (m0.PointCount == 0)
{
mp.NormalImpulse = 0.0f;
mp.TangentImpulse = 0.0f;
if (listener != null)
{
cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
Vector2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
Vector2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Normal = _manifold.Normal;
cp.Separation = mp.Separation;
cp.ID = mp.ID;
listener.Add(cp);
}
}
else
{
ManifoldPoint mp0 = m0.Points[0];
mp.NormalImpulse = mp0.NormalImpulse;
mp.TangentImpulse = mp0.TangentImpulse;
if (listener != null)
{
cp.Position = b1.GetWorldPoint(mp.LocalPoint1);
Vector2 v1 = b1.GetLinearVelocityFromLocalPoint(mp.LocalPoint1);
Vector2 v2 = b2.GetLinearVelocityFromLocalPoint(mp.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Normal = _manifold.Normal;
cp.Separation = mp.Separation;
cp.ID = mp.ID;
listener.Persist(cp);
}
}
}
else
{
_manifoldCount = 0;
if (m0.PointCount > 0 && listener != null)
{
ManifoldPoint mp0 = m0.Points[0];
cp.Position = b1.GetWorldPoint(mp0.LocalPoint1);
Vector2 v1 = b1.GetLinearVelocityFromLocalPoint(mp0.LocalPoint1);
Vector2 v2 = b2.GetLinearVelocityFromLocalPoint(mp0.LocalPoint2);
cp.Velocity = v2 - v1;
cp.Normal = m0.Normal;
cp.Separation = mp0.Separation;
cp.ID = mp0.ID;
listener.Remove(cp);
}
}
}