public override void Step(Settings settings)
{
base.Step(settings);
// Traverse the contact results. Apply a force on shapes
// that overlap the sensor.
for (int i = 0; i < _pointCount; ++i)
{
MyContactPoint point = _points[i];
if ((int)point.state == 2)
{
continue;
}
Shape shape1 = point.shape1;
Shape shape2 = point.shape2;
Body other;
if (shape1 == _sensor)
{
other = _fixtureCircle.Body;
}
else if (shape2 == _sensor)
{
other = _fixtureCircle.Body;
}
else
{
continue;
}
Body ground = _fixtureCircle.Body;
CircleShape circle = (CircleShape)_sensor;
Vec2 center = ground.GetWorldPoint(circle.Position);
Vec2 d = center - point.position;
if (d.LengthSquared() < Box2DNet.Common.Settings.FLT_EPSILON * Box2DNet.Common.Settings.FLT_EPSILON)
{
continue;
}
d.Normalize();
Vec2 F = 100.0f * d;
other.ApplyForce(F, point.position);
}
}
public override void Remove(ContactPoint point)
{
if (test._pointCount == Test.k_maxContactPoints)
{
return;
}
MyContactPoint cp = new MyContactPoint();
cp.shape1 = point.Shape1;
cp.shape2 = point.Shape2;
cp.position = point.Position;
cp.normal = point.Normal;
cp.id = point.ID;
cp.state = ContactState.ContactRemoved;
test._points[test._pointCount] = cp;
++test._pointCount;
}
public override void Step(Settings 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;
Dictionary <int, Body> nuke = new Dictionary <int, Body>(k_maxNuke);
int nukeCount = 0;
// Traverse the contact results. Destroy bodies that
// are touching heavier bodies.
for (int i = 0; i < _pointCount; ++i)
{
MyContactPoint point = _points[i];
Body body1 = point.shape1.GetBody();
Body body2 = point.shape2.GetBody();
float mass1 = body1.GetMass();
float mass2 = body2.GetMass();
if (mass1 > 0.0f && mass2 > 0.0f)
{
if (mass2 > mass1)
{
int hc = body1.GetHashCode();
if (!nuke.ContainsKey(hc))
{
nuke.Add(hc, body1); //nuke[nukeCount++] = body1;
}
}
else
{
int hc = body2.GetHashCode();
if (!nuke.ContainsKey(hc))
{
nuke.Add(hc, body2); //nuke[nukeCount++] = body2;
}
}
if (nukeCount == k_maxNuke)
{
break;
}
}
}
// Sort the nuke array to group duplicates.
//Array.Sort(nuke);
// Destroy the bodies, skipping duplicates.
/*int i_ = 0;
* while (i_ < nukeCount)
* {
* Body b = nuke[i_++];
* while (i_ < nukeCount && nuke[i_] == b)
* {
++i_;
* }
*
* _world.DestroyBody(b);
* }*/
foreach (KeyValuePair <int, Body> kvp in nuke)
{
_world.DestroyBody(nuke[kvp.Key]);
}
}
public virtual void Step(Settings settings)
{
float timeStep = settings.hz > 0.0f ? 1.0f / settings.hz : 0.0f;
if (settings.pause != 0)
{
if (settings.singleStep != 0)
{
settings.singleStep = 0;
}
else
{
timeStep = 0.0f;
}
OpenGLDebugDraw.DrawString(5, _textLine, "****PAUSED****");
_textLine += 15;
}
uint flags = 0;
flags += (uint)settings.drawShapes * (uint)DebugDraw.DrawFlags.Shape;
flags += (uint)settings.drawJoints * (uint)DebugDraw.DrawFlags.Joint;
flags += (uint)settings.drawCoreShapes * (uint)DebugDraw.DrawFlags.CoreShape;
flags += (uint)settings.drawAABBs * (uint)DebugDraw.DrawFlags.Aabb;
flags += (uint)settings.drawOBBs * (uint)DebugDraw.DrawFlags.Obb;
flags += (uint)settings.drawPairs * (uint)DebugDraw.DrawFlags.Pair;
flags += (uint)settings.drawCOMs * (uint)DebugDraw.DrawFlags.CenterOfMass;
flags += (uint)settings.drawController * (uint)DebugDraw.DrawFlags.Controller;
_debugDraw.Flags = (DebugDraw.DrawFlags)flags;
_world.SetWarmStarting(settings.enableWarmStarting > 0);
_world.SetContinuousPhysics(settings.enableTOI > 0);
_pointCount = 0;
_world.Step(timeStep, settings.velocityIterations, settings.positionIterations);
_world.Validate();
if (_bomb != null && _bomb.IsFrozen())
{
_world.DestroyBody(_bomb);
_bomb = null;
}
if (settings.drawStats != 0)
{
OpenGLDebugDraw.DrawString(5, _textLine, String.Format("proxies(max) = {0}({1}), pairs(max) = {2}({3})",
new object[] { _world.GetProxyCount(), Box2DX.Common.Settings.MaxProxies,
_world.GetPairCount(), Box2DX.Common.Settings.MaxProxies }));
_textLine += 15;
OpenGLDebugDraw.DrawString(5, _textLine, String.Format("bodies/contacts/joints = {0}/{1}/{2}",
new object[] { _world.GetBodyCount(), _world.GetContactCount(), _world.GetJointCount() }));
_textLine += 15;
}
if (_mouseJoint != null)
{
Body body = _mouseJoint.GetBody2();
Vec2 p1 = body.GetWorldPoint(_mouseJoint._localAnchor);
Vec2 p2 = _mouseJoint._target;
Gl.glPointSize(4.0f);
Gl.glColor3f(0.0f, 1.0f, 0.0f);
Gl.glBegin(Gl.GL_POINTS);
Gl.glVertex2f(p1.X, p1.Y);
Gl.glVertex2f(p2.X, p2.Y);
Gl.glEnd();
Gl.glPointSize(1.0f);
Gl.glColor3f(0.8f, 0.8f, 0.8f);
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex2f(p1.X, p1.Y);
Gl.glVertex2f(p2.X, p2.Y);
Gl.glEnd();
}
if (settings.drawContactPoints != 0)
{
//float k_forceScale = 0.01f;
float k_axisScale = 0.3f;
for (int i = 0; i < _pointCount; ++i)
{
MyContactPoint point = _points[i];
if (point.state == ContactState.ContactAdded)
{
// Add
OpenGLDebugDraw.DrawPoint(point.position, 10.0f, new Color(0.3f, 0.95f, 0.3f));
}
else if (point.state == ContactState.ContactPersisted)
{
// Persist
OpenGLDebugDraw.DrawPoint(point.position, 5.0f, new Color(0.3f, 0.3f, 0.95f));
}
else
{
// Remove
OpenGLDebugDraw.DrawPoint(point.position, 10.0f, new Color(0.95f, 0.3f, 0.3f));
}
if (settings.drawContactNormals == 1)
{
Vec2 p1 = point.position;
Vec2 p2 = p1 + k_axisScale * point.normal;
OpenGLDebugDraw.DrawSegment(p1, p2, new Color(0.4f, 0.9f, 0.4f));
}
else if (settings.drawContactForces == 1)
{
/*Vector2 p1 = point.position;
* Vector2 p2 = p1 + k_forceScale * point.normalForce * point.normal;
* OpenGLDebugDraw.DrawSegment(p1, p2, new Color(0.9f, 0.9f, 0.3f));*/
}
if (settings.drawFrictionForces == 1)
{
/*Vector2 tangent = Vector2.Cross(point.normal, 1.0f);
* Vector2 p1 = point.position;
* Vector2 p2 = p1 + k_forceScale * point.tangentForce * tangent;
* OpenGLDebugDraw.DrawSegment(p1, p2, new Color(0.9f, 0.9f, 0.3f));*/
}
}
}
}
public override void Remove(ContactPoint point)
{
if (test._pointCount == Test.k_maxContactPoints)
{
return;
}
MyContactPoint cp = new MyContactPoint();
cp.shape1 = point.Shape1;
cp.shape2 = point.Shape2;
cp.position = point.Position;
cp.normal = point.Normal;
cp.id = point.ID;
cp.state = ContactState.ContactRemoved;
test._points[test._pointCount] = cp;
++test._pointCount;
}
