private RopeTest()
{
Body ground;
{
ground = new Body(World);
EdgeShape shape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape);
}
{
Body prevBody = ground;
PolygonShape largeShape = new PolygonShape(PolygonTools.CreateRectangle(1.5f, 1.5f), 100);
PolygonShape smallShape = new PolygonShape(PolygonTools.CreateRectangle(0.5f, 0.125f), 20);
const int N = 10;
const float y = 15;
for (int i = 0; i < N; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.5f + 1.0f * i, y);
if (i == N - 1)
{
Fixture fixture = body.CreateFixture(largeShape);
fixture.Friction = 0.2f;
fixture.CollisionCategories = Category.Cat2;
fixture.CollidesWith = Category.All & ~Category.Cat2;
body.Position = new Vector2(1.0f * i, y);
body.AngularDamping = 0.4f;
}
else
{
Fixture fixture = body.CreateFixture(smallShape);
fixture.Friction = 0.2f;
fixture.CollisionCategories = Category.Cat1;
fixture.CollidesWith = Category.All & ~Category.Cat2;
}
Vector2 anchor = new Vector2(i, y);
RevoluteJoint jd = new RevoluteJoint(prevBody, body, prevBody.GetLocalPoint(ref anchor),
body.GetLocalPoint(ref anchor));
jd.CollideConnected = false;
World.AddJoint(jd);
prevBody = body;
}
_rj = new RopeJoint(ground, prevBody, new Vector2(0, y), Vector2.Zero);
//FPE: The two following lines are actually not needed as FPE sets the MaxLength to a default value
const float extraLength = 0.01f;
_rj.MaxLength = N - 1.0f + extraLength;
World.AddJoint(_rj);
}
}
private SensorTest()
{
{
Body ground = BodyFactory.CreateBody(World);
{
EdgeShape shape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape);
}
{
CircleShape shape = new CircleShape(5.0f, 1);
shape.Position = new Vector2(0.0f, 10.0f);
_sensor = ground.CreateFixture(shape);
_sensor.IsSensor = true;
}
}
{
CircleShape shape = new CircleShape(1.0f, 1);
for (int i = 0; i < Count; ++i)
{
_touching[i] = false;
_bodies[i] = BodyFactory.CreateBody(World);
_bodies[i].BodyType = BodyType.Dynamic;
_bodies[i].Position = new Vector2(-10.0f + 3.0f * i, 20.0f);
_bodies[i].UserData = i;
_bodies[i].CreateFixture(shape);
}
}
}
private CircleBenchmarkTest()
{
Body ground = BodyFactory.CreateBody(World);
// Floor
EdgeShape ashape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(ashape);
// Left wall
ashape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(-40.0f, 45.0f));
ground.CreateFixture(ashape);
// Right wall
ashape = new EdgeShape(new Vector2(40.0f, 0.0f), new Vector2(40.0f, 45.0f));
ground.CreateFixture(ashape);
// Roof
ashape = new EdgeShape(new Vector2(-40.0f, 45.0f), new Vector2(40.0f, 45.0f));
ground.CreateFixture(ashape);
CircleShape shape = new CircleShape(1.0f, 1);
for (int i = 0; i < XCount; i++)
{
for (int j = 0; j < YCount; ++j)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-38f + 2.1f * i, 2.0f + 2.0f * j);
body.CreateFixture(shape);
}
}
}
private PrismaticTest()
{
Body ground;
{
ground = BodyFactory.CreateBody(World);
EdgeShape shape3 = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape3);
}
PolygonShape shape = new PolygonShape(5);
shape.SetAsBox(2.0f, 0.5f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 10.0f);
body.CreateFixture(shape);
_fixedJoint = new FixedPrismaticJoint(body, body.Position, new Vector2(0.5f, 1.0f));
_fixedJoint.MotorSpeed = 5.0f;
_fixedJoint.MaxMotorForce = 1000.0f;
_fixedJoint.MotorEnabled = true;
_fixedJoint.LowerLimit = -10.0f;
_fixedJoint.UpperLimit = 20.0f;
_fixedJoint.LimitEnabled = true;
World.AddJoint(_fixedJoint);
PolygonShape shape2 = new PolygonShape(5);
shape2.SetAsBox(2.0f, 0.5f);
Body body2 = BodyFactory.CreateBody(World);
body2.BodyType = BodyType.Dynamic;
body2.Position = new Vector2(10.0f, 10.0f);
body2.CreateFixture(shape2);
_joint = new PrismaticJoint(ground, body2, ground.GetLocalPoint(body2.Position), Vector2.Zero,
new Vector2(0.5f, 1.0f));
_joint.MotorSpeed = 5.0f;
_joint.MaxMotorForce = 1000.0f;
_joint.MotorEnabled = true;
_joint.LowerLimit = -10.0f;
_joint.UpperLimit = 20.0f;
_joint.LimitEnabled = true;
World.AddJoint(_joint);
}
public void Initialize(Map.Map map)
{
var collision = new CollisionMap(map);
if (_world == null)
_world = new World(new Vector2(0, 0));
else
_world.Clear();
var obstacles = collision.GetObstacles();
var layer2Obstacles = obstacles.GetObstacles(2);
foreach (var obstacle in layer2Obstacles)
{
var body = new Body(_world) { BodyType = BodyType.Static };
_json.SetName(body, "Building" + obstacle.Z);
Shape shape;
Fixture fixture;
switch (obstacle.Type)
{
case ObstacleType.Line:
var lineObstacle = (LineObstacle)obstacle;
shape = new EdgeShape(lineObstacle.Start.ToMeters(), lineObstacle.End.ToMeters());
fixture = body.CreateFixture(shape);
_json.SetName(fixture, "Building" + obstacle.Z);
break;
case ObstacleType.Polygon:
var polygonObstacle = (PolygonObstacle)obstacle;
var convexPolygons = BayazitDecomposer.ConvexPartition(polygonObstacle.Vertices);
foreach (var convexPolygon in convexPolygons)
{
shape = new PolygonShape(convexPolygon.ToMeters(), 1);
fixture = body.CreateFixture(shape);
_json.SetName(fixture, "Building" + obstacle.Z);
}
break;
case ObstacleType.Rectangle:
var rectangleObstacle = (RectangleObstacle)obstacle;
shape = new PolygonShape(rectangleObstacle.Vertices.ToMeters(), 1);
fixture = body.CreateFixture(shape);
_json.SetName(fixture, "Building" + obstacle.Z);
break;
}
}
}
private ConfinedTest()
{
{
Body ground = BodyFactory.CreateBody(World);
// Floor
EdgeShape shape = new EdgeShape(new Vector2(-10.0f, 0.0f), new Vector2(10.0f, 0.0f));
ground.CreateFixture(shape);
// Left wall
shape = new EdgeShape(new Vector2(-10.0f, 0.0f), new Vector2(-10.0f, 20.0f));
ground.CreateFixture(shape);
// Right wall
shape = new EdgeShape(new Vector2(10.0f, 0.0f), new Vector2(10.0f, 20.0f));
ground.CreateFixture(shape);
// Roof
shape = new EdgeShape(new Vector2(-10.0f, 20.0f), new Vector2(10.0f, 20.0f));
ground.CreateFixture(shape);
}
const float radius = 0.5f;
CircleShape shape2 = new CircleShape(radius, 1);
shape2.Position = Vector2.Zero;
for (int j = 0; j < ColumnCount; ++j)
{
for (int i = 0; i < RowCount; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-10.0f + (2.1f * j + 1.0f + 0.01f * i) * radius,
(2.0f * i + 1.0f) * radius);
Fixture fixture = body.CreateFixture(shape2);
fixture.Friction = 0.1f;
}
}
World.Gravity = Vector2.Zero;
}
public static List<Fixture> AttachCompoundPolygon(List<Vertices> list, float density, Body body, object userData)
{
List<Fixture> res = new List<Fixture>(list.Count);
//Then we create several fixtures using the body
foreach (Vertices vertices in list)
{
if (vertices.Count == 2)
{
EdgeShape shape = new EdgeShape(vertices[0], vertices[1]);
res.Add(body.CreateFixture(shape, userData));
}
else
{
PolygonShape shape = new PolygonShape(vertices, density);
res.Add(body.CreateFixture(shape, userData));
}
}
return res;
}
private EdgeShapeBenchmark()
{
// Ground body
{
Body ground = BodyFactory.CreateBody(World);
float x1 = -20.0f;
float y1 = 2.0f * (float)Math.Cos(x1 / 10.0f * (float)Math.PI);
for (int i = 0; i < 80; ++i)
{
float x2 = x1 + 0.5f;
float y2 = 2.0f * (float)Math.Cos(x2 / 10.0f * (float)Math.PI);
EdgeShape shape = new EdgeShape(new Vector2(x1, y1), new Vector2(x2, y2));
ground.CreateFixture(shape);
x1 = x2;
y1 = y2;
}
}
const float w = 1.0f;
const float t = 2.0f;
float b = w / (2.0f + (float)Math.Sqrt(t));
float s = (float)Math.Sqrt(t) * b;
Vertices vertices = new Vertices(8);
vertices.Add(new Vector2(0.5f * s, 0.0f));
vertices.Add(new Vector2(0.5f * w, b));
vertices.Add(new Vector2(0.5f * w, b + s));
vertices.Add(new Vector2(0.5f * s, w));
vertices.Add(new Vector2(-0.5f * s, w));
vertices.Add(new Vector2(-0.5f * w, b + s));
vertices.Add(new Vector2(-0.5f * w, b));
vertices.Add(new Vector2(-0.5f * s, 0.0f));
_polyShape = new PolygonShape(20);
_polyShape.Set(vertices);
}
/// <summary>
/// Get a child edge.
/// </summary>
/// <param name="edge">The edge.</param>
/// <param name="index">The index.</param>
public void GetChildEdge(ref EdgeShape edge, int index)
{
Debug.Assert(2 <= Vertices.Count);
Debug.Assert(0 <= index && index < Vertices.Count);
edge.ShapeType = ShapeType.Edge;
edge.Radius = Radius;
edge.HasVertex0 = true;
edge.HasVertex3 = true;
int i0 = index - 1 >= 0 ? index - 1 : Vertices.Count - 1;
int i1 = index;
int i2 = index + 1 < Vertices.Count ? index + 1 : 0;
int i3 = index + 2;
while (i3 >= Vertices.Count)
{
i3 -= Vertices.Count;
}
edge.Vertex0 = Vertices[i0];
edge.Vertex1 = Vertices[i1];
edge.Vertex2 = Vertices[i2];
edge.Vertex3 = Vertices[i3];
}
/// <summary>
/// Get a child edge.
/// </summary>
/// <param name="edge">The edge.</param>
/// <param name="index">The index.</param>
public void GetChildEdge(ref EdgeShape edge, int index)
{
Debug.Assert(2 <= Vertices.Count);
Debug.Assert(0 <= index && index < Vertices.Count);
edge.ShapeType = ShapeType.Edge;
edge.Radius = Radius;
edge.HasVertex0 = true;
edge.HasVertex3 = true;
int i0 = index - 1 >= 0 ? index - 1 : Vertices.Count - 1;
int i1 = index;
int i2 = index + 1 < Vertices.Count ? index + 1 : 0;
int i3 = index + 2;
while (i3 >= Vertices.Count)
{
i3 -= Vertices.Count;
}
edge.Vertex0 = Vertices[i0];
edge.Vertex1 = Vertices[i1];
edge.Vertex2 = Vertices[i2];
edge.Vertex3 = Vertices[i3];
}
/// <summary>
/// Collides and edge and a polygon, taking into account edge adjacency.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="edgeA">The edge A.</param>
/// <param name="xfA">The xf A.</param>
/// <param name="polygonB">The polygon B.</param>
/// <param name="xfB">The xf B.</param>
public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
EPCollider collider = new EPCollider();
collider.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
}
public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
_xf = MathUtils.MulT(xfA, xfB);
_centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid);
_v0 = edgeA.Vertex0;
_v1 = edgeA._vertex1;
_v2 = edgeA._vertex2;
_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
Vector2 edge1 = _v2 - _v1;
edge1.Normalize();
_normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(_normal1, _centroidB - _v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vector2 edge0 = _v1 - _v0;
edge0.Normalize();
_normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(_normal0, _centroidB - _v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vector2 edge2 = _v3 - _v2;
edge2.Normalize();
_normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(_normal2, _centroidB - _v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
}
else if (convex1)
{
_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal1;
}
}
else if (convex2)
{
_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal0;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
}
else
{
_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = _normal1;
}
}
}
else
{
_front = offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
}
// Get polygonB in frameA
_polygonB.Count = polygonB.Vertices.Count;
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
_polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]);
_polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]);
}
_radius = 2.0f * Settings.PolygonRadius;
manifold.PointCount = 0;
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > _radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.Type == EPAxisType.Unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
FixedArray2<ClipVertex> ie = new FixedArray2<ClipVertex>();
ReferenceFace rf;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]);
for (int i = 1; i < _polygonB.Count; ++i)
{
float value = Vector2.Dot(_normal, _polygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0;
ClipVertex c0 = ie[0];
c0.V = _polygonB.Vertices[i1];
c0.ID.Features.IndexA = 0;
c0.ID.Features.IndexB = (byte)i1;
c0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
c0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
ie[0] = c0;
ClipVertex c1 = ie[1];
c1.V = _polygonB.Vertices[i2];
c1.ID.Features.IndexA = 0;
c1.ID.Features.IndexB = (byte)i2;
c1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
c1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
ie[1] = c1;
if (_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = _v1;
rf.v2 = _v2;
rf.normal = _normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = _v2;
rf.v2 = _v1;
rf.normal = -_normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ClipVertex c0 = ie[0];
c0.V = _v1;
c0.ID.Features.IndexA = 0;
c0.ID.Features.IndexB = (byte)primaryAxis.Index;
c0.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
c0.ID.Features.TypeB = (byte)ContactFeatureType.Face;
ie[0] = c0;
ClipVertex c1 = ie[1];
c1.V = _v2;
c1.ID.Features.IndexA = 0;
c1.ID.Features.IndexB = (byte)primaryAxis.Index;
c1.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
c1.ID.Features.TypeB = (byte)ContactFeatureType.Face;
ie[1] = c1;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0;
rf.v1 = _polygonB.Vertices[rf.i1];
rf.v2 = _polygonB.Vertices[rf.i2];
rf.normal = _polygonB.Normals[rf.i1];
}
rf.sideNormal1 = new Vector2(rf.normal.Y, -rf.normal.X);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = Vector2.Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Vector2.Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
FixedArray2<ClipVertex> clipPoints1;
FixedArray2<ClipVertex> clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = rf.normal;
manifold.LocalPoint = rf.v1;
}
else
{
manifold.LocalNormal = polygonB.Normals[rf.i1];
manifold.LocalPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(rf.normal, clipPoints2[i].V - rf.v1);
if (separation <= _radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.Features.TypeA = clipPoints2[i].ID.Features.TypeB;
cp.Id.Features.TypeB = clipPoints2[i].ID.Features.TypeA;
cp.Id.Features.IndexA = clipPoints2[i].ID.Features.IndexB;
cp.Id.Features.IndexB = clipPoints2[i].ID.Features.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
public bool CompareTo( EdgeShape shape )
{
return ( hasVertex0 == shape.hasVertex0 &&
hasVertex3 == shape.hasVertex3 &&
vertex0 == shape.vertex0 &&
vertex1 == shape.vertex1 &&
vertex2 == shape.vertex2 &&
vertex3 == shape.vertex3 );
}
public static Fixture AttachEdge(Vector2 start, Vector2 end, Body body, object userData)
{
EdgeShape edgeShape = new EdgeShape(start, end);
return body.CreateFixture(edgeShape, userData);
}
private CharacterCollisionTest()
{
//Ground body
Body ground = BodyFactory.CreateEdge(World, new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
// Collinear edges
EdgeShape shape = new EdgeShape(new Vector2(-8.0f, 1.0f), new Vector2(-6.0f, 1.0f));
ground.CreateFixture(shape);
shape = new EdgeShape(new Vector2(-6.0f, 1.0f), new Vector2(-4.0f, 1.0f));
ground.CreateFixture(shape);
shape = new EdgeShape(new Vector2(-4.0f, 1.0f), new Vector2(-2.0f, 1.0f));
ground.CreateFixture(shape);
// Square tiles
PolygonShape tile = new PolygonShape(1);
tile.SetAsBox(1.0f, 1.0f, new Vector2(4.0f, 3.0f), 0.0f);
ground.CreateFixture(tile);
tile.SetAsBox(1.0f, 1.0f, new Vector2(6.0f, 3.0f), 0.0f);
ground.CreateFixture(tile);
tile.SetAsBox(1.0f, 1.0f, new Vector2(8.0f, 3.0f), 0.0f);
ground.CreateFixture(tile);
// Square made from an edge loop.
Vertices vertices = new Vertices(4);
vertices.Add(new Vector2(-1.0f, 3.0f));
vertices.Add(new Vector2(1.0f, 3.0f));
vertices.Add(new Vector2(1.0f, 5.0f));
vertices.Add(new Vector2(-1.0f, 5.0f));
LoopShape loopShape = new LoopShape(vertices);
ground.CreateFixture(loopShape);
// Edge loop.
vertices = new Vertices(10);
vertices.Add(new Vector2(0.0f, 0.0f));
vertices.Add(new Vector2(6.0f, 0.0f));
vertices.Add(new Vector2(6.0f, 2.0f));
vertices.Add(new Vector2(4.0f, 1.0f));
vertices.Add(new Vector2(2.0f, 2.0f));
vertices.Add(new Vector2(-2.0f, 2.0f));
vertices.Add(new Vector2(-4.0f, 3.0f));
vertices.Add(new Vector2(-6.0f, 2.0f));
vertices.Add(new Vector2(-6.0f, 0.0f));
BodyFactory.CreateLoopShape(World, vertices, new Vector2(-10, 4));
// Square character
Body squareCharacter = BodyFactory.CreateRectangle(World, 1, 1, 20);
squareCharacter.Position = new Vector2(-3.0f, 5.0f);
squareCharacter.BodyType = BodyType.Dynamic;
squareCharacter.FixedRotation = true;
squareCharacter.SleepingAllowed = false;
squareCharacter.OnCollision += CharacterOnCollision;
squareCharacter.OnSeparation += CharacterOnSeparation;
// Square character 2
Body squareCharacter2 = BodyFactory.CreateRectangle(World, 0.5f, 0.5f, 20);
squareCharacter2.Position = new Vector2(-5.0f, 5.0f);
squareCharacter2.BodyType = BodyType.Dynamic;
squareCharacter2.FixedRotation = true;
squareCharacter2.SleepingAllowed = false;
// Hexagon character
float angle = 0.0f;
const float delta = Settings.Pi / 3.0f;
vertices = new Vertices(6);
for (int i = 0; i < 6; ++i)
{
vertices.Add(new Vector2(0.5f * (float)Math.Cos(angle), 0.5f * (float)Math.Sin(angle)));
angle += delta;
}
Body hexCharacter = BodyFactory.CreatePolygon(World, vertices, 20);
hexCharacter.Position = new Vector2(-5.0f, 8.0f);
hexCharacter.BodyType = BodyType.Dynamic;
hexCharacter.FixedRotation = true;
hexCharacter.SleepingAllowed = false;
// Circle character
Body circleCharacter = BodyFactory.CreateCircle(World, 0.5f, 20);
circleCharacter.Position = new Vector2(3.0f, 5.0f);
circleCharacter.BodyType = BodyType.Dynamic;
circleCharacter.FixedRotation = true;
circleCharacter.SleepingAllowed = false;
}
public override Shape clone()
{
var clone = new EdgeShape();
clone.shapeType = shapeType;
clone._radius = _radius;
clone._density = _density;
clone.hasVertex0 = hasVertex0;
clone.hasVertex3 = hasVertex3;
clone.vertex0 = vertex0;
clone._vertex1 = _vertex1;
clone._vertex2 = _vertex2;
clone.vertex3 = vertex3;
clone.massData = massData;
return clone;
}
/// <summary>
/// Get a child edge.
/// </summary>
/// <param name="index">The index.</param>
public EdgeShape GetChildEdge(int index)
{
EdgeShape edgeShape = new EdgeShape();
GetChildEdge(edgeShape, index);
return edgeShape;
}
private ApplyForceTest()
{
World.Gravity = Vector2.Zero;
const float restitution = 0.4f;
Body ground;
{
ground = BodyFactory.CreateBody(World);
ground.Position = new Vector2(0.0f, 20.0f);
EdgeShape shape = new EdgeShape(new Vector2(-20.0f, -20.0f), new Vector2(-20.0f, 20.0f));
// Left vertical
Fixture fixture = ground.CreateFixture(shape);
fixture.Restitution = restitution;
// Right vertical
shape = new EdgeShape(new Vector2(20.0f, -20.0f), new Vector2(20.0f, 20.0f));
ground.CreateFixture(shape);
// Top horizontal
shape = new EdgeShape(new Vector2(-20.0f, 20.0f), new Vector2(20.0f, 20.0f));
ground.CreateFixture(shape);
// Bottom horizontal
shape = new EdgeShape(new Vector2(-20.0f, -20.0f), new Vector2(20.0f, -20.0f));
ground.CreateFixture(shape);
}
{
Transform xf1 = new Transform();
xf1.q.Set(0.3524f * Settings.Pi);
xf1.p = MathUtils.Mul(ref xf1.q, new Vector2(1.0f, 0.0f));
Vertices vertices = new Vertices(3);
vertices.Add(MathUtils.Mul(ref xf1, new Vector2(-1.0f, 0.0f)));
vertices.Add(MathUtils.Mul(ref xf1, new Vector2(1.0f, 0.0f)));
vertices.Add(MathUtils.Mul(ref xf1, new Vector2(0.0f, 0.5f)));
PolygonShape poly1 = new PolygonShape(vertices, 4);
Transform xf2 = new Transform();
xf2.q.Set(-0.3524f * Settings.Pi);
xf2.p = MathUtils.Mul(ref xf2.q, new Vector2(-1.0f, 0.0f));
vertices[0] = MathUtils.Mul(ref xf2, new Vector2(-1.0f, 0.0f));
vertices[1] = MathUtils.Mul(ref xf2, new Vector2(1.0f, 0.0f));
vertices[2] = MathUtils.Mul(ref xf2, new Vector2(0.0f, 0.5f));
PolygonShape poly2 = new PolygonShape(vertices, 2);
_body = BodyFactory.CreateBody(World);
_body.BodyType = BodyType.Dynamic;
_body.Position = new Vector2(0.0f, 2.0f);
_body.Rotation = Settings.Pi;
_body.AngularDamping = 5.0f;
_body.LinearDamping = 0.8f;
_body.SleepingAllowed = true;
_body.CreateFixture(poly1);
_body.CreateFixture(poly2);
}
{
Vertices box = PolygonTools.CreateRectangle(0.5f, 0.5f);
PolygonShape shape = new PolygonShape(box, 1);
for (int i = 0; i < 10; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.Position = new Vector2(0.0f, 5.0f + 1.54f * i);
body.BodyType = BodyType.Dynamic;
Fixture fixture = body.CreateFixture(shape);
fixture.Friction = 0.3f;
const float gravity = 10.0f;
float I = body.Inertia;
float mass = body.Mass;
// For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
float radius = (float)Math.Sqrt(2.0 * (I / mass));
FrictionJoint jd = new FrictionJoint(ground, body, Vector2.Zero);
jd.CollideConnected = true;
jd.MaxForce = mass * gravity;
jd.MaxTorque = mass * radius * gravity;
World.AddJoint(jd);
}
}
}
private EdgeShapes()
{
// Ground body
{
Body ground = BodyFactory.CreateBody(World);
float x1 = -20.0f;
float y1 = 2.0f * (float)Math.Cos(x1 / 10.0f * (float)Math.PI);
for (int i = 0; i < 80; ++i)
{
float x2 = x1 + 0.5f;
float y2 = 2.0f * (float)Math.Cos(x2 / 10.0f * (float)Math.PI);
EdgeShape shape = new EdgeShape(new Vector2(x1, y1), new Vector2(x2, y2));
ground.CreateFixture(shape);
x1 = x2;
y1 = y2;
}
}
{
Vertices vertices = new Vertices(3);
vertices.Add(new Vector2(-0.5f, 0.0f));
vertices.Add(new Vector2(0.5f, 0.0f));
vertices.Add(new Vector2(0.0f, 1.5f));
_polygons[0] = new PolygonShape(20);
_polygons[0].Set(vertices);
}
{
Vertices vertices = new Vertices(3);
vertices.Add(new Vector2(-0.1f, 0.0f));
vertices.Add(new Vector2(0.1f, 0.0f));
vertices.Add(new Vector2(0.0f, 1.5f));
_polygons[1] = new PolygonShape(20);
_polygons[1].Set(vertices);
}
{
const float w = 1.0f;
float b = w / (2.0f + (float)Math.Sqrt(2.0f));
float s = (float)Math.Sqrt(2.0f) * b;
Vertices vertices = new Vertices(8);
vertices.Add(new Vector2(0.5f * s, 0.0f));
vertices.Add(new Vector2(0.5f * w, b));
vertices.Add(new Vector2(0.5f * w, b + s));
vertices.Add(new Vector2(0.5f * s, w));
vertices.Add(new Vector2(-0.5f * s, w));
vertices.Add(new Vector2(-0.5f * w, b + s));
vertices.Add(new Vector2(-0.5f * w, b));
vertices.Add(new Vector2(-0.5f * s, 0.0f));
_polygons[2] = new PolygonShape(20);
_polygons[2].Set(vertices);
}
{
_polygons[3] = new PolygonShape(20);
_polygons[3].SetAsBox(0.5f, 0.5f);
}
{
_circle = new CircleShape(0.5f, 1);
}
_bodyIndex = 0;
_angle = 0.0f;
}
private CarTest()
{
_hz = 4.0f;
_zeta = 0.7f;
_speed = 50.0f;
Body ground = new Body(World);
{
EdgeShape shape = new EdgeShape(new Vector2(-20.0f, 0.0f), new Vector2(20.0f, 0.0f));
ground.CreateFixture(shape);
float[] hs = new[] { 0.25f, 1.0f, 4.0f, 0.0f, 0.0f, -1.0f, -2.0f, -2.0f, -1.25f, 0.0f };
float x = 20.0f, y1 = 0.0f;
const float dx = 5.0f;
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
FixtureFactory.AttachEdge(new Vector2(x, y1), new Vector2(x + dx, y2), ground);
y1 = y2;
x += dx;
}
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
FixtureFactory.AttachEdge(new Vector2(x, y1), new Vector2(x + dx, y2), ground);
y1 = y2;
x += dx;
}
FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f), ground);
x += 80.0f;
FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f), ground);
x += 40.0f;
FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 10.0f, 5.0f), ground);
x += 20.0f;
FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f), ground);
x += 40.0f;
FixtureFactory.AttachEdge(new Vector2(x, 0.0f), new Vector2(x, 20.0f), ground);
ground.Friction = 0.6f;
}
// Teeter
{
Body body = new Body(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(140.0f, 1.0f);
PolygonShape box = new PolygonShape(1);
box.SetAsBox(10.0f, 0.25f);
body.CreateFixture(box);
RevoluteJoint jd = JointFactory.CreateRevoluteJoint(ground, body, Vector2.Zero);
jd.LowerLimit = -8.0f * Settings.Pi / 180.0f;
jd.UpperLimit = 8.0f * Settings.Pi / 180.0f;
jd.LimitEnabled = true;
World.AddJoint(jd);
body.ApplyAngularImpulse(100.0f);
}
//Bridge
{
const int N = 20;
PolygonShape shape = new PolygonShape(1);
shape.SetAsBox(1.0f, 0.125f);
Body prevBody = ground;
for (int i = 0; i < N; ++i)
{
Body body = new Body(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(161.0f + 2.0f * i, -0.125f);
Fixture fix = body.CreateFixture(shape);
fix.Friction = 0.6f;
Vector2 anchor = new Vector2(-1, 0);
JointFactory.CreateRevoluteJoint(World, prevBody, body, anchor);
prevBody = body;
}
Vector2 anchor2 = new Vector2(1.0f, 0);
JointFactory.CreateRevoluteJoint(World, ground, prevBody, anchor2);
}
// Boxes
{
PolygonShape box = new PolygonShape(0.5f);
box.SetAsBox(0.5f, 0.5f);
Body body = new Body(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 0.5f);
body.CreateFixture(box);
body = new Body(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 1.5f);
body.CreateFixture(box);
body = new Body(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 2.5f);
body.CreateFixture(box);
body = new Body(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 3.5f);
body.CreateFixture(box);
body = new Body(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 4.5f);
body.CreateFixture(box);
}
// Car
{
Vertices vertices = new Vertices(8);
vertices.Add(new Vector2(-1.5f, -0.5f));
vertices.Add(new Vector2(1.5f, -0.5f));
vertices.Add(new Vector2(1.5f, 0.0f));
vertices.Add(new Vector2(0.0f, 0.9f));
vertices.Add(new Vector2(-1.15f, 0.9f));
vertices.Add(new Vector2(-1.5f, 0.2f));
PolygonShape chassis = new PolygonShape(vertices, 1);
CircleShape circle = new CircleShape(0.4f, 1);
_car = new Body(World);
_car.BodyType = BodyType.Dynamic;
_car.Position = new Vector2(0.0f, 1.0f);
_car.CreateFixture(chassis);
_wheel1 = new Body(World);
_wheel1.BodyType = BodyType.Dynamic;
_wheel1.Position = new Vector2(-1.0f, 0.35f);
Fixture fix = _wheel1.CreateFixture(circle);
fix.Friction = 0.9f;
_wheel2 = new Body(World);
_wheel2.BodyType = BodyType.Dynamic;
_wheel2.Position = new Vector2(1.0f, 0.4f);
_wheel2.CreateFixture(circle);
Vector2 axis = new Vector2(0.0f, 1.0f);
_spring1 = new WheelJoint(_car, _wheel1, _wheel1.Position, axis);
_spring1.MotorSpeed = 0.0f;
_spring1.MaxMotorTorque = 20.0f;
_spring1.MotorEnabled = true;
_spring1.Frequency = _hz;
_spring1.DampingRatio = _zeta;
World.AddJoint(_spring1);
_spring2 = new WheelJoint(_car, _wheel2, _wheel2.Position, axis);
_spring2.MotorSpeed = 0.0f;
_spring2.MaxMotorTorque = 10.0f;
_spring2.MotorEnabled = false;
_spring2.Frequency = _hz;
_spring2.DampingRatio = _zeta;
World.AddJoint(_spring2);
}
}
/// <summary>
/// Collides and edge and a polygon, taking into account edge adjacency.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="edgeA">The edge A.</param>
/// <param name="xfA">The xf A.</param>
/// <param name="polygonB">The polygon B.</param>
/// <param name="xfB">The xf B.</param>
public static void CollideEdgeAndPolygon(ref Manifold manifold,
EdgeShape edgeA, ref Transform xfA,
PolygonShape polygonB, ref Transform xfB)
{
MathUtils.MultiplyT(ref xfA, ref xfB, out _xf);
// Edge geometry
_edgeA.V0 = edgeA.Vertex0;
_edgeA.V1 = edgeA.Vertex1;
_edgeA.V2 = edgeA.Vertex2;
_edgeA.V3 = edgeA.Vertex3;
Vector2 e = _edgeA.V2 - _edgeA.V1;
// Normal points outwards in CCW order.
_edgeA.Normal = new Vector2(e.Y, -e.X);
_edgeA.Normal.Normalize();
_edgeA.HasVertex0 = edgeA.HasVertex0;
_edgeA.HasVertex3 = edgeA.HasVertex3;
// Proxy for edge
_proxyA.Vertices[0] = _edgeA.V1;
_proxyA.Vertices[1] = _edgeA.V2;
_proxyA.Normals[0] = _edgeA.Normal;
_proxyA.Normals[1] = -_edgeA.Normal;
_proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2);
_proxyA.Count = 2;
// Proxy for polygon
_proxyB.Count = polygonB.Vertices.Count;
_proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid);
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
_proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]);
_proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]);
}
_radius = 2.0f * Settings.PolygonRadius;
_limit11 = Vector2.Zero;
_limit12 = Vector2.Zero;
_limit21 = Vector2.Zero;
_limit22 = Vector2.Zero;
//Collide(ref manifold); inline start
manifold.PointCount = 0;
//ComputeAdjacency(); inline start
Vector2 v0 = _edgeA.V0;
Vector2 v1 = _edgeA.V1;
Vector2 v2 = _edgeA.V2;
Vector2 v3 = _edgeA.V3;
// Determine allowable the normal regions based on adjacency.
// Note: it may be possible that no normal is admissable.
Vector2 centerB = _proxyB.Centroid;
if (_edgeA.HasVertex0)
{
Vector2 e0 = v1 - v0;
Vector2 e1 = v2 - v1;
Vector2 n0 = new Vector2(e0.Y, -e0.X);
Vector2 n1 = new Vector2(e1.Y, -e1.X);
n0.Normalize();
n1.Normalize();
bool convex = MathUtils.Cross(n0, n1) >= 0.0f;
bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f;
bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
if (convex)
{
if (front0 || front1)
{
_limit11 = n1;
_limit12 = n0;
}
else
{
_limit11 = -n1;
_limit12 = -n0;
}
}
else
{
if (front0 && front1)
{
_limit11 = n0;
_limit12 = n1;
}
else
{
_limit11 = -n0;
_limit12 = -n1;
}
}
}
else
{
_limit11 = Vector2.Zero;
_limit12 = Vector2.Zero;
}
if (_edgeA.HasVertex3)
{
Vector2 e1 = v2 - v1;
Vector2 e2 = v3 - v2;
Vector2 n1 = new Vector2(e1.Y, -e1.X);
Vector2 n2 = new Vector2(e2.Y, -e2.X);
n1.Normalize();
n2.Normalize();
bool convex = MathUtils.Cross(n1, n2) >= 0.0f;
bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f;
if (convex)
{
if (front1 || front2)
{
_limit21 = n2;
_limit22 = n1;
}
else
{
_limit21 = -n2;
_limit22 = -n1;
}
}
else
{
if (front1 && front2)
{
_limit21 = n1;
_limit22 = n2;
}
else
{
_limit21 = -n1;
_limit22 = -n2;
}
}
}
else
{
_limit21 = Vector2.Zero;
_limit22 = Vector2.Zero;
}
//ComputeAdjacency(); inline end
//EPAxis edgeAxis = ComputeEdgeSeparation(); inline start
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
// This can happen on the middle edge of a 3-edge zig-zag chain.
if (edgeAxis.Type == EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > _radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.Type == EPAxisType.Unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
EPProxy proxy1;
EPProxy proxy2;
FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
if (primaryAxis.Type == EPAxisType.EdgeA)
{
proxy1 = _proxyA;
proxy2 = _proxyB;
manifold.Type = ManifoldType.FaceA;
}
else
{
proxy1 = _proxyB;
proxy2 = _proxyA;
manifold.Type = ManifoldType.FaceB;
}
int edge1 = primaryAxis.Index;
FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2);
int count1 = proxy1.Count;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vector2 v11 = proxy1.Vertices[iv1];
Vector2 v12 = proxy1.Vertices[iv2];
Vector2 tangent = v12 - v11;
tangent.Normalize();
Vector2 normal = MathUtils.Cross(tangent, 1.0f);
Vector2 planePoint = 0.5f * (v11 + v12);
// Face offset.
float frontOffset = Vector2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius;
float sideOffset2 = Vector2.Dot(tangent, v12) + _radius;
// Clip incident edge against extruded edge1 side edges.
FixedArray2<ClipVertex> clipPoints1;
FixedArray2<ClipVertex> clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = normal;
manifold.LocalPoint = planePoint;
}
else
{
manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal);
manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint);
}
int pointCount = 0;
for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1)
{
float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset;
if (separation <= _radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V);
cp.Id = clipPoints2[i1].ID;
}
else
{
cp.LocalPoint = clipPoints2[i1].V;
cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB;
cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA;
cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB;
cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
//Collide(ref manifold); inline end
}
public void Deserialize(World world, Stream stream)
{
world.Clear();
XMLFragmentElement root = XMLFragmentParser.LoadFromStream(stream);
if (root.Name.ToLower() != "world")
throw new Exception();
foreach (XMLFragmentElement main in root.Elements)
{
if (main.Name.ToLower() == "gravity")
{
world.Gravity = ReadVector(main);
}
}
foreach (XMLFragmentElement shapeElement in root.Elements)
{
if (shapeElement.Name.ToLower() == "shapes")
{
foreach (XMLFragmentElement n in shapeElement.Elements)
{
if (n.Name.ToLower() != "shape")
throw new Exception();
ShapeType type = (ShapeType)Enum.Parse(typeof(ShapeType), n.Attributes[0].Value, true);
switch (type)
{
case ShapeType.Circle:
{
CircleShape shape = new CircleShape();
foreach (XMLFragmentElement sn in n.Elements)
{
switch (sn.Name.ToLower())
{
case "radius":
shape.Radius = float.Parse(sn.Value);
break;
case "position":
shape.Position = ReadVector(sn);
break;
default:
throw new Exception();
}
}
_shapes.Add(shape);
}
break;
case ShapeType.Polygon:
{
PolygonShape shape = new PolygonShape();
foreach (XMLFragmentElement sn in n.Elements)
{
switch (sn.Name.ToLower())
{
case "vertices":
{
List<Vector2> verts = new List<Vector2>();
foreach (XMLFragmentElement vert in sn.Elements)
verts.Add(ReadVector(vert));
shape.Set(new Vertices(verts.ToArray()));
}
break;
case "centroid":
shape.MassData.Centroid = ReadVector(sn);
break;
}
}
_shapes.Add(shape);
}
break;
case ShapeType.Edge:
{
EdgeShape shape = new EdgeShape();
foreach (XMLFragmentElement sn in n.Elements)
{
switch (sn.Name.ToLower())
{
case "hasvertex0":
shape.HasVertex0 = bool.Parse(sn.Value);
break;
case "hasvertex3":
shape.HasVertex0 = bool.Parse(sn.Value);
break;
case "vertex0":
shape.Vertex0 = ReadVector(sn);
break;
case "vertex1":
shape.Vertex1 = ReadVector(sn);
break;
case "vertex2":
shape.Vertex2 = ReadVector(sn);
break;
case "vertex3":
shape.Vertex3 = ReadVector(sn);
break;
default:
throw new Exception();
}
}
_shapes.Add(shape);
}
break;
}
}
}
}
foreach (XMLFragmentElement fixtureElement in root.Elements)
{
if (fixtureElement.Name.ToLower() == "fixtures")
{
foreach (XMLFragmentElement n in fixtureElement.Elements)
{
Fixture fixture = new Fixture();
if (n.Name.ToLower() != "fixture")
throw new Exception();
foreach (XMLFragmentElement sn in n.Elements)
{
switch (sn.Name.ToLower())
{
case "shape":
fixture.Shape = _shapes[int.Parse(sn.Value)];
break;
case "density":
fixture.Shape.Density = float.Parse(sn.Value);
break;
case "filterdata":
foreach (XMLFragmentElement ssn in sn.Elements)
{
switch (ssn.Name.ToLower())
{
case "categorybits":
fixture._collisionCategories = (Category)int.Parse(ssn.Value);
break;
case "maskbits":
fixture._collidesWith = (Category)int.Parse(ssn.Value);
break;
case "groupindex":
fixture._collisionGroup = short.Parse(ssn.Value);
break;
}
}
break;
case "friction":
fixture.Friction = float.Parse(sn.Value);
break;
case "issensor":
fixture.IsSensor = bool.Parse(sn.Value);
break;
case "restitution":
fixture.Restitution = float.Parse(sn.Value);
break;
case "userdata":
fixture.UserData = ReadSimpleType(sn, null, false);
break;
}
}
_fixtures.Add(fixture);
}
}
}
foreach (XMLFragmentElement bodyElement in root.Elements)
{
if (bodyElement.Name.ToLower() == "bodies")
{
foreach (XMLFragmentElement n in bodyElement.Elements)
{
Body body = new Body(world);
if (n.Name.ToLower() != "body")
throw new Exception();
body.BodyType = (BodyType)Enum.Parse(typeof(BodyType), n.Attributes[0].Value, true);
foreach (XMLFragmentElement sn in n.Elements)
{
switch (sn.Name.ToLower())
{
case "active":
if (bool.Parse(sn.Value))
body.Flags |= BodyFlags.Enabled;
else
body.Flags &= ~BodyFlags.Enabled;
break;
case "allowsleep":
body.SleepingAllowed = bool.Parse(sn.Value);
break;
case "angle":
{
Vector2 position = body.Position;
body.SetTransformIgnoreContacts(ref position, float.Parse(sn.Value));
}
break;
case "angulardamping":
body.AngularDamping = float.Parse(sn.Value);
break;
case "angularvelocity":
body.AngularVelocity = float.Parse(sn.Value);
break;
case "awake":
body.Awake = bool.Parse(sn.Value);
break;
case "bullet":
body.IsBullet = bool.Parse(sn.Value);
break;
case "fixedrotation":
body.FixedRotation = bool.Parse(sn.Value);
break;
case "lineardamping":
body.LinearDamping = float.Parse(sn.Value);
break;
case "linearvelocity":
body.LinearVelocity = ReadVector(sn);
break;
case "position":
{
float rotation = body.Rotation;
Vector2 position = ReadVector(sn);
body.SetTransformIgnoreContacts(ref position, rotation);
}
break;
case "userdata":
body.UserData = ReadSimpleType(sn, null, false);
break;
case "fixtures":
{
foreach (XMLFragmentElement v in sn.Elements)
{
Fixture blueprint = _fixtures[int.Parse(v.Value)];
Fixture f = new Fixture(body, blueprint.Shape, blueprint.CollisionCategories);
f.Restitution = blueprint.Restitution;
f.UserData = blueprint.UserData;
f.Friction = blueprint.Friction;
f.CollidesWith = blueprint.CollidesWith;
f.CollisionGroup = blueprint.CollisionGroup;
}
break;
}
}
}
_bodies.Add(body);
}
}
}
foreach (XMLFragmentElement jointElement in root.Elements)
{
if (jointElement.Name.ToLower() == "joints")
{
foreach (XMLFragmentElement n in jointElement.Elements)
{
Joint joint;
if (n.Name.ToLower() != "joint")
throw new Exception();
JointType type = (JointType)Enum.Parse(typeof(JointType), n.Attributes[0].Value, true);
int bodyAIndex = -1, bodyBIndex = -1;
bool collideConnected = false;
object userData = null;
foreach (XMLFragmentElement sn in n.Elements)
{
switch (sn.Name.ToLower())
{
case "bodya":
bodyAIndex = int.Parse(sn.Value);
break;
case "bodyb":
bodyBIndex = int.Parse(sn.Value);
break;
case "collideconnected":
collideConnected = bool.Parse(sn.Value);
break;
case "userdata":
userData = ReadSimpleType(sn, null, false);
break;
}
}
Body bodyA = _bodies[bodyAIndex];
Body bodyB = _bodies[bodyBIndex];
switch (type)
{
case JointType.Distance:
joint = new DistanceJoint();
break;
case JointType.Friction:
joint = new FrictionJoint();
break;
case JointType.Line:
joint = new LineJoint();
break;
case JointType.Prismatic:
joint = new PrismaticJoint();
break;
case JointType.Pulley:
joint = new PulleyJoint();
break;
case JointType.Revolute:
joint = new RevoluteJoint();
break;
case JointType.Weld:
joint = new WeldJoint();
break;
case JointType.Rope:
joint = new RopeJoint();
break;
case JointType.Angle:
joint = new AngleJoint();
break;
case JointType.Slider:
joint = new SliderJoint();
break;
case JointType.Gear:
throw new Exception("GearJoint is not supported.");
default:
throw new Exception("Invalid or unsupported joint.");
}
joint.CollideConnected = collideConnected;
joint.UserData = userData;
joint.BodyA = bodyA;
joint.BodyB = bodyB;
_joints.Add(joint);
world.AddJoint(joint);
foreach (XMLFragmentElement sn in n.Elements)
{
// check for specific nodes
switch (type)
{
case JointType.Distance:
{
switch (sn.Name.ToLower())
{
case "dampingratio":
((DistanceJoint)joint).DampingRatio = float.Parse(sn.Value);
break;
case "frequencyhz":
((DistanceJoint)joint).Frequency = float.Parse(sn.Value);
break;
case "length":
((DistanceJoint)joint).Length = float.Parse(sn.Value);
break;
case "localanchora":
((DistanceJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((DistanceJoint)joint).LocalAnchorB = ReadVector(sn);
break;
}
}
break;
case JointType.Friction:
{
switch (sn.Name.ToLower())
{
case "localanchora":
((FrictionJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((FrictionJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "maxforce":
((FrictionJoint)joint).MaxForce = float.Parse(sn.Value);
break;
case "maxtorque":
((FrictionJoint)joint).MaxTorque = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Line:
{
switch (sn.Name.ToLower())
{
case "enablemotor":
((LineJoint)joint).MotorEnabled = bool.Parse(sn.Value);
break;
case "localanchora":
((LineJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((LineJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "motorspeed":
((LineJoint)joint).MotorSpeed = float.Parse(sn.Value);
break;
case "dampingratio":
((LineJoint)joint).DampingRatio = float.Parse(sn.Value);
break;
case "maxmotortorque":
((LineJoint)joint).MaxMotorTorque = float.Parse(sn.Value);
break;
case "frequencyhz":
((LineJoint)joint).Frequency = float.Parse(sn.Value);
break;
case "localxaxis":
((LineJoint)joint).LocalXAxis = ReadVector(sn);
break;
}
}
break;
case JointType.Prismatic:
{
switch (sn.Name.ToLower())
{
case "enablelimit":
((PrismaticJoint)joint).LimitEnabled = bool.Parse(sn.Value);
break;
case "enablemotor":
((PrismaticJoint)joint).MotorEnabled = bool.Parse(sn.Value);
break;
case "localanchora":
((PrismaticJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((PrismaticJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "local1axis1":
((PrismaticJoint)joint).LocalXAxis1 = ReadVector(sn);
break;
case "maxmotorforce":
((PrismaticJoint)joint).MaxMotorForce = float.Parse(sn.Value);
break;
case "motorspeed":
((PrismaticJoint)joint).MotorSpeed = float.Parse(sn.Value);
break;
case "lowertranslation":
((PrismaticJoint)joint).LowerLimit = float.Parse(sn.Value);
break;
case "uppertranslation":
((PrismaticJoint)joint).UpperLimit = float.Parse(sn.Value);
break;
case "referenceangle":
((PrismaticJoint)joint).ReferenceAngle = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Pulley:
{
switch (sn.Name.ToLower())
{
case "groundanchora":
((PulleyJoint)joint).GroundAnchorA = ReadVector(sn);
break;
case "groundanchorb":
((PulleyJoint)joint).GroundAnchorB = ReadVector(sn);
break;
case "lengtha":
((PulleyJoint)joint).LengthA = float.Parse(sn.Value);
break;
case "lengthb":
((PulleyJoint)joint).LengthB = float.Parse(sn.Value);
break;
case "localanchora":
((PulleyJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((PulleyJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "maxlengtha":
((PulleyJoint)joint).MaxLengthA = float.Parse(sn.Value);
break;
case "maxlengthb":
((PulleyJoint)joint).MaxLengthB = float.Parse(sn.Value);
break;
case "ratio":
((PulleyJoint)joint).Ratio = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Revolute:
{
switch (sn.Name.ToLower())
{
case "enablelimit":
((RevoluteJoint)joint).LimitEnabled = bool.Parse(sn.Value);
break;
case "enablemotor":
((RevoluteJoint)joint).MotorEnabled = bool.Parse(sn.Value);
break;
case "localanchora":
((RevoluteJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((RevoluteJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "maxmotortorque":
((RevoluteJoint)joint).MaxMotorTorque = float.Parse(sn.Value);
break;
case "motorspeed":
((RevoluteJoint)joint).MotorSpeed = float.Parse(sn.Value);
break;
case "lowerangle":
((RevoluteJoint)joint).LowerLimit = float.Parse(sn.Value);
break;
case "upperangle":
((RevoluteJoint)joint).UpperLimit = float.Parse(sn.Value);
break;
case "referenceangle":
((RevoluteJoint)joint).ReferenceAngle = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Weld:
{
switch (sn.Name.ToLower())
{
case "localanchora":
((WeldJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((WeldJoint)joint).LocalAnchorB = ReadVector(sn);
break;
}
}
break;
case JointType.Rope:
{
switch (sn.Name.ToLower())
{
case "localanchora":
((RopeJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((RopeJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "maxlength":
((RopeJoint)joint).MaxLength = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Gear:
throw new Exception("Gear joint is unsupported");
case JointType.Angle:
{
switch (sn.Name.ToLower())
{
case "biasfactor":
((AngleJoint)joint).BiasFactor = float.Parse(sn.Value);
break;
case "maximpulse":
((AngleJoint)joint).MaxImpulse = float.Parse(sn.Value);
break;
case "softness":
((AngleJoint)joint).Softness = float.Parse(sn.Value);
break;
case "targetangle":
((AngleJoint)joint).TargetAngle = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Slider:
{
switch (sn.Name.ToLower())
{
case "dampingratio":
((SliderJoint)joint).DampingRatio = float.Parse(sn.Value);
break;
case "frequencyhz":
((SliderJoint)joint).Frequency = float.Parse(sn.Value);
break;
case "maxlength":
((SliderJoint)joint).MaxLength = float.Parse(sn.Value);
break;
case "minlength":
((SliderJoint)joint).MinLength = float.Parse(sn.Value);
break;
case "localanchora":
((SliderJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((SliderJoint)joint).LocalAnchorB = ReadVector(sn);
break;
}
}
break;
}
}
}
}
}
}
public override Shape Clone()
{
EdgeShape clone = new EdgeShape();
clone.ShapeType = ShapeType;
clone._radius = _radius;
clone._density = _density;
clone.HasVertex0 = HasVertex0;
clone.HasVertex3 = HasVertex3;
clone.Vertex0 = Vertex0;
clone._vertex1 = _vertex1;
clone._vertex2 = _vertex2;
clone.Vertex3 = Vertex3;
clone.MassData = MassData;
return clone;
}
public override Shape Clone()
{
EdgeShape edge = new EdgeShape();
edge._radius = _radius;
edge._density = _density;
edge.HasVertex0 = HasVertex0;
edge.HasVertex3 = HasVertex3;
edge.Vertex0 = Vertex0;
edge._vertex1 = _vertex1;
edge._vertex2 = _vertex2;
edge.Vertex3 = Vertex3;
edge.MassData = MassData;
return edge;
}
/// <summary>
/// This method has been optimized to reduce garbage.
/// </summary>
/// <param name="edge">The cached edge to set properties on.</param>
/// <param name="index">The index.</param>
internal void GetChildEdge(EdgeShape edge, int index)
{
Debug.Assert(0 <= index && index < Vertices.Count - 1);
Debug.Assert(edge != null);
edge.ShapeType = ShapeType.Edge;
edge._radius = _radius;
edge.Vertex1 = Vertices[index + 0];
edge.Vertex2 = Vertices[index + 1];
if (index > 0)
{
edge.Vertex0 = Vertices[index - 1];
edge.HasVertex0 = true;
}
else
{
edge.Vertex0 = _prevVertex;
edge.HasVertex0 = _hasPrevVertex;
}
if (index < Vertices.Count - 2)
{
edge.Vertex3 = Vertices[index + 2];
edge.HasVertex3 = true;
}
else
{
edge.Vertex3 = _nextVertex;
edge.HasVertex3 = _hasNextVertex;
}
}
public bool CompareTo(EdgeShape shape)
{
return (HasVertex0 == shape.HasVertex0 &&
HasVertex3 == shape.HasVertex3 &&
Vertex0 == shape.Vertex0 &&
Vertex1 == shape.Vertex1 &&
Vertex2 == shape.Vertex2 &&
Vertex3 == shape.Vertex3);
}
private EdgeTest()
{
{
Body ground = BodyFactory.CreateBody(World);
Vector2 v1 = new Vector2(-10.0f, 0.0f);
Vector2 v2 = new Vector2(-7.0f, -2.0f);
Vector2 v3 = new Vector2(-4.0f, 0.0f);
Vector2 v4 = Vector2.Zero;
Vector2 v5 = new Vector2(4.0f, 0.0f);
Vector2 v6 = new Vector2(7.0f, 2.0f);
Vector2 v7 = new Vector2(10.0f, 0.0f);
EdgeShape shape = new EdgeShape(v1, v2);
shape.HasVertex3 = true;
shape.Vertex3 = v3;
ground.CreateFixture(shape);
shape.Set(v2, v3);
shape.HasVertex0 = true;
shape.HasVertex3 = true;
shape.Vertex0 = v1;
shape.Vertex3 = v4;
ground.CreateFixture(shape);
shape.Set(v3, v4);
shape.HasVertex0 = true;
shape.HasVertex3 = true;
shape.Vertex0 = v2;
shape.Vertex3 = v5;
ground.CreateFixture(shape);
shape.Set(v4, v5);
shape.HasVertex0 = true;
shape.HasVertex3 = true;
shape.Vertex0 = v3;
shape.Vertex3 = v6;
ground.CreateFixture(shape);
shape.Set(v5, v6);
shape.HasVertex0 = true;
shape.HasVertex3 = true;
shape.Vertex0 = v4;
shape.Vertex3 = v7;
ground.CreateFixture(shape);
shape.Set(v6, v7);
shape.HasVertex0 = true;
shape.Vertex0 = v5;
ground.CreateFixture(shape);
}
{
Body body = BodyFactory.CreateBody(World, new Vector2(-0.5f, 0.6f));
body.BodyType = BodyType.Dynamic;
body.SleepingAllowed = false;
CircleShape shape = new CircleShape(0.5f, 1);
_circleFixture = body.CreateFixture(shape);
}
{
Body body = BodyFactory.CreateBody(World, new Vector2(1.0f, 0.6f));
body.BodyType = BodyType.Dynamic;
body.SleepingAllowed = false;
PolygonShape shape = new PolygonShape(1);
shape.Vertices = PolygonTools.CreateRectangle(0.5f, 0.5f);
body.CreateFixture(shape);
}
}
/// <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;
}
private SliderCrankTest()
{
Body ground;
{
ground = BodyFactory.CreateBody(World);
EdgeShape shape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape);
}
{
Body prevBody = ground;
// Define crank.
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(0.5f, 2.0f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 7.0f);
body.CreateFixture(shape);
_joint1 = new RevoluteJoint(prevBody, body, new Vector2(0f, 5f), true);
_joint1.MotorSpeed = 1.0f * Settings.Pi;
_joint1.MaxMotorTorque = 10000.0f;
_joint1.MotorEnabled = true;
World.AddJoint(_joint1);
prevBody = body;
}
// Define follower.
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(0.5f, 4.0f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 13.0f);
body.CreateFixture(shape);
RevoluteJoint rjd3 = new RevoluteJoint(prevBody, body, new Vector2(0, 9), true);
rjd3.MotorEnabled = false;
World.AddJoint(rjd3);
prevBody = body;
}
// Define piston
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(1.5f, 1.5f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 17.0f);
body.CreateFixture(shape);
RevoluteJoint rjd = new RevoluteJoint(prevBody, body, new Vector2(0, 17), true);
World.AddJoint(rjd);
_joint2 = new PrismaticJoint(ground, body, new Vector2(0.0f, 17.0f), new Vector2(0.0f, 1.0f), true);
_joint2.MaxMotorForce = 1000.0f;
_joint2.MotorEnabled = true;
World.AddJoint(_joint2);
}
// Create a payload
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(1.5f, 1.5f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 23.0f);
body.CreateFixture(shape);
}
}
}
private CantileverTest()
{
Body ground;
{
ground = BodyFactory.CreateBody(World);
EdgeShape shape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape);
}
{
Vertices box = PolygonTools.CreateRectangle(0.5f, 0.125f);
PolygonShape shape = new PolygonShape(box, 20);
Body prevBody = ground;
for (int i = 0; i < Count; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-14.5f + 1.0f * i, 5.0f);
body.CreateFixture(shape);
Vector2 anchor = new Vector2(-15.0f + 1.0f * i, 5.0f);
WeldJoint jd = new WeldJoint(prevBody, body, prevBody.GetLocalPoint(anchor),
body.GetLocalPoint(anchor));
World.AddJoint(jd);
prevBody = body;
}
}
{
Vertices box = PolygonTools.CreateRectangle(0.5f, 0.125f);
PolygonShape shape = new PolygonShape(box, 20);
Body prevBody = ground;
for (int i = 0; i < Count; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-14.5f + 1.0f * i, 15.0f);
body.CreateFixture(shape);
Vector2 anchor = new Vector2(-15.0f + 1.0f * i, 15.0f);
WeldJoint jd = new WeldJoint(prevBody, body, prevBody.GetLocalPoint(anchor),
body.GetLocalPoint(anchor));
World.AddJoint(jd);
prevBody = body;
}
}
{
Vertices box = PolygonTools.CreateRectangle(0.5f, 0.125f);
PolygonShape shape = new PolygonShape(box, 20);
Body prevBody = ground;
for (int i = 0; i < Count; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-4.5f + 1.0f * i, 5.0f);
body.CreateFixture(shape);
if (i > 0)
{
Vector2 anchor = new Vector2(-5.0f + 1.0f * i, 5.0f);
WeldJoint jd = new WeldJoint(prevBody, body, prevBody.GetLocalPoint(anchor),
body.GetLocalPoint(anchor));
World.AddJoint(jd);
}
prevBody = body;
}
}
{
Vertices box = PolygonTools.CreateRectangle(0.5f, 0.125f);
PolygonShape shape = new PolygonShape(box, 20);
Body prevBody = ground;
for (int i = 0; i < Count; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(5.5f + 1.0f * i, 10.0f);
body.CreateFixture(shape);
if (i > 0)
{
Vector2 anchor = new Vector2(5.0f + 1.0f * i, 10.0f);
WeldJoint jd = new WeldJoint(prevBody, body, prevBody.GetLocalPoint(anchor),
body.GetLocalPoint(anchor));
World.AddJoint(jd);
}
prevBody = body;
}
}
//Triangels
Vertices vertices = new Vertices(3);
vertices.Add(new Vector2(-0.5f, 0.0f));
vertices.Add(new Vector2(0.5f, 0.0f));
vertices.Add(new Vector2(0.0f, 1.5f));
for (int i = 0; i < 2; ++i)
{
PolygonShape shape = new PolygonShape(vertices, 1);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-8.0f + 8.0f * i, 12.0f);
body.CreateFixture(shape);
}
//Circles
for (int i = 0; i < 2; ++i)
{
CircleShape shape = new CircleShape(0.5f, 1);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-6.0f + 6.0f * i, 10.0f);
body.CreateFixture(shape);
}
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
Debug.Assert(childIndex < Vertices.Count);
EdgeShape edgeShape = new EdgeShape();
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
edgeShape.Vertex1 = Vertices[i1];
edgeShape.Vertex2 = Vertices[i2];
return edgeShape.RayCast(out output, ref input, ref transform, 0);
}
