void DrawCirsleShape(Body body, CircleShape circleShape)
{
var p = circleShape.GetPoint() + body.GetPosition();
var r = circleShape.GetRadius();
Graphics.Circle(DrawMode.Line, p.X, p.Y, r, 100);
}
// This is used for polygon-vs-circle distance.
// GJK is more robust with polygon-vs-point than polygon-vs-circle.
// So we convert polygon-vs-circle to polygon-vs-point.
public static float DistancePC(out Vector2 x1, out Vector2 x2, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
Point point = new Point();
point.p = CommonMath.Mul(xf2, circle.GetLocalPosition());
float distance = DistanceGeneric(out x1, out x2, polygon, xf1, point, XForm.Identity);
float r = circle.GetRadius() - Settings.ToiSlop;
if (distance > r)
{
distance -= r;
Vector2 d = x2 - x1;
d.Normalize();
x2 -= r * d;
}
else
{
distance = 0.0f;
x2 = x1;
}
return(distance);
}
public static float DistanceCC(out Vector2 x1, out Vector2 x2, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
Vector2 p1 = CommonMath.Mul(xf1, circle1.GetLocalPosition());
Vector2 p2 = CommonMath.Mul(xf2, circle2.GetLocalPosition());
Vector2 d = p2 - p1;
float dSqr = Vector2.Dot(d, d);
float r1 = circle1.GetRadius() - Settings.ToiSlop;
float r2 = circle2.GetRadius() - Settings.ToiSlop;
float r = r1 + r2;
if (dSqr > r * r)
{
float dLen = CommonMath.Normalize(ref d);
float distance = dLen - r;
x1 = p1 + r1 * d;
x2 = p2 - r2 * d;
return(distance);
}
else if (dSqr > Settings.FLT_EPSILON * Settings.FLT_EPSILON)
{
d.Normalize();
x1 = p1 + r1 * d;
x2 = x1;
return(0.0f);
}
x1 = p1;
x2 = x1;
return(0.0f);
}
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
manifold.PointCount = 0;
Vector2 p1 = CommonMath.Mul(xf1, circle1.GetLocalPosition());
Vector2 p2 = CommonMath.Mul(xf2, circle2.GetLocalPosition());
Vector2 d = p2 - p1;
float distSqr = Vector2.Dot(d, d);
float r1 = circle1.GetRadius();
float r2 = circle2.GetRadius();
float radiusSum = r1 + r2;
if (distSqr > radiusSum * radiusSum)
{
return;
}
float separation;
if (distSqr < Settings.FLT_EPSILON)
{
separation = -radiusSum;
manifold.Normal = new Vector2(0.0f, 1.0f);
}
else
{
float dist = CommonMath.Sqrt(distSqr);
separation = dist - radiusSum;
float a = 1.0f / dist;
manifold.Normal.X = a * d.X;
manifold.Normal.Y = a * d.Y;
}
manifold.PointCount = 1;
manifold.Points[0].ID.Key = 0;
manifold.Points[0].Separation = separation;
p1 += r1 * manifold.Normal;
p2 -= r2 * manifold.Normal;
Vector2 p = 0.5f * (p1 + p2);
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, p);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, p);
}
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = CommonMath.Mul(xf2, circle.GetLocalPosition());
Vector2 cLocal = CommonMath.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = circle.GetRadius();
int vertexCount = polygon.VertexCount;
Vector2[] vertices = polygon.GetVertices();
Vector2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vector2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// If the center is inside the polygon ...
if (separation < Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, normals[normalIndex]);
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = NullFeature;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
Vector2 position = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position);
manifold.Points[0].Separation = separation - radius;
return;
}
// Project the circle center onto the edge segment.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 e = vertices[vertIndex2] - vertices[vertIndex1];
float length = CommonMath.Normalize(ref e);
//Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
// Project the center onto the edge.
float u = Vector2.Dot(cLocal - vertices[vertIndex1], e);
Vector2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold.Points[0].ID.Features.IncidentEdge = NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold.Points[0].ID.Features.IncidentEdge = NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2;
}
else
{
p = vertices[vertIndex1] + u * e;
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = NullFeature;
}
Vector2 d = cLocal - p;
float dist = CommonMath.Normalize(ref d);
if (dist > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, d);
Vector2 position_ = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position_);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position_);
manifold.Points[0].Separation = dist - radius;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
}
public override void Load()
{
Physics.SetMeter(1);
Body ground;
{
ground = Physics.NewBody(m_world);
Physics.NewEdgeShape(50.0f, 0.0f, -50.0f, 0.0f);
}
{
CircleShape circle1 = Physics.NewCircleShape(1.0f);
PolygonShape box = Physics.NewRectangleShape(1f, 10f);
CircleShape circle2 = Physics.NewCircleShape(2.0f);
var body1 = Physics.NewBody(m_world, 10.0f, 9.0f, BodyType.Static);
Physics.NewFixture(body1, circle1, 5.0f);
var body2 = Physics.NewBody(m_world, 10.0f, 8.0f, BodyType.Dynamic);
Physics.NewFixture(body2, box, 5.0f);
var body3 = Physics.NewBody(m_world, 10.0f, 6.0f, BodyType.Dynamic);
Physics.NewFixture(body3, circle2, 5.0f);
var joint1 = Physics.NewRevoluteJoint(body2, body1, body1.GetPosition());
var joint2 = Physics.NewRevoluteJoint(body2, body3, body3.GetPosition());
var gj = Physics.NewGearJoint(joint1, joint2, circle2.GetRadius() / circle1.GetRadius());
}
{
CircleShape circle1 = Physics.NewCircleShape(1.0f);
CircleShape circle2 = Physics.NewCircleShape(2.0f);
PolygonShape box = Physics.NewRectangleShape(1, 10);
Body body1 = Physics.NewBody(m_world, -3.0f, 12.0f, BodyType.Dynamic);
Physics.NewFixture(body1, circle1, 5.0f);
RevoluteJoint m_joint1 = Physics.NewRevoluteJoint(ground, body1,
body1.GetWorldCenter(),
body1.GetWorldCenter(),
false,
body1.GetAngle() - ground.GetAngle());
Body body2 = Physics.NewBody(m_world, 0.0f, 12.0f, BodyType.Dynamic);
Physics.NewFixture(body2, circle2, 5.0f);
RevoluteJoint m_joint2 = Physics.NewRevoluteJoint(ground, body2, body2.GetPosition());
Body body3 = Physics.NewBody(m_world, 2.5f, 12.0f, BodyType.Dynamic);
Physics.NewFixture(body3, box, 5.0f);
PrismaticJoint m_joint3 = Physics.NewPrismaticJoint(ground,
body3, Vector2.Zero, Vector2.Zero, new Vector2(0.0f, 1.0f));
m_joint3.SetLimits(-0.5f, 0.5f);
m_joint3.SetLimitsEnabled(true);
GearJoint m_joint4 = Physics.NewGearJoint(m_joint1, m_joint2,
circle2.GetRadius() / circle1.GetRadius());
GearJoint m_joint5 = Physics.NewGearJoint(m_joint2, m_joint3, -1.0f / circle2.GetRadius());
}
}