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 static void CollideCircles(out Manifold manifold,
CircleShape circle1, Transform xf1,
CircleShape circle2, Transform xf2)
{
manifold = new Manifold();
manifold.PointCount = 0;
Vec2 p1 = Math.Mul(xf1, circle1._p);
Vec2 p2 = Math.Mul(xf2, circle2._p);
Vec2 d = p2 - p1;
float distSqr = Vec2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalPoint = circle1._p;
manifold.LocalPlaneNormal.SetZero();
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._p;
manifold.Points[0].ID.Key = 0;
}
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 override Shape Clone()
{
CircleShape shape = new CircleShape();
shape._p = this._p;
shape.Type = this.Type;
shape._radius = this._radius;
return shape;
}
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 static void CollideCircles(ref Manifold manifold,
CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
manifold.PointCount = 0;
Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition());
Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition());
Vec2 d = p2 - p1;
float distSqr = Vec2.Dot(d, d);
float r1 = circle1.GetRadius();
float r2 = circle2.GetRadius();
float radiusSum = r1 + r2;
if (distSqr > radiusSum * radiusSum)
{
return;
}
float separation;
if (distSqr < Common.Settings.FLT_EPSILON)
{
separation = -radiusSum;
manifold.Normal.Set(0.0f, 1.0f);
}
else
{
float dist = Common.Math.Sqrt(distSqr);
separation = dist - radiusSum;
float a = 1.0f / dist;
manifold.Normal.X = a * d.X;
manifold.Normal.Y = a * d.Y;
}
manifold.PointCount = 1;
manifold.Points[0].ID.Key = 0;
manifold.Points[0].Separation = separation;
p1 += r1 * manifold.Normal;
p2 -= r2 * manifold.Normal;
Vec2 p = 0.5f * (p1 + p2);
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p);
}
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, Transform xf1, CircleShape circle2, Transform xf2)
{
manifold.PointCount = 0;
Vector2 p1 = xf1.TransformPoint(circle1._position);
Vector2 p2 = xf2.TransformPoint(circle2._position);
Vector2 d = p2 - p1;
float distSqr = Vector2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circle1._position;
manifold.LocalPlaneNormal = Vector2.zero;
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._position;
manifold.Points[0].ID.Key = 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);
}
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;
}
}
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 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 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);
}
}
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;
}
public CompoundShapes()
{
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 0.0f);
Body body = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(50.0f, 0.0f), new Vec2(-50.0f, 0.0f));
body.CreateFixture(shape, 0);
}
{
CircleShape circle1 = new CircleShape();
circle1._radius = 0.5f;
circle1._p.Set(-0.5f, 0.5f);
CircleShape circle2 = new CircleShape();
circle2._radius = 0.5f;
circle2._p.Set(0.5f, 0.5f);
for (int i = 0; i < 10; ++i)
{
float x = Math.Random(-0.1f, 0.1f);
BodyDef bd = new BodyDef();
bd.Position.Set(x + 5.0f, 1.05f + 2.5f * i);
bd.Angle = Math.Random(-Box2DX.Common.Settings.PI, Box2DX.Common.Settings.PI);
Body body = _world.CreateBody(bd);
body.CreateFixture(circle1, 2.0f);
body.CreateFixture(circle2, 0);
}
}
{
PolygonShape polygon1 = new PolygonShape();
polygon1.SetAsBox(0.25f, 0.5f);
PolygonShape polygon2 = new PolygonShape();
polygon2.SetAsBox(0.25f, 0.5f, new Vec2(0.0f, -0.5f), 0.5f * Box2DX.Common.Settings.PI);
for (int i = 0; i < 10; ++i)
{
float x = Math.Random(-0.1f, 0.1f);
BodyDef bd = new BodyDef();
bd.Position.Set(x - 5.0f, 1.05f + 2.5f * i);
bd.Angle = Math.Random(-Box2DX.Common.Settings.PI, Box2DX.Common.Settings.PI);
Body body = _world.CreateBody(bd);
body.CreateFixture(polygon1, 2.0f);
body.CreateFixture(polygon2, 2.0f);
}
}
{
Transform xf1 = new Transform();
xf1.R.Set(0.3524f * Box2DX.Common.Settings.PI);
xf1.Position = Math.Mul(xf1.R, new Vec2(1.0f, 0.0f));
Vec2[] vertices = new Vec2[3];
PolygonShape triangle1 = new PolygonShape();
vertices[0] = Math.Mul(xf1, new Vec2(-1.0f, 0.0f));
vertices[1] = Math.Mul(xf1, new Vec2(1.0f, 0.0f));
vertices[2] = Math.Mul(xf1, new Vec2(0.0f, 0.5f));
triangle1.Set(vertices, 3);
Transform xf2 = new Transform();
xf2.R.Set(-0.3524f * Box2DX.Common.Settings.PI);
xf2.Position = Math.Mul(xf2.R, new Vec2(-1.0f, 0.0f));
PolygonShape triangle2 = new PolygonShape();
vertices[0] = Math.Mul(xf2, new Vec2(-1.0f, 0.0f));
vertices[1] = Math.Mul(xf2, new Vec2(1.0f, 0.0f));
vertices[2] = Math.Mul(xf2, new Vec2(0.0f, 0.5f));
triangle2.Set(vertices, 3);
for (int i = 0; i < 10; ++i)
{
float x = Math.Random(-0.1f, 0.1f);
BodyDef bd = new BodyDef();
bd.Position.Set(x, 2.05f + 2.5f * i);
bd.Angle = 0.0f;
Body body = _world.CreateBody(bd);
body.CreateFixture(triangle1, 2.0f);
body.CreateFixture(triangle2, 2.0f);
}
}
{
PolygonShape bottom = new PolygonShape();
bottom.SetAsBox(1.5f, 0.15f);
PolygonShape left = new PolygonShape();
left.SetAsBox(0.15f, 2.7f, new Vec2(-1.45f, 2.35f), 0.2f);
PolygonShape right = new PolygonShape();
right.SetAsBox(0.15f, 2.7f, new Vec2(1.45f, 2.35f), -0.2f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 2.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(bottom, 4.0f);
body.CreateFixture(left, 4.0f);
body.CreateFixture(right, 4.0f);
}
}
private void CreateCircle()
{
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.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(Math.Random(), (2.0f + Math.Random()) * radius);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
}
public Bridge()
{
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.125f);
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 20.0f;
fd.Friction = 0.2f;
RevoluteJointDef jd = new RevoluteJointDef();
const int numPlanks = 30;
Body prevBody = ground;
for (int i = 0; i < _count; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(-14.5f + 1.0f * i, 5.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
Vec2 anchor = new Vec2(-15.0f + 1.0f * i, 5.0f);
jd.Initialize(prevBody, body, anchor);
_world.CreateJoint(jd);
if (i == (_count >> 1))
{
_middle = body;
}
prevBody = body;
}
Vec2 anchor2 = new Vec2(-15.0f + 1.0f * _count, 5.0f);
jd.Initialize(prevBody, ground, anchor2);
_world.CreateJoint(jd);
}
for (int i = 0; i < 2; ++i)
{
Vec2[] vertices = new Vec2[3];
vertices[0].Set(-0.5f, 0.0f);
vertices[1].Set(0.5f, 0.0f);
vertices[2].Set(0.0f, 1.5f);
PolygonShape shape = new PolygonShape();
shape.Set(vertices, 3);
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 1.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(-8.0f + 8.0f * i, 12.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
}
for (int i = 0; i < 3; ++i)
{
CircleShape shape = new CircleShape();
shape._radius = 0.5f;
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 1.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(-6.0f + 6.0f * i, 10.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
}
}
public Dominos()
{
Body b1;
{
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
BodyDef bd = new BodyDef();
b1 = _world.CreateBody(bd);
b1.CreateFixture(shape,0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(6.0f, 0.25f);
BodyDef bd = new BodyDef();
bd.Position.Set(-1.5f, 10.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(shape,0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.1f, 1.0f);
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 20.0f;
fd.Friction = 0.1f;
for (int i = 0; i < 10; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(-6.0f + 1.0f * i, 11.25f);
Body body = _world.CreateBody(bd);
body.CreateFixture(fd);
}
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(7.0f, 0.25f, Vec2.Zero, 0.3f);
BodyDef bd = new BodyDef();
bd.Position.Set(1.0f, 6.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(shape,0);
}
Body b2;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f, 1.5f);
BodyDef bd = new BodyDef();
bd.Position.Set(-7.0f, 4.0f);
b2 = _world.CreateBody(bd);
b2.CreateFixture(shape,0);
}
Body b3;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(6.0f, 0.125f);
BodyDef bd = new BodyDef();
bd.Position.Set(-0.9f, 1.0f);
bd.Angle = -0.15f;
b3 = _world.CreateBody(bd);
b3.CreateFixture(shape, 10.0f);
}
RevoluteJointDef jd = new RevoluteJointDef();
Vec2 anchor = new Vec2();
anchor.Set(-2.0f, 1.0f);
jd.Initialize(b1, b3, anchor);
jd.CollideConnected = true;
_world.CreateJoint(jd);
Body b4;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f, 0.25f);
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f, 15.0f);
b4 = _world.CreateBody(bd);
b4.CreateFixture(shape, 10.0f);
}
anchor.Set(-7.0f, 15.0f);
jd.Initialize(b2, b4, anchor);
_world.CreateJoint(jd);
Body b5;
{
BodyDef bd = new BodyDef();
bd.Position.Set(6.5f, 3.0f);
b5 = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 10.0f;
fd.Friction = 0.1f;
shape.SetAsBox(1.0f, 0.1f,new Vec2(0.0f, -0.9f), 0.0f);
b5.CreateFixture(fd);
shape.SetAsBox(0.1f, 1.0f, new Vec2(-0.9f, 0.0f), 0.0f);
b5.CreateFixture(fd);
shape.SetAsBox(0.1f, 1.0f,new Vec2(0.9f, 0.0f), 0.0f);
b5.CreateFixture(fd);
}
anchor.Set(6.0f, 2.0f);
jd.Initialize(b1, b5, anchor);
_world.CreateJoint(jd);
Body b6;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.0f, 0.1f);
BodyDef bd = new BodyDef();
bd.Position.Set(6.5f, 4.1f);
b6 = _world.CreateBody(bd);
b6.CreateFixture(shape, 30.0f);
}
anchor.Set(7.5f, 4.0f);
jd.Initialize(b5, b6, anchor);
_world.CreateJoint(jd);
Body b7;
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.1f, 1.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(7.4f, 1.0f);
b7 = _world.CreateBody(bd);
b7.CreateFixture(shape, 10.0f);
}
DistanceJointDef djd =new DistanceJointDef();
djd.Body1 = b3;
djd.Body2 = b7;
djd.LocalAnchor1.Set(6.0f, 0.0f);
djd.LocalAnchor2.Set(0.0f, -1.0f);
Vec2 d = djd.Body2.GetWorldPoint(djd.LocalAnchor2) - djd.Body1.GetWorldPoint(djd.LocalAnchor1);
djd.Length = d.Length();
_world.CreateJoint(djd);
{
float radius = 0.2f;
CircleShape shape = new CircleShape();
shape._radius = radius;
for (int i = 0; i < 4; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(5.9f + 2.0f * radius * i, 2.4f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 10.0f);
}
}
}
public void LaunchBomb(Vec2 position, Vec2 velocity)
{
if (_bomb != null)
{
_world.DestroyBody(_bomb);
_bomb = null;
}
BodyDef bd = new BodyDef();
bd.AllowSleep = true;
bd.Position = position;
bd.IsBullet = true;
_bomb = _world.CreateBody(bd);
_bomb.SetLinearVelocity(velocity);
CircleShape circle = new CircleShape();
circle._radius = 0.3f;
FixtureDef fd = new FixtureDef();
fd.Shape = circle;
fd.Density = 20.0f;
fd.Restitution = 0.1f;
Vec2 minV = position - new Vec2(0.3f, 0.3f);
Vec2 maxV = position + new Vec2(0.3f, 0.3f);
AABB aabb = new AABB();
aabb.LowerBound = minV;
aabb.UpperBound = maxV;
_bomb.CreateFixture(fd);
}
public PolyShapes()
{
// 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);
}
for (int i = 0; i < 4; i++)
{
polygons[i] = new PolygonShape();
}
{
Vec2[] vertices = new Vec2[3];
vertices[0].Set(-0.5f, 0.0f);
vertices[1].Set(0.5f, 0.0f);
vertices[2].Set(0.0f, 1.5f);
polygons[0].Set(vertices, 3);
}
{
Vec2[] vertices = new Vec2[3];
vertices[0].Set(-0.1f, 0.0f);
vertices[1].Set(0.1f, 0.0f);
vertices[2].Set(0.0f, 1.5f);
polygons[1].Set(vertices, 3);
}
{
float w = 1.0f;
float b = w / (2.0f + Math.Sqrt(2.0f));
float s = Math.Sqrt(2.0f) * b;
Vec2[] vertices = new Vec2[8];
vertices[0].Set(0.5f * s, 0.0f);
vertices[1].Set(0.5f * w, b);
vertices[2].Set(0.5f * w, b + s);
vertices[3].Set(0.5f * s, w);
vertices[4].Set(-0.5f * s, w);
vertices[5].Set(-0.5f * w, b + s);
vertices[6].Set(-0.5f * w, b);
vertices[7].Set(-0.5f * s, 0.0f);
polygons[2].Set(vertices, 8);
}
{
polygons[3].SetAsBox(0.5f, 0.5f);
}
{
circle = new CircleShape();
circle._radius = 0.5f;
}
bodyIndex = 0;
//memset(bodies, 0, sizeof(bodies));
}
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;
}
// This implements 2-sided edge vs circle collision.
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
{
manifold.PointCount = 0;
Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vector2 normal = edge._normal;
Vector2 v1 = edge._v1;
Vector2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if ((cLocal- v1).sqrMagnitude > 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)
{
// Ahead of v2
if ((cLocal- v2).sqrMagnitude > 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
{
float separation = Vector2.Dot(cLocal - v1, normal);
if (separation < -radius || radius < separation)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, Transform xf1, CircleShape circle2, Transform xf2)
{
manifold.PointCount = 0;
Vector2 p1 = xf1.TransformPoint(circle1._position);
Vector2 p2 = xf2.TransformPoint(circle2._position);
Vector2 d = p2 - p1;
float distSqr = Vector2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circle1._position;
manifold.LocalPlaneNormal = Vector2.zero;
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._position;
manifold.Points[0].ID.Key = 0;
}
// This implements 2-sided edge vs circle collision.
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
{
manifold.PointCount = 0;
Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vector2 normal = edge._normal;
Vector2 v1 = edge._v1;
Vector2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if ((cLocal - v1).LengthSquared > 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)
{
// Ahead of v2
if ((cLocal - v2).LengthSquared > 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
{
float separation = Vector2.Dot(cLocal - v1, normal);
if (separation < -radius || radius < separation)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, Transform xf1, CircleShape circle, Transform xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = xf2.TransformPoint(circle._position);
Vector2 cLocal = xf1.InverseTransformPoint(c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon._vertexCount;
Vector2[] vertices = polygon._vertices;
Vector2[] normals = polygon._normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vector2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 v1 = vertices[vertIndex1];
Vector2 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 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if ((cLocal - v1).sqrMagnitude > 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 ((cLocal - v2).sqrMagnitude > 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
{
Vector2 faceCenter = 0.5f * (v1 + v2);
float separation_ = Vector2.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;
}
}
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;
}
}