public static Fixture CreateCircle(float radius, float density, Body body, Vector2 offset, Object userData)
{
if (radius <= 0)
throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters");
CircleShape circleShape = new CircleShape(radius, density);
circleShape.Position = offset;
return body.CreateFixture(circleShape, userData);
}
public static List<Fixture> CreateCapsule(World world, float height, float endRadius, float density,
Object userData)
{
//Create the middle rectangle
Vertices rectangle = PolygonTools.CreateRectangle(endRadius, height/2);
List<Vertices> list = new List<Vertices>();
list.Add(rectangle);
List<Fixture> fixtures = CreateCompoundPolygon(world, list, density, userData);
//Create the two circles
CircleShape topCircle = new CircleShape(endRadius, density);
topCircle.Position = new Vector2(0, height/2);
fixtures.Add(fixtures[0].Body.CreateFixture(topCircle, userData));
CircleShape bottomCircle = new CircleShape(endRadius, density);
bottomCircle.Position = new Vector2(0, -(height/2));
fixtures.Add(fixtures[0].Body.CreateFixture(bottomCircle, userData));
return fixtures;
}
/// <summary>
/// Compute contact points for edge versus circle.
/// This accounts for edge connectivity.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="edgeA">The edge A.</param>
/// <param name="transformA">The transform A.</param>
/// <param name="circleB">The circle B.</param>
/// <param name="transformB">The transform B.</param>
public static void CollideEdgeAndCircle(ref Manifold manifold,
EdgeShape edgeA, ref Transform transformA,
CircleShape circleB, ref Transform transformB)
{
manifold.PointCount = 0;
// Compute circle in frame of edge
Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB.Position));
Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
Vector2 e = B - A;
// Barycentric coordinates
float u = Vector2.Dot(e, B - Q);
float v = Vector2.Dot(e, Q - A);
float radius = edgeA.Radius + circleB.Radius;
ContactFeature cf;
cf.IndexB = 0;
cf.TypeB = (byte) ContactFeatureType.Vertex;
Vector2 P, d;
// Region A
if (v <= 0.0f)
{
P = A;
d = Q - P;
float dd;
Vector2.Dot(ref d, ref d, out dd);
if (dd > radius*radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
Vector2 A1 = edgeA.Vertex0;
Vector2 B1 = A;
Vector2 e1 = B1 - A1;
float u1 = Vector2.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.IndexA = 0;
cf.TypeA = (byte) ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.Zero;
manifold.LocalPoint = P;
ManifoldPoint mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB.Position;
manifold.Points[0] = mp;
return;
}
// Region B
if (u <= 0.0f)
{
P = B;
d = Q - P;
float dd;
Vector2.Dot(ref d, ref d, out dd);
if (dd > radius*radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
Vector2 B2 = edgeA.Vertex3;
Vector2 A2 = B;
Vector2 e2 = B2 - A2;
float v2 = Vector2.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.IndexA = 1;
cf.TypeA = (byte) ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.Zero;
manifold.LocalPoint = P;
ManifoldPoint mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB.Position;
manifold.Points[0] = mp;
return;
}
// Region AB
float den;
Vector2.Dot(ref e, ref e, out den);
Debug.Assert(den > 0.0f);
P = (1.0f/den)*(u*A + v*B);
d = Q - P;
float dd2;
Vector2.Dot(ref d, ref d, out dd2);
if (dd2 > radius*radius)
{
return;
}
Vector2 n = new Vector2(-e.Y, e.X);
if (Vector2.Dot(n, Q - A) < 0.0f)
{
n = new Vector2(-n.X, -n.Y);
}
n.Normalize();
cf.IndexA = 0;
cf.TypeA = (byte) ContactFeatureType.Face;
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = n;
manifold.LocalPoint = A;
ManifoldPoint mp2 = new ManifoldPoint();
mp2.Id.Key = 0;
mp2.Id.Features = cf;
mp2.LocalPoint = circleB.Position;
manifold.Points[0] = mp2;
}
/// Compute the collision manifold between two circles.
public static void CollideCircles(ref Manifold manifold,
CircleShape circleA, ref Transform xfA,
CircleShape circleB, ref Transform xfB)
{
manifold.PointCount = 0;
float pAx = xfA.Position.X + xfA.R.Col1.X*circleA.Position.X + xfA.R.Col2.X*circleA.Position.Y;
float pAy = xfA.Position.Y + xfA.R.Col1.Y*circleA.Position.X + xfA.R.Col2.Y*circleA.Position.Y;
float pBx = xfB.Position.X + xfB.R.Col1.X*circleB.Position.X + xfB.R.Col2.X*circleB.Position.Y;
float pBy = xfB.Position.Y + xfB.R.Col1.Y*circleB.Position.X + xfB.R.Col2.Y*circleB.Position.Y;
float distSqr = (pBx - pAx)*(pBx - pAx) + (pBy - pAy)*(pBy - pAy);
float radius = circleA.Radius + circleB.Radius;
if (distSqr > radius*radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circleA.Position;
manifold.LocalNormal = Vector2.Zero;
manifold.PointCount = 1;
ManifoldPoint p0 = manifold.Points[0];
p0.LocalPoint = circleB.Position;
p0.Id.Key = 0;
manifold.Points[0] = p0;
}
/// <summary>
/// Compute the collision manifold between a polygon and a circle.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="polygonA">The polygon A.</param>
/// <param name="transformA">The transform of A.</param>
/// <param name="circleB">The circle B.</param>
/// <param name="transformB">The transform of B.</param>
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygonA, ref Transform transformA,
CircleShape circleB, ref Transform transformB)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c =
new Vector2(
transformB.Position.X + transformB.R.Col1.X*circleB.Position.X +
transformB.R.Col2.X*circleB.Position.Y,
transformB.Position.Y + transformB.R.Col1.Y*circleB.Position.X +
transformB.R.Col2.Y*circleB.Position.Y);
Vector2 cLocal =
new Vector2(
(c.X - transformA.Position.X)*transformA.R.Col1.X +
(c.Y - transformA.Position.Y)*transformA.R.Col1.Y,
(c.X - transformA.Position.X)*transformA.R.Col2.X +
(c.Y - transformA.Position.Y)*transformA.R.Col2.Y);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.MaxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.Vertices.Count;
for (int i = 0; i < vertexCount; ++i)
{
Vector2 value1 = polygonA.Normals[i];
Vector2 value2 = cLocal - polygonA.Vertices[i];
float s = value1.X*value2.X + value1.Y*value2.Y;
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 v1 = polygonA.Vertices[vertIndex1];
Vector2 v2 = polygonA.Vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.Epsilon)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = polygonA.Normals[normalIndex];
manifold.LocalPoint = 0.5f*(v1 + v2);
ManifoldPoint p0 = manifold.Points[0];
p0.LocalPoint = circleB.Position;
p0.Id.Key = 0;
manifold.Points[0] = p0;
return;
}
// Compute barycentric coordinates
float u1 = (cLocal.X - v1.X)*(v2.X - v1.X) + (cLocal.Y - v1.Y)*(v2.Y - v1.Y);
float u2 = (cLocal.X - v2.X)*(v1.X - v2.X) + (cLocal.Y - v2.Y)*(v1.Y - v2.Y);
if (u1 <= 0.0f)
{
float r = (cLocal.X - v1.X)*(cLocal.X - v1.X) + (cLocal.Y - v1.Y)*(cLocal.Y - v1.Y);
if (r > radius*radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = cLocal - v1;
float factor = 1f/
(float)
Math.Sqrt(manifold.LocalNormal.X*manifold.LocalNormal.X +
manifold.LocalNormal.Y*manifold.LocalNormal.Y);
manifold.LocalNormal.X = manifold.LocalNormal.X*factor;
manifold.LocalNormal.Y = manifold.LocalNormal.Y*factor;
manifold.LocalPoint = v1;
ManifoldPoint p0b = manifold.Points[0];
p0b.LocalPoint = circleB.Position;
p0b.Id.Key = 0;
manifold.Points[0] = p0b;
}
else if (u2 <= 0.0f)
{
float r = (cLocal.X - v2.X)*(cLocal.X - v2.X) + (cLocal.Y - v2.Y)*(cLocal.Y - v2.Y);
if (r > radius*radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = cLocal - v2;
float factor = 1f/
(float)
Math.Sqrt(manifold.LocalNormal.X*manifold.LocalNormal.X +
manifold.LocalNormal.Y*manifold.LocalNormal.Y);
manifold.LocalNormal.X = manifold.LocalNormal.X*factor;
manifold.LocalNormal.Y = manifold.LocalNormal.Y*factor;
manifold.LocalPoint = v2;
ManifoldPoint p0c = manifold.Points[0];
p0c.LocalPoint = circleB.Position;
p0c.Id.Key = 0;
manifold.Points[0] = p0c;
}
else
{
Vector2 faceCenter = 0.5f*(v1 + v2);
Vector2 value1 = cLocal - faceCenter;
Vector2 value2 = polygonA.Normals[vertIndex1];
float separation2 = value1.X*value2.X + value1.Y*value2.Y;
if (separation2 > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = polygonA.Normals[vertIndex1];
manifold.LocalPoint = faceCenter;
ManifoldPoint p0d = manifold.Points[0];
p0d.LocalPoint = circleB.Position;
p0d.Id.Key = 0;
manifold.Points[0] = p0d;
}
}
public override Shape Clone()
{
CircleShape shape = new CircleShape();
shape.ShapeType = ShapeType;
shape.Radius = Radius;
shape.Position = Position;
shape._density = _density;
shape.MassData = MassData;
return shape;
}
