public static Mat22 Abs(Mat22 A)
{
Mat22 B = new Mat22();
B.Set(MathB2.Abs(A.Col1), MathB2.Abs(A.Col2));
return(B);
}
public static Vec2 Abs(Vec2 a)
{
Vec2 b = new Vec2();
b.Set(MathB2.Abs(a.X), MathB2.Abs(a.Y));
return(b);
}
public static Vec2 Max(Vec2 a, Vec2 b)
{
Vec2 c = new Vec2
{
X = MathB2.Max(a.X, b.X),
Y = MathB2.Max(a.Y, b.Y)
};
return(c);
}
public override void Step(Settings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
SimplexCache cache = new SimplexCache();
cache.Count = 0;
Collision.Distance(out DistanceOutput output, ref cache, ref input, _polygonA, _polygonB);
StringBuilder strBld = new StringBuilder();
strBld.AppendFormat("distance = {0}", new object[] { output.Distance });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
strBld = new StringBuilder();
strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
int i;
for (i = 0; i < _polygonA.VertexCount; ++i)
{
_dv[i] = MathB2.Mul(_transformA, _polygonA.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color);
for (i = 0; i < _polygonB.VertexCount; ++i)
{
_dv[i] = MathB2.Mul(_transformB, _polygonB.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color);
}
Vec2 x1 = output.PointA;
Vec2 x2 = output.PointB;
OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0));
OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0));
OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0));
}
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;
Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared();
if (r2 < Settings.FLT_EPSILON)
{
continue;
}
Vec2 f = G / r2 / MathB2.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;
Vec2 d = body2.GetWorldCenter() - body1.GetWorldCenter();
float r2 = d.LengthSquared();
if (r2 < Settings.FLT_EPSILON)
{
continue;
}
Vec2 f = G / r2 * body1.GetMass() * body2.GetMass() * d;
body1.ApplyForce(f, body1.GetWorldCenter());
body2.ApplyForce(-1.0f * f, body2.GetWorldCenter());
}
}
}
}
/// Calculate the angle that the rotation matrix represents.
public float GetAngle()
{
return(MathB2.Atan2(R.Col1.Y, R.Col1.X));
}
