/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1._vertexCount;
Vec2[] vertices1 = poly1._vertices;
Vec2[] normals1 = poly1._normals;
int count2 = poly2._vertexCount;
Vec2[] vertices2 = poly2._vertices;
Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vec2 normal1World = Common.Math.Mul(xf1.R, normals1[edge1]);
Vec2 normal1 = Common.Math.MulT(xf2.R, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Common.Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vec2 v1 = Common.Math.Mul(xf1, vertices1[edge1]);
Vec2 v2 = Common.Math.Mul(xf2, vertices2[index]);
float separation = Vec2.Dot(v2 - v1, normal1World);
return separation;
}
public SphereStack()
{
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
CircleShape shape = new CircleShape();
shape._radius = 1.0f;
for (int i = 0; i < _count; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 4.0f + 3.0f * i);
_bodies[i] = _world.CreateBody(bd);
_bodies[i].CreateFixture(shape, 1.0f);
//m_bodies[i]->SetLinearVelocity(b2Vec2(0.0f, -100.0f));
}
}
}
public TimeOfImpact()
{
{
PolygonDef sd = new PolygonDef();
sd.Density = 0.0f;
sd.SetAsBox(0.1f, 10.0f, new Vec2(10.0f, 0.0f), 0.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 20.0f);
bd.Angle = 0.0f;
_body1 = _world.CreateBody(bd);
_shape1 = _body1.CreateShape(sd);
}
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.25f, 0.25f);
sd.Density = 1.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(9.6363468f, 28.050615f);
bd.Angle = 1.6408679f;
_body2 = _world.CreateBody(bd);
_shape2 = (PolygonShape)_body2.CreateShape(sd);
_body2.SetMassFromShapes();
}
}
public Breakable()
{
// Ground body
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
// Breakable dynamic body
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 40.0f);
bd.Angle = 0.25f * Box2DX.Common.Settings.PI;
_body1 = _world.CreateBody(bd);
_shape1.SetAsBox(0.5f, 0.5f, new Vec2(-0.5f, 0.0f), 0.0f);
_piece1 = _body1.CreateFixture(_shape1, 1.0f);
_shape2.SetAsBox(0.5f, 0.5f, new Vec2(0.5f, 0.0f), 0.0f);
_piece2 = _body1.CreateFixture(_shape2, 1.0f);
}
_break = false;
_broke = false;
}
public VaryingRestitution()
{
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
CircleShape shape = new CircleShape();
shape._radius = 1.0f;
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 1.0f;
float[] restitution = { 0.0f, 0.1f, 0.3f, 0.5f, 0.75f, 0.9f, 1.0f };
for (int i = 0; i < 7; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f + 3.0f * i, 20.0f);
Body body = _world.CreateBody(bd);
fd.Restitution = restitution[i];
body.CreateFixture(fd);
}
}
}
public Test()
{
AABB aabb = new AABB();
aabb.UpperBound = new UnityEngine.Vector2(100,100);
aabb.LowerBound = new UnityEngine.Vector2(-100,-100);
World world = new World(aabb, new Vector2(0, -1), false);
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0, 0);
PolygonShape groundBox = new PolygonShape();
groundBox.SetAsBox(10, 1);
Body body = new Body(groundBodyDef, world);
}
public TimeOfImpact()
{
{
_shapeA = new PolygonShape();
_shapeA.SetAsBox(10.0f, 0.2f);
}
{
_shapeB = new PolygonShape();
_shapeB.SetAsBox(2.0f, 0.1f);
}
}
public Confined()
{
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
// Floor
shape.SetAsEdge(new Vec2(-10.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
// Left wall
shape.SetAsEdge(new Vec2(-10.0f, 0.0f), new Vec2(-10.0f, 20.0f));
ground.CreateFixture(shape, 0);
// Right wall
shape.SetAsEdge(new Vec2(10.0f, 0.0f), new Vec2(10.0f, 20.0f));
ground.CreateFixture(shape, 0);
// Roof
shape.SetAsEdge(new Vec2(-10.0f, 20.0f), new Vec2(10.0f, 20.0f));
ground.CreateFixture(shape, 0);
}
{
float radius = 0.5f;
CircleShape shape = new CircleShape();
shape._p.SetZero();
shape._radius = radius;
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 1.0f;
fd.Friction = 0.1f;
for (int j = 0; j < _columnCount; ++j)
{
for (int i = 0; i < _rowCount; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f + (2.1f * j + 1.0f + 0.01f * i) * radius, (2.0f * i + 1.0f) * radius);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
}
}
}
_world.Gravity = new Vec2(0, 0);
}
public SensorTest()
{
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
{
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
#if false
{
b2FixtureDef sd;
sd.SetAsBox(10.0f, 2.0f, b2Vec2(0.0f, 20.0f), 0.0f);
sd.isSensor = true;
m_sensor = ground->CreateFixture(&sd);
}
#else
{
CircleShape shape = new CircleShape();
shape._radius = 5.0f;
shape._p.Set(0.0f, 10.0f);
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.IsSensor = true;
_sensor = ground.CreateFixture(fd);
}
#endif
}
{
CircleShape shape = new CircleShape();
shape._radius = 1.0f;
for (int i = 0; i < _count; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f + 3.0f * i, 20.0f);
bd.UserData = false;
_bodies[i] = _world.CreateBody(bd);
_bodies[i].CreateFixture(shape, 1.0f);
}
}
}
public DistanceTest()
{
{
_polygonA = new PolygonShape();
_transformA.SetIdentity();
_transformA.Position.Set(0.0f, -0.2f);
_polygonA.SetAsBox(10.0f, 0.2f);
}
{
_polygonB = new PolygonShape();
_positionB.Set(12.017401f, 0.13678508f);
_angleB = -0.0109265f;
_transformB.Set(_positionB, _angleB);
_polygonB.SetAsBox(2.0f, 0.1f);
}
}
public Chain()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.6f, 0.125f);
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 20.0f;
fd.Friction = 0.2f;
RevoluteJointDef jd = new RevoluteJointDef();
jd.CollideConnected = false;
const float y = 25.0f;
Body prevBody = ground;
for (int i = 0; i < 30; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.5f + i, y);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
Vec2 anchor = new Vec2(i, y);
jd.Initialize(prevBody, body, anchor);
_world.CreateJoint(jd);
prevBody = body;
}
}
}
public Pulleys()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
float a = 2.0f;
float b = 4.0f;
float y = 16.0f;
float L = 12.0f;
PolygonShape shape = new PolygonShape();
shape.SetAsBox(a, b);
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f, y);
Body body1 = _world.CreateBody(bd);
body1.CreateFixture(shape, 5.0f);
bd.Position.Set(10.0f, y);
Body body2 = _world.CreateBody(bd);
body2.CreateFixture(shape, 5.0f);
PulleyJointDef pulleyDef = new PulleyJointDef();
Vec2 anchor1 = new Vec2(-10.0f, y + b);
Vec2 anchor2 = new Vec2(10.0f, y + b);
Vec2 groundAnchor1 = new Vec2(-10.0f, y + b + L);
Vec2 groundAnchor2 = new Vec2(10.0f, y + b + L);
pulleyDef.Initialize(body1, body2, groundAnchor1, groundAnchor2, anchor1, anchor2, 2.0f);
_joint1 = (PulleyJoint)_world.CreateJoint(pulleyDef);
}
}
public Prismatic()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.0f, 0.5f);
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f, 10.0f);
bd.Angle = 0.5f * Box2DX.Common.Settings.PI;
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
PrismaticJointDef pjd = new PrismaticJointDef();
// Bouncy limit
pjd.Initialize(ground, body, new Vec2(0.0f, 0.0f), new Vec2(1.0f, 0.0f));
// Non-bouncy limit
//pjd.Initialize(ground, body, b2Vec2(-10.0f, 10.0f), b2Vec2(1.0f, 0.0f));
pjd.MotorSpeed = 10.0f;
pjd.MaxMotorForce = 1000.0f;
pjd.EnableMotor = true;
pjd.LowerTranslation = 0.0f;
pjd.UpperTranslation = 20.0f;
pjd.EnableLimit = true;
_joint = (PrismaticJoint)_world.CreateJoint(pjd);
}
}
public Pyramid()
{
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
float a = 0.5f;
PolygonShape shape = new PolygonShape();
shape.SetAsBox(a, a);
Vec2 x = new Vec2(-7.0f, 0.75f);
Vec2 y;
Vec2 deltaX = new Vec2(0.5625f, 1.25f);
Vec2 deltaY = new Vec2(1.125f, 0.0f);
for (int i = 0; i < _count; ++i)
{
y = x;
for (int j = i; j < _count; ++j)
{
BodyDef bd = new BodyDef();
bd.Position = y;
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
y += deltaY;
}
x += deltaX;
}
}
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
/// <param name="edgeIndex"></param>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public static float FindMaxSeparation(ref int edgeIndex,
PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vec2[] normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vec2 d = Common.Math.Mul(xf2, poly2.GetCentroid()) - Common.Math.Mul(xf1, poly1.GetCentroid());
Vec2 dLocal1 = Common.Math.MulT(xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Common.Settings.FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vec2.Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
if (sPrev > 0.0f)
{
return sPrev;
}
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
if (sNext > 0.0f)
{
return sNext;
}
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for (; ; )
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
}
public static void FindIncidentEdge(out ClipVertex[] c,
PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vec2[] vertices2 = poly2.GetVertices();
Vec2[] normals2 = poly2.Normals;
Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vec2 normal1 = Common.Math.MulT(xf2.R, Common.Math.Mul(xf1.R, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c = new ClipVertex[2];
c[0].V = Common.Math.Mul(xf2, vertices2[i1]);
c[0].ID.Features.ReferenceEdge = (byte)edge1;
c[0].ID.Features.IncidentEdge = (byte)i1;
c[0].ID.Features.IncidentVertex = 0;
c[1].V = Common.Math.Mul(xf2, vertices2[i2]);
c[1].ID.Features.ReferenceEdge = (byte)edge1;
c[1].ID.Features.IncidentEdge = (byte)i2;
c[1].ID.Features.IncidentVertex = 1;
}
// Polygon versus 2-sided edge.
public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, Transform TransformA, EdgeShape edge, Transform TransformB)
{
PolygonShape polygonB = new PolygonShape();
polygonB.SetAsEdge(edge._v1, edge._v2);
CollidePolygons(ref manifold, polygon, TransformA, polygonB, TransformB);
}
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.
Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition());
Vec2 cLocal = Common.Math.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = circle.GetRadius();
int vertexCount = polygon.VertexCount;
Vec2[] vertices = polygon.GetVertices();
Vec2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vec2.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 < Common.Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]);
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
Vec2 position = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position);
manifold.Points[0].LocalPoint2 = Common.Math.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;
Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
float length = e.Normalize();
Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
// Project the center onto the edge.
float u = Vec2.Dot(cLocal - vertices[vertIndex1], e);
Vec2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold.Points[0].ID.Features.IncidentEdge = Collision.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 = Collision.NullFeature;
}
Vec2 d = cLocal - p;
float dist = d.Normalize();
if (dist > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Normal = Common.Math.Mul(xf1.R, d);
Vec2 position_ = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_);
manifold.Points[0].Separation = dist - radius;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
}
public CollisionProcessing()
{
// Ground body
{
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-50.0f, 0.0f), new Vec2(50.0f, 0.0f));
FixtureDef sd = new FixtureDef();
sd.Shape = shape; ;
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
ground.CreateFixture(sd);
}
float xLo = -5.0f, xHi = 5.0f;
float yLo = 2.0f, yHi = 35.0f;
// Small triangle
Vec2[] vertices = new Vec2[3];
vertices[0].Set(-1.0f, 0.0f);
vertices[1].Set(1.0f, 0.0f);
vertices[2].Set(0.0f, 2.0f);
PolygonShape polygon = new PolygonShape();
polygon.Set(vertices, 3);
FixtureDef triangleShapeDef = new FixtureDef();
triangleShapeDef.Shape = polygon;
triangleShapeDef.Density = 1.0f;
BodyDef triangleBodyDef = new BodyDef();
triangleBodyDef.Position.Set(Math.Random(xLo, xHi), Math.Random(yLo, yHi));
Body body1 = _world.CreateBody(triangleBodyDef);
body1.CreateFixture(triangleShapeDef);
// Large triangle (recycle definitions)
vertices[0] *= 2.0f;
vertices[1] *= 2.0f;
vertices[2] *= 2.0f;
polygon.Set(vertices, 3);
triangleBodyDef.Position.Set(Math.Random(xLo, xHi), Math.Random(yLo, yHi));
Body body2 = _world.CreateBody(triangleBodyDef);
body2.CreateFixture(triangleShapeDef);
// Small box
polygon.SetAsBox(1.0f, 0.5f);
FixtureDef boxShapeDef = new FixtureDef();
boxShapeDef.Shape = polygon;
boxShapeDef.Density = 1.0f;
BodyDef boxBodyDef = new BodyDef();
boxBodyDef.Position.Set(Math.Random(xLo, xHi), Math.Random(yLo, yHi));
Body body3 = _world.CreateBody(boxBodyDef);
body3.CreateFixture(boxShapeDef);
// Large box (recycle definitions)
polygon.SetAsBox(2.0f, 1.0f);
boxBodyDef.Position.Set(Math.Random(xLo, xHi), Math.Random(yLo, yHi));
Body body4 = _world.CreateBody(boxBodyDef);
body4.CreateFixture(boxShapeDef);
// Small circle
CircleShape circle = new CircleShape();
circle._radius = 1.0f;
FixtureDef circleShapeDef = new FixtureDef();
circleShapeDef.Shape = circle;
circleShapeDef.Density = 1.0f;
BodyDef circleBodyDef = new BodyDef();
circleBodyDef.Position.Set(Math.Random(xLo, xHi), Math.Random(yLo, yHi));
Body body5 = _world.CreateBody(circleBodyDef);
body5.CreateFixture(circleShapeDef);
// Large circle
circle._radius *= 2.0f;
circleBodyDef.Position.Set(Math.Random(xLo, xHi), Math.Random(yLo, yHi));
Body body6 = _world.CreateBody(circleBodyDef);
body6.CreateFixture(circleShapeDef);
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(out Manifold manifold,
PolygonShape polyA, Transform xfA,
PolygonShape polyB, Transform xfB)
{
manifold = new Manifold();
manifold.PointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = Manifold.ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = Manifold.ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.Vertices;
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 localTangent = v12 - v11;
localTangent.Normalize();
Vec2 localNormal = Vec2.Cross(localTangent, 1.0f);
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 tangent = Math.Mul(xf1.R, localTangent);
Vec2 normal = Vec2.Cross(tangent, 1.0f);
v11 = Math.Mul(xf1, v11);
v12 = Math.Mul(xf1, v12);
// Face offset.
float frontOffset = Vec2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(normal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = Math.MulT(xf2, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
public void Create(BroadPhase broadPhase, Body body, XForm xf, FixtureDef def)
{
UserData = def.UserData;
Friction = def.Friction;
Restitution = def.Restitution;
Density = def.Density;
_body = body;
_next = null;
Filter = def.Filter;
_isSensor = def.IsSensor;
_type = def.Type;
// Allocate and initialize the child shape.
switch (_type)
{
case ShapeType.CircleShape:
{
CircleShape circle = new CircleShape();
CircleDef circleDef = (CircleDef)def;
circle._position = circleDef.LocalPosition;
circle._radius = circleDef.Radius;
_shape = circle;
}
break;
case ShapeType.PolygonShape:
{
PolygonShape polygon = new PolygonShape();
PolygonDef polygonDef = (PolygonDef)def;
polygon.Set(polygonDef.Vertices, polygonDef.VertexCount);
_shape = polygon;
}
break;
case ShapeType.EdgeShape:
{
EdgeShape edge = new EdgeShape();
EdgeDef edgeDef = (EdgeDef)def;
edge.Set(edgeDef.Vertex1, edgeDef.Vertex2);
_shape = edge;
}
break;
default:
Box2DXDebug.Assert(false);
break;
}
// Create proxy in the broad-phase.
AABB aabb;
_shape.ComputeAABB(out aabb, xf);
bool inRange = broadPhase.InRange(aabb);
// You are creating a shape outside the world box.
Box2DXDebug.Assert(inRange);
if (inRange)
{
_proxyId = broadPhase.CreateProxy(aabb, this);
}
else
{
_proxyId = PairManager.NullProxy;
}
}
internal static Shape Create(ShapeDef def)
{
Shape result;
switch (def.Type)
{
case ShapeType.CircleShape:
{
result = new CircleShape(def);
break;
}
case ShapeType.PolygonShape:
{
result = new PolygonShape(def);
break;
}
default:
{
Box2DXDebug.Assert(false);
result = null;
break;
}
}
return result;
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
{
manifold.PointCount = 0;
int edgeA = 0;
float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
return;
int edgeB = 0;
float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
return;
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
XForm xf1, xf2;
int edge1; // reference edge
byte flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = 0;
}
ClipVertex[] incidentEdge;
Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.GetVertices();
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 dv = v12 - v11;
Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f);
v11 = Common.Math.Mul(xf1, v11);
v12 = Common.Math.Mul(xf1, v12);
float frontOffset = Vec2.Dot(frontNormal, v11);
float sideOffset1 = -Vec2.Dot(sideNormal, v11);
float sideOffset2 = Vec2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
return;
// Now clipPoints2 contains the clipped points.
manifold.Normal = flip!=0 ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= 0.0f)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.Separation = separation;
cp.LocalPoint1 = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V);
cp.LocalPoint2 = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
private void CreateLeg(float s, Vec2 wheelAnchor)
{
Vec2 p1 = new Vec2(5.4f * s, -6.1f);
Vec2 p2 = new Vec2(7.2f * s, -1.2f);
Vec2 p3 = new Vec2(4.3f * s, -1.9f);
Vec2 p4 = new Vec2(3.1f * s, 0.8f);
Vec2 p5 = new Vec2(6.0f * s, 1.5f);
Vec2 p6 = new Vec2(2.5f * s, 3.7f);
FixtureDef fd1 = new FixtureDef();
FixtureDef fd2 = new FixtureDef();
fd1.Filter.GroupIndex = -1;
fd2.Filter.GroupIndex = -1;
fd1.Density = 1.0f;
fd2.Density = 1.0f;
PolygonShape poly1 = new PolygonShape();
PolygonShape poly2 = new PolygonShape();
if (s > 0.0f)
{
Vec2[] vertices = new Vec2[3];
vertices[0] = p1;
vertices[1] = p2;
vertices[2] = p3;
poly1.Set(vertices, 3);
vertices[0] = Vec2.Zero;
vertices[1] = p5 - p4;
vertices[2] = p6 - p4;
poly2.Set(vertices, 3);
}
else
{
Vec2[] vertices = new Vec2[3];
vertices[0] = p1;
vertices[1] = p3;
vertices[2] = p2;
poly1.Set(vertices, 3);
vertices[0] = Vec2.Zero;
vertices[1] = p6 - p4;
vertices[2] = p5 - p4;
poly2.Set(vertices, 3);
}
fd1.Shape = poly1;
fd2.Shape = poly2;
BodyDef bd1 = new BodyDef();
BodyDef bd2 = new BodyDef();
bd1.Position = _offset;
bd2.Position = p4 + _offset;
bd1.AngularDamping = 10.0f;
bd2.AngularDamping = 10.0f;
Body body1 = _world.CreateBody(bd1);
Body body2 = _world.CreateBody(bd2);
body1.CreateFixture(fd1);
body2.CreateFixture(fd2);
DistanceJointDef djd = new DistanceJointDef();
// Using a soft distance constraint can reduce some jitter.
// It also makes the structure seem a bit more fluid by
// acting like a suspension system.
djd.DampingRatio = 0.5f;
djd.FrequencyHz = 10.0f;
djd.Initialize(body1, body2, p2 + _offset, p5 + _offset);
_world.CreateJoint(djd);
djd.Initialize(body1, body2, p3 + _offset, p4 + _offset);
_world.CreateJoint(djd);
djd.Initialize(body1, _wheel, p3 + _offset, wheelAnchor + _offset);
_world.CreateJoint(djd);
djd.Initialize(body2, _wheel, p6 + _offset, wheelAnchor + _offset);
_world.CreateJoint(djd);
RevoluteJointDef rjd = new RevoluteJointDef();
rjd.Initialize(body2, _chassis, p4 + _offset);
_world.CreateJoint(rjd);
}
public Web()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
BodyDef bd = new BodyDef();
bd.Position.Set(-5.0f, 5.0f);
_bodies[0] = _world.CreateBody(bd);
_bodies[0].CreateFixture(shape, 0);
bd.Position.Set(5.0f, 5.0f);
_bodies[1] = _world.CreateBody(bd);
_bodies[1].CreateFixture(shape, 0);
bd.Position.Set(5.0f, 15.0f);
_bodies[2] = _world.CreateBody(bd);
_bodies[2].CreateFixture(shape, 0);
bd.Position.Set(-5.0f, 15.0f);
_bodies[3] = _world.CreateBody(bd);
_bodies[3].CreateFixture(shape, 0);
DistanceJointDef jd = new DistanceJointDef();
Vec2 p1, p2, d;
jd.FrequencyHz = 4.0f;
jd.DampingRatio = 0.5f;
jd.Body1 = ground;
jd.Body2 = _bodies[0];
jd.LocalAnchor1.Set(-10.0f, 10.0f);
jd.LocalAnchor2.Set(-0.5f, -0.5f);
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[0] = _world.CreateJoint(jd);
jd.Body1 = ground;
jd.Body2 = _bodies[1];
jd.LocalAnchor1.Set(10.0f, 10.0f);
jd.LocalAnchor2.Set(0.5f, -0.5f);
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[1] = _world.CreateJoint(jd);
jd.Body1 = ground;
jd.Body2 = _bodies[2];
jd.LocalAnchor1.Set(10.0f, 30.0f);
jd.LocalAnchor2.Set(0.5f, 0.5f);
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[2] = _world.CreateJoint(jd);
jd.Body1 = ground;
jd.Body2 = _bodies[3];
jd.LocalAnchor1.Set(-10.0f, 30.0f);
jd.LocalAnchor2.Set(-0.5f, 0.5f);
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[3] = _world.CreateJoint(jd);
jd.Body1 = _bodies[0];
jd.Body2 = _bodies[1];
jd.LocalAnchor1.Set(0.5f, 0.0f);
jd.LocalAnchor2.Set(-0.5f, 0.0f); ;
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[4] = _world.CreateJoint(jd);
jd.Body1 = _bodies[1];
jd.Body2 = _bodies[2];
jd.LocalAnchor1.Set(0.0f, 0.5f);
jd.LocalAnchor2.Set(0.0f, -0.5f);
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[5] = _world.CreateJoint(jd);
jd.Body1 = _bodies[2];
jd.Body2 = _bodies[3];
jd.LocalAnchor1.Set(-0.5f, 0.0f);
jd.LocalAnchor2.Set(0.5f, 0.0f);
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[6] = _world.CreateJoint(jd);
jd.Body1 = _bodies[3];
jd.Body2 = _bodies[0];
jd.LocalAnchor1.Set(0.0f, -0.5f);
jd.LocalAnchor2.Set(0.0f, 0.5f);
p1 = jd.Body1.GetWorldPoint(jd.LocalAnchor1);
p2 = jd.Body2.GetWorldPoint(jd.LocalAnchor2);
d = p2 - p1;
jd.Length = d.Length();
_joints[7] = _world.CreateJoint(jd);
}
}
public static void CollidePolygonAndCircle(
out Manifold manifold,
PolygonShape polygon, Transform xf1,
CircleShape circle, Transform xf2)
{
manifold = new Manifold();
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vec2 c = Math.Mul(xf2, circle._p);
Vec2 cLocal = Math.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon.VertexCount;
Vec2[] vertices = polygon.Vertices;
Vec2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vec2.Dot(normals[i], cLocal - vertices[i]);
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;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._p;
manifold.Points[0].ID.Key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v1;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points[0].LocalPoint = circle._p;
manifold.Points[0].ID.Key = 0;
}
else if (u2 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v2;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points[0].LocalPoint = circle._p;
manifold.Points[0].ID.Key = 0;
}
else
{
Vec2 faceCenter = 0.5f * (v1 + v2);
separation = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points[0].LocalPoint = circle._p;
manifold.Points[0].ID.Key = 0;
}
}
public override Shape Clone()
{
PolygonShape polygonShape = new PolygonShape();
polygonShape.Centroid = Centroid;
polygonShape.Normals = Normals;
polygonShape._radius = _radius;
polygonShape.Type = Type;
polygonShape.VertexCount = VertexCount;
polygonShape.Vertices = Vertices;
return polygonShape;
}
public Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(50.0f, 0.0f), new Vec2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
CircleShape circle1 = new CircleShape();
circle1._radius = 1.0f;
CircleShape circle2 = new CircleShape();
circle2._radius = 2.0f;
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.Position.Set(-3.0f, 12.0f);
Body body1 = _world.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.Body1 = ground;
jd1.Body2 = body1;
jd1.LocalAnchor1 = ground.GetLocalPoint(bd1.Position);
jd1.LocalAnchor2 = body1.GetLocalPoint(bd1.Position);
jd1.ReferenceAngle = body1.GetAngle() - ground.GetAngle();
_joint1 = (RevoluteJoint)_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.Position.Set(0.0f, 12.0f);
Body body2 = _world.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
_joint2 = (RevoluteJoint)_world.CreateJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.Position.Set(2.5f, 12.0f);
Body body3 = _world.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.Position, new Vec2(0.0f, 1.0f));
jd3.LowerTranslation = -5.0f;
jd3.UpperTranslation = 5.0f;
jd3.EnableLimit = true;
_joint3 = (PrismaticJoint)_world.CreateJoint(jd3);
GearJointDef jd4 = new GearJointDef();
jd4.Body1 = body1;
jd4.Body2 = body2;
jd4.Joint1 = _joint1;
jd4.Joint2 = _joint2;
jd4.Ratio = circle2._radius / circle1._radius;
_joint4 = (GearJoint)_world.CreateJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.Body1 = body2;
jd5.Body2 = body3;
jd5.Joint1 = _joint2;
jd5.Joint2 = _joint3;
jd5.Ratio = -1.0f / circle2._radius;
_joint5 = (GearJoint)_world.CreateJoint(jd5);
}
}
public CollisionFiltering()
{
// Ground body
{
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
FixtureDef sd = new FixtureDef();
sd.Shape = shape;
sd.Friction = 0.3f;
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
ground.CreateFixture(sd);
}
// Small triangle
Vec2[] vertices = new Vec2[3];
vertices[0].Set(-1.0f, 0.0f);
vertices[1].Set(1.0f, 0.0f);
vertices[2].Set(0.0f, 2.0f);
PolygonShape polygon = new PolygonShape();
polygon.Set(vertices, 3);
FixtureDef triangleShapeDef = new FixtureDef();
triangleShapeDef.Shape = polygon;
triangleShapeDef.Density = 1.0f;
triangleShapeDef.Filter.GroupIndex = k_smallGroup;
triangleShapeDef.Filter.CategoryBits = k_triangleCategory;
triangleShapeDef.Filter.MaskBits = k_triangleMask;
BodyDef triangleBodyDef = new BodyDef();
triangleBodyDef.Position.Set(-5.0f, 2.0f);
Body body1 = _world.CreateBody(triangleBodyDef);
body1.CreateFixture(triangleShapeDef);
// Large triangle (recycle definitions)
vertices[0] *= 2.0f;
vertices[1] *= 2.0f;
vertices[2] *= 2.0f;
polygon.Set(vertices, 3);
triangleShapeDef.Filter.GroupIndex = k_largeGroup;
triangleBodyDef.Position.Set(-5.0f, 6.0f);
triangleBodyDef.FixedRotation = true; // look at me!
Body body2 = _world.CreateBody(triangleBodyDef);
body2.CreateFixture(triangleShapeDef);
// Small box
polygon.SetAsBox(1.0f, 0.5f);
FixtureDef boxShapeDef = new FixtureDef();
boxShapeDef.Shape = polygon;
boxShapeDef.Density = 1.0f;
boxShapeDef.Restitution = 0.1f;
boxShapeDef.Filter.GroupIndex = k_smallGroup;
boxShapeDef.Filter.CategoryBits = k_boxCategory;
boxShapeDef.Filter.MaskBits = k_boxMask;
BodyDef boxBodyDef = new BodyDef();
boxBodyDef.Position.Set(0.0f, 2.0f);
Body body3 = _world.CreateBody(boxBodyDef);
body3.CreateFixture(boxShapeDef);
// Large box (recycle definitions)
polygon.SetAsBox(2.0f, 1.0f);
boxShapeDef.Filter.GroupIndex = k_largeGroup;
boxBodyDef.Position.Set(0.0f, 6.0f);
Body body4 = _world.CreateBody(boxBodyDef);
body4.CreateFixture(boxShapeDef);
// Small circle
CircleShape circle = new CircleShape();
circle._radius = 1.0f;
FixtureDef circleShapeDef = new FixtureDef();
circleShapeDef.Shape = circle;
circleShapeDef.Density = 1.0f;
circleShapeDef.Filter.GroupIndex = k_smallGroup;
circleShapeDef.Filter.CategoryBits = k_circleCategory;
circleShapeDef.Filter.MaskBits = k_circleMask;
BodyDef circleBodyDef = new BodyDef();
circleBodyDef.Position.Set(5.0f, 2.0f);
Body body5 = _world.CreateBody(circleBodyDef);
body5.CreateFixture(circleShapeDef);
// Large circle
circle._radius *= 2.0f;
circleShapeDef.Filter.GroupIndex = k_largeGroup;
circleBodyDef.Position.Set(5.0f, 6.0f);
Body body6 = _world.CreateBody(circleBodyDef);
body6.CreateFixture(circleShapeDef);
}
public TheoJansen()
{
_offset.Set(0.0f, 8.0f);
_motorSpeed = 2.0f;
_motorOn = true;
Vec2 pivot = new Vec2(0.0f, 0.8f);
// Ground
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-50.0f, 0.0f), new Vec2(50.0f, 0.0f));
ground.CreateFixture(shape, 0);
shape.SetAsEdge(new Vec2(-50.0f, 0.0f), new Vec2(-50.0f, 10.0f));
ground.CreateFixture(shape, 0);
shape.SetAsEdge(new Vec2(50.0f, 0.0f), new Vec2(50.0f, 10.0f));
ground.CreateFixture(shape, 0);
}
// Balls
for (int i = 0; i < 40; ++i)
{
CircleShape shape = new CircleShape();
shape._radius = 0.25f;
BodyDef bd = new BodyDef();
bd.Position.Set(-40.0f + 2.0f * i, 0.5f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 1.0f);
}
// Chassis
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.5f, 1.0f);
FixtureDef sd = new FixtureDef();
sd.Density = 1.0f;
sd.Shape = shape;
sd.Filter.GroupIndex = -1;
BodyDef bd = new BodyDef();
bd.Position = pivot + _offset;
_chassis = _world.CreateBody(bd);
_chassis.CreateFixture(sd);
}
{
CircleShape shape = new CircleShape();
shape._radius = 1.6f;
FixtureDef sd = new FixtureDef();
sd.Density = 1.0f;
sd.Shape = shape;
sd.Filter.GroupIndex = -1;
BodyDef bd = new BodyDef();
bd.Position = pivot + _offset;
_wheel = _world.CreateBody(bd);
_wheel.CreateFixture(sd);
}
{
RevoluteJointDef jd = new RevoluteJointDef();
jd.Initialize(_wheel, _chassis, pivot + _offset);
jd.CollideConnected = false;
jd.MotorSpeed = _motorSpeed;
jd.MaxMotorTorque = 400.0f;
jd.EnableMotor = _motorOn;
_motorJoint = (RevoluteJoint)_world.CreateJoint(jd);
}
Vec2 wheelAnchor;
wheelAnchor = pivot + new Vec2(0.0f, -0.8f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), 120.0f * Box2DX.Common.Settings.PI / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), -120.0f * Box2DX.Common.Settings.PI / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
}
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.
Vec2 c = Common.Math.Mul(xf2, circle._position);
Vec2 cLocal = Common.Math.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon._vertexCount;
Vec2[] vertices = polygon._vertices;
Vec2[] normals = polygon._normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vec2.Dot(normals[i], cLocal - vertices[i]);
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;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Common.Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v1;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else if (u2 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v2;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else
{
Vec2 faceCenter = 0.5f * (v1 + v2);
float separation_ = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation_ > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}