/// <summary>
/// A * B
/// </summary>
public static Mat22 Mul(Mat22 A, Mat22 B)
{
Mat22 C = new Mat22();
C.Set(Math.Mul(A, B.Col1), Math.Mul(A, B.Col2));
return(C);
}
// <summary>
// Transforms direction from local space to world space.
// </summary>
public Vector2 TransformDirection(Vector2 vector)
{
#if USE_MATRIX_FOR_ROTATION
return(Math.Mul(rotation, vector));
#else
return((rotation.Xyz() * vector.ToVector3()).ToVector2());
#endif
}
public Vector2 TransformPoint(Vector2 vector)
{
#if USE_MATRIX_FOR_ROTATION
return(position + Math.Mul(rotation, vector));
#else
return(position + (rotation.Xyz() * vector.ToVector3()).ToVector2());
#endif
}
public static Vector2 Mul(Transform T, Vector2 v)
{
#if USE_MATRIX_FOR_ROTATION
return(T.position + Math.Mul(T.R, v));
#else
return(T.position + T.TransformDirection(v));
#endif
}
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;
DistanceOutput output;
Collision.Distance(out 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] = Math.Mul(_transformA, _polygonA.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color);
for (i = 0; i < _polygonB.VertexCount; ++i)
{
_dv[i] = Math.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 static Vec2 Mul(XForm T, Vec2 v)
{
return(T.Position + Math.Mul(T.R, v));
}
public Vector2 TransformPoint(Vector2 vector)
{
return(position + Math.Mul(R, vector));
}
public Vector2 TransformDirection(Vector2 vector)
{
return(Math.Mul(R, vector));
}