public TimeOfImpact()
{
m_shapeA = new PolygonShape();
m_shapeB = new PolygonShape();
m_shapeA.SetAsBox(25.0f, 5.0f);
m_shapeB.SetAsBox(2.5f, 2.5f);
}
public ShapeEditing()
{
{
BodyDef bd1 = new BodyDef();
Body ground = m_world.CreateBody(bd1);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 10.0f);
m_body = m_world.CreateBody(bd);
PolygonShape shape2 = new PolygonShape();
shape2.SetAsBox(4.0f, 4.0f, new Vec2(0.0f, 0.0f), 0.0f);
shape2.Density = 10;
m_fixture1 = m_body.CreateFixture(shape2);
m_fixture2 = null;
m_sensor = false;
}
public override void Step(TestSettings settings)
{
base.Step(settings);
PolygonShape shape = new PolygonShape();
shape.Set(m_points, e_count);
m_debugDraw.DrawString("Press g to generate a new random convex hull");
m_debugDraw.DrawPolygon(shape.m_vertices, shape.m_count, Color.FromArgb(225, 225, 225));
for (int i = 0; i < e_count; ++i)
{
m_debugDraw.DrawPoint(m_points[i], 2.0f, Color.FromArgb(225, 128, 128));
//m_debugDraw.DrawString(m_points[i] + new Vec2(0.05f, 0.05f), "%d", i);
}
if (shape.Validate() == false)
{
m_textLine += 0;
}
if (m_auto)
{
Generate();
}
}
//e_columnCount = 1,
//e_rowCount = 1
public VerticalStack()
{
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
shape.Set(new Vec2(20.0f, 0.0f), new Vec2(20.0f, 20.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
float[] xs = {0.0f, -10.0f, -5.0f, 5.0f, 10.0f};
for (int j = 0; j < e_columnCount; ++j)
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
fd.friction = 0.3f;
for (int i = 0; i < e_rowCount; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
int n = j * e_rowCount + i;
Utilities.Assert(n < e_rowCount * e_columnCount);
m_indices[n] = n;
bd.UserData = m_indices[n];
float x = 0.0f;
//float x = RandomFloat(-0.02f, 0.02f);
//float x = i % 2 == 0 ? -0.025f : 0.025f;
bd.Position.Set(xs[j] + x, 0.752f + 1.54f * i);
Body body = m_world.CreateBody(bd);
m_bodies[n] = body;
body.CreateFixture(fd);
}
}
m_bullet = null;
}
public GravityTest() {
// Define the gravity vector.
Vec2 gravity = new Vec2(0.0f, 10.0f);
// Construct a world object, which will hold and simulate the rigid bodies.
m_world.SetGravity(gravity);
// Define the ground body.
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0.0f, 20.0f);
// Call the body factory which allocates memory for the ground body
// from a pool and creates the ground box shape (also from a pool).
// The body is also added to the world.
Body groundBody = m_world.CreateBody(groundBodyDef);
// Define the ground box shape.
PolygonShape groundBox = new PolygonShape();
// The extents are the half-widths of the box.
groundBox.SetAsBox(50.0f, 10.0f);
groundBox.Density = 0;
// Add the ground fixture to the ground body.
groundBody.CreateFixture(groundBox);
// Define the dynamic body. We set its position and call the body factory.
BodyDef bodyDef = new BodyDef();
bodyDef.type = BodyType._dynamicBody;
bodyDef.Position = new Vec2(0.0f, 4.0f);
Body body = m_world.CreateBody(bodyDef);
// Define another box shape for our dynamic body.
PolygonShape dynamicBox = new PolygonShape();
dynamicBox.SetAsBox(1.0f, 1.0f);
// Define the dynamic body fixture.
FixtureDef fixtureDef = new FixtureDef();
fixtureDef.shape = dynamicBox;
// Set the box Density to be non-zero, so it will be dynamic.
fixtureDef.Density = 1.0f;
// Override the default friction.
fixtureDef.friction = 0.3f;
// Add the shape to the body.
body.CreateFixture(fixtureDef);
}
public PolyCollision()
{
{
m_polygonA = new PolygonShape();
m_polygonA.SetAsBox(0.2f, 0.4f);
m_transformA.Set(new Vec2(0.0f, 0.0f), 0.0f);
}
{
m_polygonB = new PolygonShape();
m_polygonB.SetAsBox(0.5f, 0.5f);
m_positionB.Set(19.345284f, 1.5632932f);
m_angleB = 1.9160721f;
m_transformB.Set(m_positionB, m_angleB);
}
}
public OneSidedPlatform()
{
// Ground
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-20.0f, 0.0f), new Vec2(20.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
// Platform
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 10.0f);
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(3.0f, 0.5f);
shape.Density = 0;
m_platform = body.CreateFixture(shape);
m_bottom = 10.0f - 0.5f;
m_top = 10.0f + 0.5f;
}
// Actor
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 12.0f);
Body body = m_world.CreateBody(bd);
m_radius = 0.5f;
CircleShape shape = new CircleShape();
shape.m_radius = m_radius;
shape.Density = 20;
m_character = body.CreateFixture(shape);
body.SetLinearVelocity(new Vec2(0.0f, -50.0f));
m_state = State.e_unknown;
}
}
public Prismatic() {
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.0f, 0.5f);
shape.Density = 5;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-10.0f, 10.0f);
bd.angle = 0.5f * (float)Math.PI;
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
body.CreateFixture(shape);
PrismaticJointDef pjd = new PrismaticJointDef();
// Bouncy limit
Vec2 axis = new Vec2(2.0f, 1.0f);
axis.Normalize();
pjd.Initialize(ground, body, new Vec2(0.0f, 0.0f), axis);
// Non-bouncy limit
//pjd.Initialize(ground, body, new Vec2(-10.0f, 10.0f), new Vec2(1.0f, 0.0f));
pjd.motorSpeed = 10.0f;
pjd.maxMotorForce = 10000.0f;
pjd.enableMotor = true;
pjd.lowerTranslation = 0.0f;
pjd.upperTranslation = 20.0f;
pjd.enableLimit = true;
m_joint = (PrismaticJoint)m_world.CreateJoint(pjd);
}
}
public override void Step(TestSettings settings)
{
base.Step(settings);
if (m_count < e_count)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 10.0f);
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.125f, 0.125f);
body.CreateFixture(shape);
++m_count;
}
}
public BulletTest()
{
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 0.0f);
Body body = m_world.CreateBody(bd);
EdgeShape edge = new EdgeShape();
edge.Set(new Vec2(-10.0f, 0.0f), new Vec2(10.0f, 0.0f));
edge.Density = 0;
body.CreateFixture(edge);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.2f, 1.0f, new Vec2(0.5f, 1.0f), 0.0f);
shape.Density = 0;
body.CreateFixture(shape);
}
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 4.0f);
PolygonShape box = new PolygonShape();
box.SetAsBox(2.0f, 0.1f);
box.Density = 100;
m_body = m_world.CreateBody(bd);
m_body.CreateFixture(box);
box.SetAsBox(0.25f, 0.25f);
//m_x = RandomFloat(-1.0f, 1.0f);
m_x = 0.20352793f;
bd.Position.Set(m_x, 10.0f);
bd.bullet = true;
m_bullet = m_world.CreateBody(bd);
m_bullet.CreateFixture(box);
m_bullet.SetLinearVelocity(new Vec2(0.0f, -50.0f));
}
}
public Body AddNode(Body parent, Vec2 localAnchor, int depth, float offset, float a)
{
Vec2 h = new Vec2(0.0f, a);
Vec2 p = parent.GetPosition() + localAnchor - h;
BodyDef bodyDef = new BodyDef();
bodyDef.type = BodyType._dynamicBody;
bodyDef.Position = p;
Body body = m_world.CreateBody(bodyDef);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f * a, a);
shape.Density = 20;
body.CreateFixture(shape);
if (depth == e_depth)
{
return body;
}
shape.SetAsBox(offset, 0.25f * a, new Vec2(0, -a), 0.0f);
body.CreateFixture(shape);
Vec2 a1 = new Vec2(offset, -a);
Vec2 a2 = new Vec2(-offset, -a);
Body body1 = AddNode(body, a1, depth + 1, 0.5f * offset, a);
Body body2 = AddNode(body, a2, depth + 1, 0.5f * offset, a);
RevoluteJointDef jointDef = new RevoluteJointDef();
jointDef.bodyA = body;
jointDef.localAnchorB = h;
jointDef.localAnchorA = a1;
jointDef.bodyB = body1;
m_world.CreateJoint(jointDef);
jointDef.localAnchorA = a2;
jointDef.bodyB = body2;
m_world.CreateJoint(jointDef);
return body;
}
public Chain()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
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.type = BodyType._dynamicBody;
bd.Position.Set(0.5f + i, y);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
Vec2 anchor = new Vec2((float)(i), y);
jd.Initialize(prevBody, body, anchor);
m_world.CreateJoint(jd);
prevBody = body;
}
}
}
public Pyramid()
{
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
float a = 0.5f;
PolygonShape shape = new PolygonShape();
shape.SetAsBox(a, a);
shape.Density = 5;
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 < e_count; ++i)
{
y = x;
for (int j = i; j < e_count; ++j)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position = y;
Body body = m_world.CreateBody(bd);
body.CreateFixture(shape);
y += deltaY;
}
x += deltaX;
}
}
}
public MotorJointTest()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-20.0f, 0.0f), new Vec2(20.0f, 0.0f));
FixtureDef fd = new FixtureDef();
fd.shape = shape;
ground.CreateFixture(fd);
}
// Define motorized body
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 8.0f);
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.0f, 0.5f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.friction = 0.6f;
fd.Density = 2.0f;
body.CreateFixture(fd);
MotorJointDef mjd = new MotorJointDef();
mjd.Initialize(ground, body);
mjd.maxForce = 1000.0f;
mjd.maxTorque = 1000.0f;
m_joint = (MotorJoint)m_world.CreateJoint(mjd);
}
m_go = false;
m_time = 0.0f;
}
public ConveyorBelt()
{
// Ground
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-20.0f, 0.0f), new Vec2(20.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
// Platform
{
BodyDef bd = new BodyDef();
bd.Position.Set(-5.0f, 5.0f);
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(10.0f, 0.5f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.friction = 0.8f;
m_platform = body.CreateFixture(fd);
}
// Boxes
for (int i = 0; i < 5; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-10.0f + 2.0f * i, 7.0f);
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
shape.Density = 20;
body.CreateFixture(shape);
}
}
public Tumbler()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
}
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.allowSleep = false;
bd.Position.Set(0.0f, 10.0f);
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.Density = 5;
shape.SetAsBox(0.5f, 10.0f, new Vec2( 10.0f, 0.0f), 0.0f);
body.CreateFixture(shape);
shape.SetAsBox(0.5f, 10.0f, new Vec2(-10.0f, 0.0f), 0.0f);
body.CreateFixture(shape);
shape.SetAsBox(10.0f, 0.5f, new Vec2(0.0f, 10.0f), 0.0f);
body.CreateFixture(shape);
shape.SetAsBox(10.0f, 0.5f, new Vec2(0.0f, -10.0f), 0.0f);
body.CreateFixture(shape);
RevoluteJointDef jd = new RevoluteJointDef();
jd.bodyA = ground;
jd.bodyB = body;
jd.localAnchorA.Set(0.0f, 10.0f);
jd.localAnchorB.Set(0.0f, 0.0f);
jd.referenceAngle = 0.0f;
jd.motorSpeed = 0.05f * (float)Math.PI;
jd.maxMotorTorque = 1e8f;
jd.enableMotor = true;
m_joint = (RevoluteJoint)m_world.CreateJoint(jd);
}
m_count = 0;
}
public AddPair()
{
m_world.SetGravity(new Vec2(0.0f,0.0f));
{
CircleShape shape = new CircleShape();
shape.m_p.SetZero();
shape.m_radius = 0.1f;
float minX = -6.0f;
float maxX = 0.0f;
float minY = 4.0f;
float maxY = 6.0f;
for (int i = 0; i < 400; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position = new Vec2(RandomFloat(minX,maxX),RandomFloat(minY,maxY));
Body body = m_world.CreateBody(bd);
shape.Density = 0.01f;
body.CreateFixture(shape);
}
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.5f, 1.5f);
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-40.0f,5.0f);
bd.bullet = true;
Body body = m_world.CreateBody(bd);
body.CreateFixture(shape);
body.SetLinearVelocity(new Vec2(150.0f, 0.0f));
}
}
public ApplyForce() {
m_world.SetGravity(new Vec2(0.0f, 0.0f));
const float k_restitution = 0.4f;
Body ground;
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 20.0f);
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
FixtureDef sd = new FixtureDef();
sd.shape = shape;
sd.Density = 0.0f;
sd.restitution = k_restitution;
// Left vertical
shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(-20.0f, 20.0f));
ground.CreateFixture(sd);
// Right vertical
shape.Set(new Vec2(20.0f, -20.0f), new Vec2(20.0f, 20.0f));
ground.CreateFixture(sd);
// Top horizontal
shape.Set(new Vec2(-20.0f, 20.0f), new Vec2(20.0f, 20.0f));
ground.CreateFixture(sd);
// Bottom horizontal
shape.Set(new Vec2(-20.0f, -20.0f), new Vec2(20.0f, -20.0f));
ground.CreateFixture(sd);
}
{
Transform xf1 = new Transform();
xf1.q.Set(0.3524f * (float)Math.PI);
xf1.p = xf1.q.GetXAxis();
Vec2[] vertices = new Vec2[3];
vertices[0] = Utilities.Mul(xf1, new Vec2(-1.0f, 0.0f));
vertices[1] = Utilities.Mul(xf1, new Vec2(1.0f, 0.0f));
vertices[2] = Utilities.Mul(xf1, new Vec2(0.0f, 0.5f));
PolygonShape poly1 = new PolygonShape();
poly1.Set(vertices, 3);
FixtureDef sd1 = new FixtureDef();
sd1.shape = poly1;
sd1.Density = 4.0f;
Transform xf2 = new Transform();
xf2.q.Set(-0.3524f * (float)Math.PI);
xf2.p = -xf2.q.GetXAxis();
vertices[0] = Utilities.Mul(xf2, new Vec2(-1.0f, 0.0f));
vertices[1] = Utilities.Mul(xf2, new Vec2(1.0f, 0.0f));
vertices[2] = Utilities.Mul(xf2, new Vec2(0.0f, 0.5f));
PolygonShape poly2 = new PolygonShape();
poly2.Set(vertices, 3);
FixtureDef sd2 = new FixtureDef();
sd2.shape = poly2;
sd2.Density = 2.0f;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.angularDamping = 5.0f;
bd.linearDamping = 0.1f;
bd.Position.Set(0.0f, 2.0f);
bd.angle = (float)Math.PI;
bd.allowSleep = false;
m_body = m_world.CreateBody(bd);
m_body.CreateFixture(sd1);
m_body.CreateFixture(sd2);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
fd.friction = 0.3f;
for (int i = 0; i < 10; ++i) {
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 5.0f + 1.54f * i);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
float gravity = 10.0f;
float I = body.GetInertia();
float mass = body.GetMass();
// For a circle: I = 0.5 * m * r * r ==> r = sqrt(2 * I / m)
float radius = (float)Math.Sqrt(2.0f * I / mass);
FrictionJointDef jd = new FrictionJointDef();
jd.localAnchorA.SetZero();
jd.localAnchorB.SetZero();
jd.bodyA = ground;
jd.bodyB = body;
jd.collideConnected = true;
jd.maxForce = mass * gravity;
jd.maxTorque = mass * radius * gravity;
m_world.CreateJoint(jd);
}
}
}
public Pinball()
{
// Ground body
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
Vec2[] vs = new Vec2[5];
vs[0].Set(0.0f, -2.0f);
vs[1].Set(8.0f, 6.0f);
vs[2].Set(8.0f, 20.0f);
vs[3].Set(-8.0f, 20.0f);
vs[4].Set(-8.0f, 6.0f);
ChainShape loop = new ChainShape();
loop.CreateLoop(vs, 5);
FixtureDef fd = new FixtureDef();
fd.shape = loop;
fd.Density = 0.0f;
ground.CreateFixture(fd);
}
// Flippers
{
Vec2 p1 = new Vec2(-2.0f, 0.0f);
Vec2 p2 = new Vec2(2.0f, 0.0f);
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position = p1;
Body leftFlipper = m_world.CreateBody(bd);
bd.Position = p2;
Body rightFlipper = m_world.CreateBody(bd);
PolygonShape box = new PolygonShape();
box.SetAsBox(1.75f, 0.1f);
FixtureDef fd = new FixtureDef();
fd.shape = box;
fd.Density = 1.0f;
leftFlipper.CreateFixture(fd);
rightFlipper.CreateFixture(fd);
RevoluteJointDef jd = new RevoluteJointDef();
jd.bodyA = ground;
jd.localAnchorB.SetZero();
jd.enableMotor = true;
jd.maxMotorTorque = 1000.0f;
jd.enableLimit = true;
jd.motorSpeed = 0.0f;
jd.localAnchorA = p1;
jd.bodyB = leftFlipper;
jd.lowerAngle = -30.0f * (float)Math.PI / 180.0f;
jd.upperAngle = 5.0f * (float)Math.PI / 180.0f;
m_leftJoint = (RevoluteJoint)m_world.CreateJoint(jd);
jd.motorSpeed = 0.0f;
jd.localAnchorA = p2;
jd.bodyB = rightFlipper;
jd.lowerAngle = -5.0f * (float)Math.PI / 180.0f;
jd.upperAngle = 30.0f * (float)Math.PI / 180.0f;
m_rightJoint = (RevoluteJoint)m_world.CreateJoint(jd);
}
// Circle character
{
BodyDef bd = new BodyDef();
bd.Position.Set(1.0f, 15.0f);
bd.type = BodyType._dynamicBody;
bd.bullet = true;
m_ball = m_world.CreateBody(bd);
CircleShape shape = new CircleShape();
shape.m_radius = 0.2f;
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
m_ball.CreateFixture(fd);
}
m_button = false;
}
public CharacterCollision()
{
// Ground body
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-20.0f, 0.0f), new Vec2(20.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
// Collinear edges with no adjacency information.
// This shows the problematic case where a box shape can hit
// an internal vertex.
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Density = 0;
shape.Set(new Vec2(-8.0f, 1.0f), new Vec2(-6.0f, 1.0f));
ground.CreateFixture(shape);
shape.Set(new Vec2(-6.0f, 1.0f), new Vec2(-4.0f, 1.0f));
ground.CreateFixture(shape);
shape.Set(new Vec2(-4.0f, 1.0f), new Vec2(-2.0f, 1.0f));
ground.CreateFixture(shape);
}
// Chain shape
{
BodyDef bd = new BodyDef();
bd.angle = 0.25f * (float)Math.PI;
Body ground = m_world.CreateBody(bd);
Vec2[] vs = new Vec2[4];
vs[0].Set(5.0f, 7.0f);
vs[1].Set(6.0f, 8.0f);
vs[2].Set(7.0f, 8.0f);
vs[3].Set(8.0f, 7.0f);
ChainShape shape = new ChainShape();
shape.CreateChain(vs, 4);
shape.Density = 0;
ground.CreateFixture(shape);
}
// Square tiles. This shows that adjacency shapes may
// have non-smooth collision. There is no solution
// to this problem.
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.Density = 0;
shape.SetAsBox(1.0f, 1.0f, new Vec2(4.0f, 3.0f), 0.0f);
ground.CreateFixture(shape);
shape.SetAsBox(1.0f, 1.0f, new Vec2(6.0f, 3.0f), 0.0f);
ground.CreateFixture(shape);
shape.SetAsBox(1.0f, 1.0f, new Vec2(8.0f, 3.0f), 0.0f);
ground.CreateFixture(shape);
}
// Square made from an edge loop. Collision should be smooth.
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
Vec2[] vs = new Vec2[4];
vs[0].Set(-1.0f, 3.0f);
vs[1].Set(1.0f, 3.0f);
vs[2].Set(1.0f, 5.0f);
vs[3].Set(-1.0f, 5.0f);
ChainShape shape = new ChainShape();
shape.CreateLoop(vs, 4);
shape.Density = 0;
ground.CreateFixture(shape);
}
// Edge loop. Collision should be smooth.
{
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f, 4.0f);
Body ground = m_world.CreateBody(bd);
Vec2[] vs = new Vec2[10];
vs[0].Set(0.0f, 0.0f);
vs[1].Set(6.0f, 0.0f);
vs[2].Set(6.0f, 2.0f);
vs[3].Set(4.0f, 1.0f);
vs[4].Set(2.0f, 2.0f);
vs[5].Set(0.0f, 2.0f);
vs[6].Set(-2.0f, 2.0f);
vs[7].Set(-4.0f, 3.0f);
vs[8].Set(-6.0f, 2.0f);
vs[9].Set(-6.0f, 0.0f);
ChainShape shape = new ChainShape();
shape.CreateLoop(vs, 10);
shape.Density = 0;
ground.CreateFixture(shape);
}
// Square character 1
{
BodyDef bd = new BodyDef();
bd.Position.Set(-3.0f, 8.0f);
bd.type = BodyType._dynamicBody;
bd.fixedRotation = true;
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 20.0f;
body.CreateFixture(fd);
}
// Square character 2
{
BodyDef bd = new BodyDef();
bd.Position.Set(-5.0f, 5.0f);
bd.type = BodyType._dynamicBody;
bd.fixedRotation = true;
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.25f, 0.25f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 20.0f;
body.CreateFixture(fd);
}
// Hexagon character
{
BodyDef bd = new BodyDef();
bd.Position.Set(-5.0f, 8.0f);
bd.type = BodyType._dynamicBody;
bd.fixedRotation = true;
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
float angle = 0.0f;
float delta = (float)Math.PI / 3.0f;
Vec2[] vertices = new Vec2[6];
for (int i = 0; i < 6; ++i)
{
vertices[i].Set(0.5f * (float)Math.Cos(angle), 0.5f * (float)Math.Sin(angle));
angle += delta;
}
PolygonShape shape = new PolygonShape();
shape.Set(vertices, 6);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 20.0f;
body.CreateFixture(fd);
}
// Circle character
{
BodyDef bd = new BodyDef();
bd.Position.Set(3.0f, 5.0f);
bd.type = BodyType._dynamicBody;
bd.fixedRotation = true;
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
CircleShape shape = new CircleShape();
shape.m_radius = 0.5f;
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 20.0f;
body.CreateFixture(fd);
}
// Circle character
{
BodyDef bd = new BodyDef();
bd.Position.Set(-7.0f, 6.0f);
bd.type = BodyType._dynamicBody;
bd.allowSleep = false;
m_character = m_world.CreateBody(bd);
CircleShape shape = new CircleShape();
shape.m_radius = 0.25f;
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 20.0f;
fd.friction = 1.0f;
m_character.CreateFixture(fd);
}
}
public EdgeTest()
{
{
BodyDef bd = new BodyDef();
Body ground = m_world.CreateBody(bd);
Vec2 v1 = new Vec2(-10.0f, 0.0f), v2 = new Vec2(-7.0f, -2.0f), v3 = new Vec2(-4.0f, 0.0f);
Vec2 v4 = new Vec2(0.0f, 0.0f), v5 = new Vec2(4.0f, 0.0f), v6 = new Vec2(7.0f, 2.0f), v7 = new Vec2(10.0f, 0.0f);
EdgeShape shape = new EdgeShape();
shape.Set(v1, v2);
shape.m_hasVertex3 = true;
shape.m_vertex3 = v3;
shape.Density = 0;
ground.CreateFixture(shape);
shape.Set(v2, v3);
shape.m_hasVertex0 = true;
shape.m_hasVertex3 = true;
shape.m_vertex0 = v1;
shape.m_vertex3 = v4;
ground.CreateFixture(shape);
shape.Set(v3, v4);
shape.m_hasVertex0 = true;
shape.m_hasVertex3 = true;
shape.m_vertex0 = v2;
shape.m_vertex3 = v5;
ground.CreateFixture(shape);
shape.Set(v4, v5);
shape.m_hasVertex0 = true;
shape.m_hasVertex3 = true;
shape.m_vertex0 = v3;
shape.m_vertex3 = v6;
ground.CreateFixture(shape);
shape.Set(v5, v6);
shape.m_hasVertex0 = true;
shape.m_hasVertex3 = true;
shape.m_vertex0 = v4;
shape.m_vertex3 = v7;
ground.CreateFixture(shape);
shape.Set(v6, v7);
shape.m_hasVertex0 = true;
shape.m_vertex0 = v5;
ground.CreateFixture(shape);
}
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-0.5f, 0.6f);
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
CircleShape shape = new CircleShape();
shape.m_radius = 0.5f;
body.CreateFixture(shape);
}
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(1.0f, 0.6f);
bd.allowSleep = false;
Body body = m_world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 0.5f);
body.CreateFixture(shape);
}
}
// 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
public void Collide(out Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB){
manifold = new Manifold();
m_xf = Utilities.MulT(xfA, xfB);
m_centroidB = Utilities.Mul(m_xf, polygonB.m_centroid);
m_v0 = edgeA.m_vertex0;
m_v1 = edgeA.m_vertex1;
m_v2 = edgeA.m_vertex2;
m_v3 = edgeA.m_vertex3;
bool hasVertex0 = edgeA.m_hasVertex0;
bool hasVertex3 = edgeA.m_hasVertex3;
Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.Y, -edge1.X);
float offset1 = Utilities.Dot(m_normal1, m_centroidB - m_v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.Y, -edge0.X);
convex1 = Utilities.Cross(edge0, edge1) >= 0.0f;
offset0 = Utilities.Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.Y, -edge2.X);
convex2 = Utilities.Cross(edge1, edge2) > 0.0f;
offset2 = Utilities.Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i)
{
m_polygonB.vertices[i] = Utilities.Mul(m_xf, polygonB.m_vertices[i]);
m_polygonB.normals[i] = Utilities.Mul(m_xf.q, polygonB.m_normals[i]);
}
m_radius = 2.0f * Settings._polygonRadius;
manifold.points.Clear();
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == EPAxisType.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != EPAxisType.e_unknown && polygonAxis.separation > m_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.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
ClipVertex[] ie = new ClipVertex[2];
ReferenceFace rf = new ReferenceFace();
if (primaryAxis.type == EPAxisType.e_edgeA)
{
manifold.type = Manifold.ManifoldType.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Utilities.Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = Utilities.Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)i1;
ie[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
ie[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)i2;
ie[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
ie[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = Manifold.ManifoldType.e_faceB;
ie[0].v = m_v1;
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)primaryAxis.index;
ie[0].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
ie[0].id.cf.typeB = ContactFeature.FeatureType.e_face;
ie[1].v = m_v2;
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)primaryAxis.index;
ie[1].id.cf.typeA = ContactFeature.FeatureType.e_vertex;
ie[1].id.cf.typeB = ContactFeature.FeatureType.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1.Set(rf.normal.Y, -rf.normal.X);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = Utilities.Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Utilities.Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
ClipVertex[] clipPoints2 = new ClipVertex[2];
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings._maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings._maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == EPAxisType.e_edgeA)
{
manifold.localNormal = rf.normal;
manifold.localPoint = rf.v1;
}
else
{
manifold.localNormal = polygonB.m_normals[rf.i1];
manifold.localPoint = polygonB.m_vertices[rf.i1];
}
manifold.points.Clear();
for (int i = 0; i < Settings._maxManifoldPoints; ++i)
{
float separation;
separation = Utilities.Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
ManifoldPoint cp = new ManifoldPoint();
if (primaryAxis.type == EPAxisType.e_edgeA)
{
cp.localPoint = Utilities.MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
manifold.points.Add(cp);
}
}
}
Bridge()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
shape.Density = 0;
ground.CreateFixture(shape);
}
{
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();
Body prevBody = ground;
for (int i = 0; i < e_count; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-14.5f + 1.0f * i, 5.0f);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
Vec2 anchor = new Vec2(-15.0f + 1.0f * i, 5.0f);
jd.Initialize(prevBody, body, anchor);
m_world.CreateJoint(jd);
if (i == (e_count >> 1))
{
m_middle = body;
}
prevBody = body;
}
Vec2 anchor2 = new Vec2(-15.0f + 1.0f * e_count, 5.0f);
jd.Initialize(prevBody, ground, anchor2);
m_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.type = BodyType._dynamicBody;
bd.Position.Set(-8.0f + 8.0f * i, 12.0f);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
}
for (int i = 0; i < 3; ++i)
{
CircleShape shape = new CircleShape();
shape.m_radius = 0.5f;
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(-6.0f + 6.0f * i, 10.0f);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
}
}
/// Compute the collision manifold between an edge and a circle.
public static void CollideEdgeAndPolygon(out Manifold manifold,
EdgeShape edgeA, Transform xfA,
PolygonShape polygonB, Transform xfB) {
EPCollider collider = new EPCollider();
collider.Collide(out manifold, edgeA, xfA, polygonB, xfB);
}
/// Compute the collision manifold between a polygon and a circle.
public static void CollidePolygonAndCircle(out Manifold manifold,
PolygonShape polygonA, Transform xfA,
CircleShape circleB, Transform xfB) {
manifold = new Manifold();
manifold.points.Clear();
// Compute circle position in the frame of the polygon.
Vec2 c = Utilities.Mul(xfB, circleB.m_p);
Vec2 cLocal = Utilities.MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Single.MaxValue;
float radius = polygonA.m_radius + circleB.m_radius;
int vertexCount = polygonA.m_count;
List<Vec2> vertices = new List<Vec2>(polygonA.m_vertices);
List<Vec2> normals = new List<Vec2>(polygonA.m_normals);
for (int i = 0; i < vertexCount; ++i)
{
float s = Utilities.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 < Single.Epsilon)
{
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Utilities.Dot(cLocal - v1, v2 - v1);
float u2 = Utilities.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Utilities.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.points.Clear();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (Utilities.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.points = new List<ManifoldPoint>();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
}
else
{
Vec2 faceCenter = 0.5f * (v1 + v2);
float separation2 = Utilities.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation2 > radius)
{
return;
}
manifold.points = new List<ManifoldPoint>();
manifold.points.Add(new ManifoldPoint());
manifold.type = Manifold.ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id.key = 0;
}
}
// 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
/// Compute the collision manifold between two polygons.
public static void CollidePolygons(out Manifold manifold,
PolygonShape polyA, Transform xfA,
PolygonShape polyB, Transform xfB) {
manifold = new Manifold();
float totalRadius = polyA.m_radius + polyB.m_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 polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
bool 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.e_faceB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = Manifold.ManifoldType.e_faceA;
flip = false;
}
ClipVertex[] incidentEdge = new ClipVertex[2];
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_count;
Vec2[] vertices1 = poly1.m_vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vec2 v11 = vertices1[iv1];
Vec2 v12 = vertices1[iv2];
Vec2 localTangent = v12 - v11;
localTangent.Normalize();
Vec2 localNormal = Utilities.Cross(localTangent, 1.0f);
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 tangent = Utilities.Mul(xf1.q, localTangent);
Vec2 normal = Utilities.Cross(tangent, 1.0f);
v11 = Utilities.Mul(xf1, v11);
v12 = Utilities.Mul(xf1, v12);
// Face offset.
float frontOffset = Utilities.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Utilities.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Utilities.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = new ClipVertex[2];
ClipVertex[] clipPoints2 = new ClipVertex[2];
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
manifold.points.Clear();
for (int i = 0; i < Settings._maxManifoldPoints; ++i)
{
float separation = Utilities.Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = new ManifoldPoint();
cp.localPoint = Utilities.MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip)
{
// Swap features
ContactFeature cf = cp.id.cf;
cp.id.cf.indexA = cf.indexB;
cp.id.cf.indexB = cf.indexA;
cp.id.cf.typeA = cf.typeB;
cp.id.cf.typeB = cf.typeA;
}
manifold.points.Add(cp);
}
}
}
static void FindIncidentEdge(ClipVertex[/*2*/] c,
PolygonShape poly1, Transform xf1, int edge1,
PolygonShape poly2, Transform xf2)
{
Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_count;
Vec2[] vertices2 = poly2.m_vertices;
Vec2[] normals2 = poly2.m_normals;
Utilities.Assert(0 <= edge1 && edge1 < poly1.m_count);
// Get the normal of the reference edge in poly2's frame.
Vec2 normal1 = Utilities.MulT(xf2.q, Utilities.Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Single.MaxValue;
for (int i = 0; i < count2; ++i)
{
float dot = Utilities.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[0].v = Utilities.Mul(xf2, vertices2[i1]);
c[0].id.cf.indexA = (byte)edge1;
c[0].id.cf.indexB = (byte)i1;
c[0].id.cf.typeA = ContactFeature.FeatureType.e_face;
c[0].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
c[1].v = Utilities.Mul(xf2, vertices2[i2]);
c[1].id.cf.indexA = (byte)edge1;
c[1].id.cf.indexB = (byte)i2;
c[1].id.cf.typeA = ContactFeature.FeatureType.e_face;
c[1].id.cf.typeB = ContactFeature.FeatureType.e_vertex;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float FindMaxSeparation(out int edgeIndex,
PolygonShape poly1, Transform xf1,
PolygonShape poly2, Transform xf2)
{
int count1 = poly1.m_count;
Vec2[] normals1 = poly1.m_normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vec2 d = Utilities.Mul(xf2, poly2.m_centroid) - Utilities.Mul(xf1, poly1.m_centroid);
Vec2 dLocal1 = Utilities.MulT(xf1.q, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Single.MaxValue;
for (int i = 0; i < count1; ++i)
{
float dot = Utilities.Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// 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 = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
}
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1,
PolygonShape poly2, Transform xf2)
{
Vec2[] vertices1 = poly1.m_vertices;
Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_count;
Vec2[] vertices2 = poly2.m_vertices;
Utilities.Assert(0 <= edge1 && edge1 < poly1.m_count);
// Convert normal from poly1's frame into poly2's frame.
Vec2 normal1World = Utilities.Mul(xf1.q, normals1[edge1]);
Vec2 normal1 = Utilities.MulT(xf2.q, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Single.MaxValue;
for (int i = 0; i < count2; ++i)
{
float dot = Utilities.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vec2 v1 = Utilities.Mul(xf1, vertices1[edge1]);
Vec2 v2 = Utilities.Mul(xf2, vertices2[index]);
float separation = Utilities.Dot(v2 - v1, normal1World);
return separation;
}
public Revolute()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
shape.Set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
FixtureDef fd = new FixtureDef();
fd.shape = shape;
//fd.Filter.CategoryBits = 2;
ground.CreateFixture(fd);
}
{
CircleShape shape = new CircleShape();
shape.m_radius = 0.5f;
shape.Density = 5;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
RevoluteJointDef rjd = new RevoluteJointDef();
bd.Position.Set(-10.0f, 20.0f);
Body body = m_world.CreateBody(bd);
body.CreateFixture(shape);
float w = 100.0f;
body.SetAngularVelocity(w);
body.SetLinearVelocity(new Vec2(-8.0f * w, 0.0f));
rjd.Initialize(ground, body, new Vec2(-10.0f, 12.0f));
rjd.motorSpeed = 1.0f * (float)Math.PI;
rjd.maxMotorTorque = 10000.0f;
rjd.enableMotor = false;
rjd.lowerAngle = -0.25f * (float)Math.PI;
rjd.upperAngle = 0.5f * (float)Math.PI;
rjd.enableLimit = true;
rjd.collideConnected = true;
m_joint = (RevoluteJoint)m_world.CreateJoint(rjd);
}
{
CircleShape circle_shape = new CircleShape();
circle_shape.m_radius = 3.0f;
BodyDef circle_bd = new BodyDef();
circle_bd.type = BodyType._dynamicBody;
circle_bd.Position.Set(5.0f, 30.0f);
FixtureDef fd = new FixtureDef();
fd.Density = 5.0f;
fd.Filter.MaskBits = 1;
fd.shape = circle_shape;
m_ball = m_world.CreateBody(circle_bd);
m_ball.CreateFixture(fd);
PolygonShape polygon_shape = new PolygonShape();
polygon_shape.SetAsBox(10.0f, 0.2f, new Vec2(-10.0f, 0.0f), 0.0f);
polygon_shape.Density = 2;
BodyDef polygon_bd = new BodyDef();
polygon_bd.Position.Set(20.0f, 10.0f);
polygon_bd.type = BodyType._dynamicBody;
polygon_bd.bullet = true;
Body polygon_body = m_world.CreateBody(polygon_bd);
polygon_body.CreateFixture(polygon_shape);
RevoluteJointDef rjd = new RevoluteJointDef();
rjd.Initialize(ground, polygon_body, new Vec2(20.0f, 10.0f));
rjd.lowerAngle = -0.25f * (float)Math.PI;
rjd.upperAngle = 0.0f * (float)Math.PI;
rjd.enableLimit = true;
m_world.CreateJoint(rjd);
}
// Tests mass computation of a small object far from the origin
{
BodyDef bodyDef = new BodyDef();
bodyDef.type = BodyType._dynamicBody;
Body body = m_world.CreateBody(bodyDef);
PolygonShape polyShape = new PolygonShape();
Vec2[] verts = new Vec2[3];
verts[0].Set( 17.63f, 36.31f );
verts[1].Set( 17.52f, 36.69f );
verts[2].Set( 17.19f, 36.36f );
polyShape.Set(verts, 3);
FixtureDef polyFixtureDef = new FixtureDef();
polyFixtureDef.shape = polyShape;
polyFixtureDef.Density = 1;
body.CreateFixture(polyFixtureDef); //assertion hits inside here
}
}
