public override void Step(TestSettings settings)
{
base.Step(settings);
if (Utilities._gjkCalls > 0)
{
m_debugDraw.DrawString("gjk calls = %d, ave gjk iters = %3.1f, max gjk iters = %d",
Utilities._gjkCalls, Utilities._gjkIters / (float)Utilities._gjkCalls, Utilities._gjkMaxIters);
}
if (Utilities._toiCalls > 0)
{
m_debugDraw.DrawString("toi calls = %d, ave toi iters = %3.1f, max toi iters = %d",
Utilities._toiCalls, Utilities._toiIters / (float)Utilities._toiCalls, Utilities._toiMaxRootIters);
m_debugDraw.DrawString("ave toi root iters = %3.1f, max toi root iters = %d",
Utilities._toiRootIters / (float)(Utilities._toiCalls), Utilities._toiMaxRootIters);
}
if (m_stepCount % 60 == 0)
{
Launch();
}
}
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();
}
}
public override void Step(TestSettings settings)
{
Manifold manifold;
Collision.CollidePolygons(out manifold, m_polygonA, m_transformA, m_polygonB, m_transformB);
WorldManifold worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, m_transformA, m_polygonA.m_radius, m_transformB, m_polygonB.m_radius);
m_debugDraw.DrawString("point count = {0}", manifold.points.Count());
{
Color color = Color.FromArgb(225, 225, 225);
Vec2[] v = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < m_polygonA.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color);
for (int i = 0; i < m_polygonB.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color);
}
for (int i = 0; i < manifold.points.Count(); ++i)
{
m_debugDraw.DrawPoint(worldManifold.points[i], 4.0f, Color.FromArgb(225, 75, 75));
}
}
public override void Step(TestSettings settings) {
base.Step(settings);
m_debugDraw.DrawString("Keys: (l) limits, (m) motors, (s) speed");
float force = m_joint.GetMotorForce(settings.hz);
m_debugDraw.DrawString("Motor Force = %4.0f", (float)force);
}
public override void Step(TestSettings settings)
{
base.Step(settings);
m_debugDraw.DrawString("Press: (c) create a shape, (d) destroy a shape.");
m_debugDraw.DrawString("sensor = %d", m_sensor);
}
public override void Step(TestSettings settings)
{
base.Step(settings);
//DynamicTree tree = m_world.m_contactManager.m_broadPhase.m_tree;
//if (m_stepCount == 400)
//{
// tree.RebuildBottomUp();
//}
}
public override void Step(TestSettings settings)
{
base.Step(settings);
//for (int i = 0; i < e_count; ++i)
//{
// printf("%g ", m_bodies[i].GetWorldCenter().Y);
//}
//for (int i = 0; i < e_count; ++i)
//{
// printf("%g ", m_bodies[i].GetLinearVelocity().Y);
//}
//printf("\n");
}
public void Step(TestSettings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.proxyA.Set(m_polygonA, 0);
input.proxyB.Set(m_polygonB, 0);
input.transformA = m_transformA;
input.transformB = m_transformB;
input.useRadii = true;
SimplexCache cache = new SimplexCache();
cache.count = 0;
DistanceOutput output;
Utilities.Distance(out output, cache, input);
m_debugDraw.DrawString("distance = %g", output.distance);
m_debugDraw.DrawString("iterations = %d", output.iterations);
{
Color color = Color.FromArgb(225, 225, 225);
Vec2[] v = new Vec2[Settings._maxPolygonVertices];
for (int i = 0; i < m_polygonA.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformA, m_polygonA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.m_count, color);
for (int i = 0; i < m_polygonB.m_count; ++i)
{
v[i] = Utilities.Mul(m_transformB, m_polygonB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.m_count, color);
}
Vec2 x1 = output.pointA;
Vec2 x2 = output.pointB;
Color c1 = Color.FromArgb(255, 0, 0);
m_debugDraw.DrawPoint(x1, 4.0f, c1);
Color c2 = Color.FromArgb(255, 255, 0);
m_debugDraw.DrawPoint(x2, 4.0f, c2);
}
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 override void Step(TestSettings settings)
{
float dt = settings.hz > 0.0f ? 1.0f / settings.hz : 0.0f;
if (settings.pause == true && settings.singleStep == false)
{
dt = 0.0f;
}
m_rope.Step(dt, 1);
base.Step(settings);
m_rope.Draw(m_debugDraw);
m_debugDraw.DrawString("Press (q,e) to adjust target angle");
m_debugDraw.DrawString("Target angle = %g degrees", m_angle * 180.0f / (float)Math.PI);
}
public override void Step(TestSettings settings)
{
if (m_go && settings.hz > 0.0f)
{
m_time += 1.0f / settings.hz;
}
Vec2 linearOffset;
linearOffset.X = 6.0f * (float)Math.Sin(2.0f * m_time);
linearOffset.Y = 8.0f + 4.0f * (float)Math.Sin(1.0f * m_time);
float angularOffset = 4.0f * m_time;
m_joint.SetLinearOffset(linearOffset);
m_joint.SetAngularOffset(angularOffset);
m_debugDraw.DrawPoint(linearOffset, 4.0f, Color.FromArgb(225, 225, 225));
base.Step(settings);
m_debugDraw.DrawString("Keys: (s) pause");
m_textLine += 15;
}
public override void Step(TestSettings settings)
{
// Drive the kinematic body.
if (m_platform.GetBodyType() == BodyType._kinematicBody)
{
Vec2 p = m_platform.GetTransform().p;
Vec2 v = m_platform.GetLinearVelocity();
if ((p.X < -10.0f && v.X < 0.0f) ||
(p.X > 10.0f && v.X > 0.0f))
{
v.X = -v.X;
m_platform.SetLinearVelocity(v);
}
}
base.Step(settings);
m_debugDraw.DrawString("Keys: (d) dynamic, (s) static, (k) kinematic");
}
public override void Step(TestSettings settings)
{
if (m_break)
{
Break();
m_broke = true;
m_break = false;
}
// Cache velocities to improve movement on breakage.
if (m_broke == false)
{
m_velocity = m_body1.GetLinearVelocity();
m_angularVelocity = m_body1.GetAngularVelocity();
}
base.Step(settings);
}
public override void Step(TestSettings settings)
{
base.Step(settings);
PolyShapesCallback callback = new PolyShapesCallback();
callback.m_circle.m_radius = 2.0f;
callback.m_circle.m_p.Set(0.0f, 1.1f);
callback.m_transform.SetIdentity();
callback.m_debugDraw = m_debugDraw;
AABB aabb;
callback.m_circle.ComputeAABB(out aabb, callback.m_transform, 0);
m_world.QueryAABB(callback, aabb);
Color color = Color.FromArgb(100, 175, 200);
m_debugDraw.DrawCircle(callback.m_circle.m_p, callback.m_circle.m_radius, color);
m_debugDraw.DrawString("Press 1-5 to drop stuff");
m_debugDraw.DrawString("Press 'a' to (de)activate some bodies");
m_debugDraw.DrawString("Press 'd' to destroy a body");
}
public override void Step(TestSettings settings)
{
m_debugDraw.DrawString("Keys: left = a, brake = s, right = d, hz down = q, hz up = e");
m_debugDraw.DrawString("frequency = %g hz, damping ratio = %g", m_hz, m_zeta);
settings.viewCenter.X = m_car.GetPosition().X;
base.Step(settings);
}
public override void Step(TestSettings settings)
{
m_debugDraw.DrawString("Keys: left = a, brake = s, right = d, toggle motor = m");
base.Step(settings);
}
public void Step(TestSettings settings)
{
m_rayActor = null;
for (int i = 0; i < e_actorCount; ++i)
{
m_actors[i].fraction = 1.0f;
m_actors[i].overlap = false;
}
if (m_automated == true)
{
int actionCount = Math.Max(1, e_actorCount >> 2);
for (int i = 0; i < actionCount; ++i)
{
Action();
}
}
Query();
RayCast();
for (int i = 0; i < e_actorCount; ++i)
{
Actor actor = m_actors[i];
if (actor.proxyId == TreeNode._nullNode)
continue;
Color cv = Color.FromArgb(225, 225, 225);
if (actor == m_rayActor && actor.overlap)
{
cv = Color.FromArgb(225, 150, 150);
}
else if (actor == m_rayActor)
{
cv = Color.FromArgb(150, 225, 150);
}
else if (actor.overlap)
{
cv = Color.FromArgb(150, 150, 225);
}
m_debugDraw.DrawAABB(actor.aabb, cv);
}
Color c = Color.FromArgb(175, 175, 175);
m_debugDraw.DrawAABB(m_queryAABB, c);
m_debugDraw.DrawSegment(m_rayCastInput.p1, m_rayCastInput.p2, c);
Color c1 = Color.FromArgb(50, 225, 50);
Color c2 = Color.FromArgb(225, 50, 50);
m_debugDraw.DrawPoint(m_rayCastInput.p1, 6.0f, c1);
m_debugDraw.DrawPoint(m_rayCastInput.p2, 6.0f, c2);
if (m_rayActor != null)
{
Color cr = Color.FromArgb(50, 50, 225);
Vec2 p = m_rayCastInput.p1 + m_rayActor.fraction * (m_rayCastInput.p2 - m_rayCastInput.p1);
m_debugDraw.DrawPoint(p, 6.0f, cr);
}
{
int height = m_tree.GetHeight();
m_debugDraw.DrawString("dynamic tree height = %d", height);
}
++m_stepCount;
}
public override void Step(TestSettings settings)
{
Vec2 v = m_character.GetLinearVelocity();
v.X = -5.0f;
m_character.SetLinearVelocity(v);
base.Step(settings);
m_debugDraw.DrawString("This tests various character collision shapes.");
m_debugDraw.DrawString("Limitation: square and hexagon can snag on aligned boxes.");
m_debugDraw.DrawString("Feature: edge chains have smooth collision inside and out.");
}
public override void Step(TestSettings settings)
{
bool advanceRay = settings.pause == false || settings.singleStep;
base.Step(settings);
m_debugDraw.DrawString("Press 1-5 to drop stuff");
float L = 25.0f;
Vec2 point1 = new Vec2(0.0f, 10.0f);
Vec2 d = new Vec2(L * (float)Math.Cos(m_angle), -L * Math.Abs((float)Math.Sin(m_angle)));
Vec2 point2 = point1 + d;
EdgeShapesCallback callback = new EdgeShapesCallback();
m_world.RayCast(callback, point1, point2);
if (callback.m_fixture != null)
{
m_debugDraw.DrawPoint(callback.m_point, 5.0f, Color.FromArgb(100, 225, 100));
m_debugDraw.DrawSegment(point1, callback.m_point, Color.FromArgb(200, 200, 200));
Vec2 head = callback.m_point + 0.5f * callback.m_normal;
m_debugDraw.DrawSegment(callback.m_point, head, Color.FromArgb(225, 225, 100));
}
else
{
m_debugDraw.DrawSegment(point1, point2, Color.FromArgb(200, 200, 200));
}
if (advanceRay)
{
m_angle += 0.25f * (float)Math.PI / 180.0f;
}
}
public override void Step(TestSettings settings)
{
bool advanceRay = settings.pause == false || settings.singleStep;
base.Step(settings);
m_debugDraw.DrawString("Press 1-5 to drop stuff, m to change the mode");
switch (m_mode)
{
case Mode.e_closest:
m_debugDraw.DrawString("Ray-cast mode: closest - find closest fixture along the ray");
break;
case Mode.e_any:
m_debugDraw.DrawString("Ray-cast mode: any - check for obstruction");
break;
case Mode.e_multiple:
m_debugDraw.DrawString("Ray-cast mode: multiple - gather multiple fixtures");
break;
}
float L = 11.0f;
Vec2 point1 = new Vec2(0.0f, 10.0f);
Vec2 d = new Vec2(L * (float)Math.Cos(m_angle), L * (float)Math.Sin(m_angle));
Vec2 point2 = point1 + d;
if (m_mode == Mode.e_closest)
{
RayCastClosestCallback callback = new RayCastClosestCallback();
m_world.RayCast(callback, point1, point2);
if (callback.m_hit)
{
m_debugDraw.DrawPoint(callback.m_point, 5.0f, Color.FromArgb(100, 225, 100));
m_debugDraw.DrawSegment(point1, callback.m_point, Color.FromArgb(200, 200, 200));
Vec2 head = callback.m_point + 0.5f * callback.m_normal;
m_debugDraw.DrawSegment(callback.m_point, head, Color.FromArgb(225, 225, 100));
}
else
{
m_debugDraw.DrawSegment(point1, point2, Color.FromArgb(200, 200, 200));
}
}
else if (m_mode == Mode.e_any)
{
RayCastAnyCallback callback = new RayCastAnyCallback();
m_world.RayCast(callback, point1, point2);
if (callback.m_hit)
{
m_debugDraw.DrawPoint(callback.m_point, 5.0f, Color.FromArgb(100, 225, 100));
m_debugDraw.DrawSegment(point1, callback.m_point, Color.FromArgb(200, 200, 200));
Vec2 head = callback.m_point + 0.5f * callback.m_normal;
m_debugDraw.DrawSegment(callback.m_point, head, Color.FromArgb(225, 225, 100));
}
else
{
m_debugDraw.DrawSegment(point1, point2, Color.FromArgb(200, 200, 200));
}
}
else if (m_mode == Mode.e_multiple)
{
RayCastMultipleCallback callback = new RayCastMultipleCallback();
m_world.RayCast(callback, point1, point2);
m_debugDraw.DrawSegment(point1, point2, Color.FromArgb(200, 200, 200));
for (int i = 0; i < callback.m_count; ++i)
{
Vec2 p = callback.m_points[i];
Vec2 n = callback.m_normals[i];
m_debugDraw.DrawPoint(p, 5.0f, Color.FromArgb(100, 225, 100));
m_debugDraw.DrawSegment(point1, p, Color.FromArgb(200, 200, 200));
Vec2 head = p + 0.5f * n;
m_debugDraw.DrawSegment(p, head, Color.FromArgb(225, 225, 100));
}
}
if (advanceRay)
{
m_angle += 0.25f * (float)Math.PI / 180.0f;
}
#if ZERO
// This case was failing.
{
Vec2[] vertices = new Vec2[4];
//vertices[0].Set(-22.875f, -3.0f);
//vertices[1].Set(22.875f, -3.0f);
//vertices[2].Set(22.875f, 3.0f);
//vertices[3].Set(-22.875f, 3.0f);
PolygonShape shape = new PolygonShape();
//shape.Set(vertices, 4);
shape.SetAsBox(22.875f, 3.0f);
RayCastInput input;
input.p1.Set(10.2725f,1.71372f);
input.p2.Set(10.2353f,2.21807f);
//input.maxFraction = 0.567623f;
input.maxFraction = 0.56762173f;
Transform xf;
xf.SetIdentity();
xf.position.Set(23.0f, 5.0f);
RayCastOutput output;
bool hit;
hit = shape.RayCast(out output, input, xf);
hit = false;
Color color(1.0f, 1.0f, 1.0f);
Vec2[] vs = new Vec2[4];
for (int i = 0; i < 4; ++i)
{
vs[i] = Utilities.Mul(xf, shape.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vs, 4, color);
m_debugDraw.DrawSegment(input.p1, input.p2, color);
}
#endif
}
public override void Step(TestSettings settings)
{
base.Step(settings);
// We are going to destroy some bodies according to contact
// points. We must buffer the bodies that should be destroyed
// because they may belong to multiple contact points.
const int k_maxNuke = 6;
Body[] nuke = new Body[k_maxNuke];
int nukeCount = 0;
// Traverse the contact results. Destroy bodies that
// are touching heavier bodies.
for (int i = 0; i < m_pointCount; ++i)
{
ContactPoint point = m_points[i];
Body body1 = point.fixtureA.GetBody();
Body body2 = point.fixtureB.GetBody();
float mass1 = body1.GetMass();
float mass2 = body2.GetMass();
if (mass1 > 0.0f && mass2 > 0.0f)
{
if (mass2 > mass1)
{
nuke[nukeCount++] = body1;
}
else
{
nuke[nukeCount++] = body2;
}
if (nukeCount == k_maxNuke)
{
break;
}
}
}
// Sort the nuke array to group duplicates.
throw new NotImplementedException();
//std::sort(nuke, nuke + nukeCount);
// Destroy the bodies, skipping duplicates.
int i2 = 0;
while (i2 < nukeCount)
{
Body b = nuke[i2++];
while (i2 < nukeCount && nuke[i2] == b)
{
++i2;
}
if (b != m_bomb)
{
m_world.DestroyBody(b);
}
}
}
public override void Step(TestSettings settings)
{
base.Step(settings);
Sweep sweepA = new Sweep();
sweepA.c0.Set(24.0f, -60.0f);
sweepA.a0 = 2.95f;
sweepA.c = sweepA.c0;
sweepA.a = sweepA.a0;
sweepA.localCenter.SetZero();
Sweep sweepB = new Sweep();
sweepB.c0.Set(53.474274f, -50.252514f);
sweepB.a0 = 513.36676f; // - 162.0f * (float)Math.PI;
sweepB.c.Set(54.595478f, -51.083473f);
sweepB.a = 513.62781f; // - 162.0f * (float)Math.PI;
sweepB.localCenter.SetZero();
//sweepB.a0 -= 300.0f * (float)Math.PI;
//sweepB.a -= 300.0f * (float)Math.PI;
TOIInput input = new TOIInput();
input.proxyA.Set(m_shapeA, 0);
input.proxyB.Set(m_shapeB, 0);
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tMax = 1.0f;
TOIOutput output;
Utilities.TimeOfImpact(out output, input);
m_debugDraw.DrawString("toi = {0}", output.t);
m_debugDraw.DrawString("max toi iters = {0}, max root iters = {1}", Utilities._toiMaxIters, Utilities._toiMaxRootIters);
Vec2[] vertices = new Vec2[Settings._maxPolygonVertices];
Transform transformA;
sweepA.GetTransform(out transformA, 0.0f);
for (int i = 0; i < m_shapeA.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformA, m_shapeA.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeA.m_count, Color.FromArgb(225, 225, 225));
Transform transformB;
sweepB.GetTransform(out transformB, 0.0f);
//Vec2 localPoint(2.0f, -0.1f);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 225, 128));
sweepB.GetTransform(out transformB, output.t);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 175, 225));
sweepB.GetTransform(out transformB, 1.0f);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 128, 128));
#if ZERO
for (float t = 0.0f; t < 1.0f; t += 0.1f)
{
sweepB.GetTransform(out transformB, t);
for (int i = 0; i < m_shapeB.m_count; ++i)
{
vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 0.5f, 0.5f));
}
#endif
}
public override void Step(TestSettings settings)
{
base.Step(settings);
if (Utilities._gjkCalls > 0)
{
m_debugDraw.DrawString("gjk calls = %d, ave gjk iters = %3.1f, max gjk iters = %d",
Utilities._gjkCalls, Utilities._gjkIters / (float)(Utilities._gjkCalls), Utilities._gjkMaxIters);
}
if (Utilities._toiCalls > 0)
{
m_debugDraw.DrawString("toi calls = %d, ave [max] toi iters = %3.1f [%d]",
Utilities._toiCalls, Utilities._toiIters / (float)(Utilities._toiCalls), Utilities._toiMaxRootIters);
m_debugDraw.DrawString("ave [max] toi root iters = %3.1f [%d]",
Utilities._toiRootIters / (float)(Utilities._toiCalls), Utilities._toiMaxRootIters);
m_debugDraw.DrawString("ave [max] toi time = %.1f [%.1f] (microseconds)",
1000.0f * Utilities._toiTime / (float)(Utilities._toiCalls), 1000.0f * Utilities._toiMaxTime);
}
if (m_stepCount % 60 == 0)
{
//Launch();
}
}
public override void Step(TestSettings settings)
{
ContactManager cm = m_world.GetContactManager();
int height = cm.m_broadPhase.GetTreeHeight();
int leafCount = cm.m_broadPhase.GetProxyCount();
int minimumNodeCount = 2 * leafCount - 1;
float minimumHeight = (float)Math.Ceiling(Math.Log(minimumNodeCount) / Math.Log(2.0f));
m_debugDraw.DrawString("dynamic tree height = %d, min = %d", height, (int)minimumHeight);
base.Step(settings);
m_debugDraw.DrawString("create time = %6.2f ms, fixture count = %d",
m_createTime, m_fixtureCount);
//DynamicTree tree = m_world.m_contactManager.m_broadPhase.m_tree;
//if (m_stepCount == 400)
//{
// tree.RebuildBottomUp();
//}
}
public override void Step(TestSettings settings)
{
base.Step(settings);
m_debugDraw.DrawString("This demonstrates a soft distance joint.");
m_debugDraw.DrawString("Press: (b) to delete a body, (j) to delete a joint");
}
public override void Step(TestSettings settings)
{
base.Step(settings);
m_debugDraw.DrawString("Keys: (l) limits, (m) motor");
//if (m_stepCount == 360)
//{
// m_ball.SetTransform(new Vec2(0.0f, 0.5f), 0.0f);
//}
//float torque1 = m_joint1.GetMotorTorque();
//m_debugDraw.DrawString("Motor Torque = %4.0f, %4.0f : Motor Force = %4.0f", (float) torque1, (float) torque2, (float) force3);
//
}
public override void Step(TestSettings settings)
{
base.Step(settings);
// Traverse the contact results. Apply a force on shapes
// that overlap the sensor.
for (int i = 0; i < e_count; ++i)
{
if (m_touching[i] == false)
{
continue;
}
Body body = m_bodies[i];
Body ground = m_sensor.GetBody();
CircleShape circle = (CircleShape)m_sensor.GetShape();
Vec2 center = ground.GetWorldPoint(circle.m_p);
Vec2 position = body.GetPosition();
Vec2 d = center - position;
if (d.LengthSquared() < Single.Epsilon * Single.Epsilon)
{
continue;
}
d.Normalize();
Vec2 F = 100.0f * d;
body.ApplyForce(F, position, false);
}
}
public override void Step(TestSettings settings)
{
if (m_button)
{
m_leftJoint.SetMotorSpeed(20.0f);
m_rightJoint.SetMotorSpeed(-20.0f);
}
else
{
m_leftJoint.SetMotorSpeed(-10.0f);
m_rightJoint.SetMotorSpeed(10.0f);
}
base.Step(settings);
m_debugDraw.DrawString("Press 'a' to control the flippers");
}
public override void Step(TestSettings settings)
{
base.Step(settings);
float ratio, value;
ratio = m_joint4.GetRatio();
value = m_joint1.GetJointAngle() + ratio * m_joint2.GetJointAngle();
m_debugDraw.DrawString("theta1 + %4.2f * theta2 = %4.2f", (float) ratio, (float) value);
ratio = m_joint5.GetRatio();
value = m_joint2.GetJointAngle() + ratio * m_joint3.GetJointTranslation();
m_debugDraw.DrawString("theta2 + %4.2f * delta = %4.2f", (float) ratio, (float) value);
}
public virtual void Step(TestSettings settings) {
float timeStep = settings.hz > 0.0f ? 1.0f / settings.hz : 0.0f;
if (settings.pause)
{
if (settings.singleStep)
{
settings.singleStep = false;
}
else
{
timeStep = 0.0f;
}
m_debugDraw.DrawString("****PAUSED****");
}
Draw.DrawFlags flags = 0;
flags |= settings.drawShapes ? Draw.DrawFlags.e_shapeBit : 0;
flags |= settings.drawJoints ? Draw.DrawFlags.e_jointBit : 0;
flags |= settings.drawAABBs ? Draw.DrawFlags.e_aabbBit : 0;
flags |= settings.drawCOMs ? Draw.DrawFlags.e_centerOfMassBit : 0;
m_debugDraw.SetFlags(flags);
m_world.SetAllowSleeping(settings.enableSleep);
m_world.SetWarmStarting(settings.enableWarmStarting);
m_world.SetContinuousPhysics(settings.enableContinuous);
m_world.SetSubStepping(settings.enableSubStepping);
m_pointCount = 0;
m_world.Step(timeStep, settings.velocityIterations, settings.positionIterations);
m_world.DrawDebugData();
if (timeStep > 0.0f)
{
++m_stepCount;
}
if (settings.drawStats)
{
int bodyCount = m_world.GetBodyCount();
int contactCount = m_world.GetContactCount();
int jointCount = m_world.GetJointCount();
m_debugDraw.DrawString("bodies/contacts/joints = {0}/{1}/{2}", bodyCount, contactCount, jointCount);
int proxyCount = m_world.GetProxyCount();
int height = m_world.GetTreeHeight();
int balance = m_world.GetTreeBalance();
float quality = m_world.GetTreeQuality();
m_debugDraw.DrawString("proxies/height/balance/quality = {0}/{1}/{2}/{3}", proxyCount, height, balance, quality);
}
// Track maximum profile times
{
Profile p = m_world.GetProfile();
m_maxProfile.step = Math.Max(m_maxProfile.step, p.step);
m_maxProfile.collide = Math.Max(m_maxProfile.collide, p.collide);
m_maxProfile.solve = Math.Max(m_maxProfile.solve, p.solve);
m_maxProfile.solveInit = Math.Max(m_maxProfile.solveInit, p.solveInit);
m_maxProfile.solveVelocity = Math.Max(m_maxProfile.solveVelocity, p.solveVelocity);
m_maxProfile.solvePosition = Math.Max(m_maxProfile.solvePosition, p.solvePosition);
m_maxProfile.solveTOI = Math.Max(m_maxProfile.solveTOI, p.solveTOI);
m_maxProfile.broadphase = Math.Max(m_maxProfile.broadphase, p.broadphase);
m_totalProfile.step += p.step;
m_totalProfile.collide += p.collide;
m_totalProfile.solve += p.solve;
m_totalProfile.solveInit += p.solveInit;
m_totalProfile.solveVelocity += p.solveVelocity;
m_totalProfile.solvePosition += p.solvePosition;
m_totalProfile.solveTOI += p.solveTOI;
m_totalProfile.broadphase += p.broadphase;
}
if (settings.drawProfile)
{
Profile p = m_world.GetProfile();
Profile aveProfile = new Profile();
if (m_stepCount > 0)
{
float scale = 1.0f / m_stepCount;
aveProfile.step = scale * m_totalProfile.step;
aveProfile.collide = scale * m_totalProfile.collide;
aveProfile.solve = scale * m_totalProfile.solve;
aveProfile.solveInit = scale * m_totalProfile.solveInit;
aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity;
aveProfile.solvePosition = scale * m_totalProfile.solvePosition;
aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
aveProfile.broadphase = scale * m_totalProfile.broadphase;
}
m_debugDraw.DrawString("step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step);
m_debugDraw.DrawString("collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, m_maxProfile.collide);
m_debugDraw.DrawString("solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, m_maxProfile.solve);
m_debugDraw.DrawString("solve init [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveInit, aveProfile.solveInit, m_maxProfile.solveInit);
m_debugDraw.DrawString("solve velocity [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity);
m_debugDraw.DrawString("solve position [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition);
m_debugDraw.DrawString("solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI);
m_debugDraw.DrawString("broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase);
}
if (m_mouseJoint != null)
{
Vec2 p1 = m_mouseJoint.GetAnchorB();
Vec2 p2 = m_mouseJoint.GetTarget();
Color c = Color.FromArgb(0, 255, 0);
m_debugDraw.DrawPoint(p1, 4.0f, c);
m_debugDraw.DrawPoint(p2, 4.0f, c);
c = Color.FromArgb(204, 204, 204);
m_debugDraw.DrawSegment(p1, p2, c);
}
if (m_bombSpawning)
{
Color c = Color.FromArgb(0, 0, 255);
m_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c);
c = Color.FromArgb(200, 200, 200);
m_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c);
}
if (settings.drawContactPoints)
{
const float k_impulseScale = 0.1f;
const float k_axisScale = 0.3f;
for (int i = 0; i < m_pointCount; ++i)
{
ContactPoint point = m_points[i];
if (point.state == PointState._addState)
{
// Add
m_debugDraw.DrawPoint(point.position, 10.0f, Color.FromArgb(75, 242, 75));
}
else if (point.state == PointState._persistState)
{
// Persist
m_debugDraw.DrawPoint(point.position, 5.0f, Color.FromArgb(75, 75, 242));
}
if (settings.drawContactNormals)
{
Vec2 p1 = point.position;
Vec2 p2 = p1 + k_axisScale * point.normal;
m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 242));
}
else if (settings.drawContactImpulse)
{
Vec2 p1 = point.position;
Vec2 p2 = p1 + k_impulseScale * point.normalImpulse * point.normal;
m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 75));
}
if (settings.drawFrictionImpulse)
{
Vec2 tangent = Utilities.Cross(point.normal, 1.0f);
Vec2 p1 = point.position;
Vec2 p2 = p1 + k_impulseScale * point.tangentImpulse * tangent;
m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 75));
}
}
}
}
public virtual void Step(TestSettings settings)
{
float timeStep = settings.hz > 0.0f ? 1.0f / settings.hz : 0.0f;
if (settings.pause)
{
if (settings.singleStep)
{
settings.singleStep = false;
}
else
{
timeStep = 0.0f;
}
m_debugDraw.DrawString("****PAUSED****");
}
Draw.DrawFlags flags = 0;
flags |= settings.drawShapes ? Draw.DrawFlags.e_shapeBit : 0;
flags |= settings.drawJoints ? Draw.DrawFlags.e_jointBit : 0;
flags |= settings.drawAABBs ? Draw.DrawFlags.e_aabbBit : 0;
flags |= settings.drawCOMs ? Draw.DrawFlags.e_centerOfMassBit : 0;
m_debugDraw.SetFlags(flags);
m_world.SetAllowSleeping(settings.enableSleep);
m_world.SetWarmStarting(settings.enableWarmStarting);
m_world.SetContinuousPhysics(settings.enableContinuous);
m_world.SetSubStepping(settings.enableSubStepping);
m_pointCount = 0;
m_world.Step(timeStep, settings.velocityIterations, settings.positionIterations);
m_world.DrawDebugData();
if (timeStep > 0.0f)
{
++m_stepCount;
}
if (settings.drawStats)
{
int bodyCount = m_world.GetBodyCount();
int contactCount = m_world.GetContactCount();
int jointCount = m_world.GetJointCount();
m_debugDraw.DrawString("bodies/contacts/joints = {0}/{1}/{2}", bodyCount, contactCount, jointCount);
int proxyCount = m_world.GetProxyCount();
int height = m_world.GetTreeHeight();
int balance = m_world.GetTreeBalance();
float quality = m_world.GetTreeQuality();
m_debugDraw.DrawString("proxies/height/balance/quality = {0}/{1}/{2}/{3}", proxyCount, height, balance, quality);
}
// Track maximum profile times
{
Profile p = m_world.GetProfile();
m_maxProfile.step = Math.Max(m_maxProfile.step, p.step);
m_maxProfile.collide = Math.Max(m_maxProfile.collide, p.collide);
m_maxProfile.solve = Math.Max(m_maxProfile.solve, p.solve);
m_maxProfile.solveInit = Math.Max(m_maxProfile.solveInit, p.solveInit);
m_maxProfile.solveVelocity = Math.Max(m_maxProfile.solveVelocity, p.solveVelocity);
m_maxProfile.solvePosition = Math.Max(m_maxProfile.solvePosition, p.solvePosition);
m_maxProfile.solveTOI = Math.Max(m_maxProfile.solveTOI, p.solveTOI);
m_maxProfile.broadphase = Math.Max(m_maxProfile.broadphase, p.broadphase);
m_totalProfile.step += p.step;
m_totalProfile.collide += p.collide;
m_totalProfile.solve += p.solve;
m_totalProfile.solveInit += p.solveInit;
m_totalProfile.solveVelocity += p.solveVelocity;
m_totalProfile.solvePosition += p.solvePosition;
m_totalProfile.solveTOI += p.solveTOI;
m_totalProfile.broadphase += p.broadphase;
}
if (settings.drawProfile)
{
Profile p = m_world.GetProfile();
Profile aveProfile = new Profile();
if (m_stepCount > 0)
{
float scale = 1.0f / m_stepCount;
aveProfile.step = scale * m_totalProfile.step;
aveProfile.collide = scale * m_totalProfile.collide;
aveProfile.solve = scale * m_totalProfile.solve;
aveProfile.solveInit = scale * m_totalProfile.solveInit;
aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity;
aveProfile.solvePosition = scale * m_totalProfile.solvePosition;
aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
aveProfile.broadphase = scale * m_totalProfile.broadphase;
}
m_debugDraw.DrawString("step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step);
m_debugDraw.DrawString("collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, m_maxProfile.collide);
m_debugDraw.DrawString("solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, m_maxProfile.solve);
m_debugDraw.DrawString("solve init [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveInit, aveProfile.solveInit, m_maxProfile.solveInit);
m_debugDraw.DrawString("solve velocity [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity);
m_debugDraw.DrawString("solve position [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition);
m_debugDraw.DrawString("solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI);
m_debugDraw.DrawString("broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase);
}
if (m_mouseJoint != null)
{
Vec2 p1 = m_mouseJoint.GetAnchorB();
Vec2 p2 = m_mouseJoint.GetTarget();
Color c = Color.FromArgb(0, 255, 0);
m_debugDraw.DrawPoint(p1, 4.0f, c);
m_debugDraw.DrawPoint(p2, 4.0f, c);
c = Color.FromArgb(204, 204, 204);
m_debugDraw.DrawSegment(p1, p2, c);
}
if (m_bombSpawning)
{
Color c = Color.FromArgb(0, 0, 255);
m_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c);
c = Color.FromArgb(200, 200, 200);
m_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c);
}
if (settings.drawContactPoints)
{
const float k_impulseScale = 0.1f;
const float k_axisScale = 0.3f;
for (int i = 0; i < m_pointCount; ++i)
{
ContactPoint point = m_points[i];
if (point.state == PointState._addState)
{
// Add
m_debugDraw.DrawPoint(point.position, 10.0f, Color.FromArgb(75, 242, 75));
}
else if (point.state == PointState._persistState)
{
// Persist
m_debugDraw.DrawPoint(point.position, 5.0f, Color.FromArgb(75, 75, 242));
}
if (settings.drawContactNormals)
{
Vec2 p1 = point.position;
Vec2 p2 = p1 + k_axisScale * point.normal;
m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 242));
}
else if (settings.drawContactImpulse)
{
Vec2 p1 = point.position;
Vec2 p2 = p1 + k_impulseScale * point.normalImpulse * point.normal;
m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 75));
}
if (settings.drawFrictionImpulse)
{
Vec2 tangent = Utilities.Cross(point.normal, 1.0f);
Vec2 p1 = point.position;
Vec2 p2 = p1 + k_impulseScale * point.tangentImpulse * tangent;
m_debugDraw.DrawSegment(p1, p2, Color.FromArgb(242, 242, 75));
}
}
}
}
