public override void Step(TimeStep step)
{
//B2_NOT_USED(step);
if (InvSqr)
{
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body1 = i.body;
for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
{
Body body2 = j.body;
Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared;
if (r2 < Settings.FLT_EPSILON)
{
continue;
}
Vector2 f = G / r2 / Math.Sqrt(r2) * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
else
{
for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
{
Body body1 = i.body;
for (ControllerEdge j = _bodyList; j != i; j = j.nextBody)
{
Body body2 = j.body;
Vector2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared;
if (r2 < Settings.FLT_EPSILON)
{
continue;
}
Vector2 f = G / r2 * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform transform)
{
output = new RayCastOutput();
Vec2 position = transform.Position + Math.Mul(transform.R, _p);
Vec2 s = input.P1 - position;
float b = Vec2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vec2 r = input.P2 - input.P1;
float c = Vec2.Dot(s, r);
float rr = Vec2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.FLT_EPSILON)
{
output.Hit = false;
return;
}
// Find the point of intersection of the line with the circle.
float a = -(c + Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Hit = true;
output.Fraction = a;
output.Normal = s + a * r;
output.Normal.Normalize();
return;
}
output.Hit = false;
return;
}
