public static Vector2 MulT(Transform T, Vector2 v)
{
#if USE_MATRIX_FOR_ROTATION
return(Math.MulT(T.R, v - T.position));
#else
return(T.InverseTransformDirection(v - T.position));
#endif
}
public Vector2 InverseTransformDirection(Vector2 vector)
{
#if USE_MATRIX_FOR_ROTATION
return(Math.MulT(rotation, vector));
#else
return(Quaternion.Invert(rotation).Xyz.ToVector2() * vector);
#endif
}
public Vector2 InverseTransformPoint(Vector2 vector)
{
#if USE_MATRIX_FOR_ROTATION
return(Math.MulT(rotation, vector - position));
#else
return(Quaternion.Inverse(rotation) * (vector - position));
#endif
}
public override bool TestPoint(Transform xf, Vec2 p)
{
Vec2 pLocal = Math.MulT(xf.R, p - xf.Position);
for (int i = 0; i < VertexCount; ++i)
{
float dot = Vec2.Dot(Normals[i], pLocal - Vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
public static Vec2 MulT(XForm T, Vec2 v)
{
return(Math.MulT(T.R, v - T.Position));
}
public Vector2 InverseTransformPoint(Vector2 vector)
{
return(Math.MulT(R, vector - position));
}
public Vector2 InverseTransformDirection(Vector2 vector)
{
return(Math.MulT(R, vector));
}
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform xf)
{
output = new RayCastOutput();
float lower = 0.0f, upper = input.MaxFraction;
// Put the ray into the polygon's frame of reference.
Vec2 p1 = Math.MulT(xf.R, input.P1 - xf.Position);
Vec2 p2 = Math.MulT(xf.R, input.P2 - xf.Position);
Vec2 d = p2 - p1;
int index = -1;
output.Hit = false;
for (int i = 0; i < VertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vec2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vec2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return;
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Hit = true;
output.Fraction = lower;
output.Normal = Math.Mul(xf.R, Normals[index]);
return;
}
}