public void ComputeDistance(XForm xf, Vec2 p, float distance, Vec2 normal, int childIndex)
{
Vec2 v1 = MathB2.Mul(xf, _v1);
Vec2 v2 = MathB2.Mul(xf, _v2);
Vec2 d = p - v1;
Vec2 s = v2 - v1;
float ds = MathB2.Dot(d, s);
if (ds > 0)
{
float s2 = MathB2.Dot(s, s);
if (ds > s2)
{
d = p - v2;
}
else
{
d -= ds / s2 * s;
}
}
float d1 = d.Length();
distance = d1;
normal = d1 > 0 ? 1 / d1 * d : new Vec2(0, 0);
}
internal bool RayCast(RayCastOutput output, RayCastInput input, XForm xf, int v)
{
Vec2 p1 = MathB2.MulT(xf.R, input.P1 - xf.Position);
Vec2 p2 = MathB2.MulT(xf.R, input.P2 - xf.Position);
Vec2 d = p2 - p1;
Vec2 v1 = _v1;
Vec2 v2 = _v2;
Vec2 e = v2 - v1;
Vec2 normal = new Vec2(e.Y, -e.X);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = MathB2.Dot(normal, v1 - p1);
float denominator = MathB2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.MaxFraction < t)
{
return(false);
}
Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vec2 r = v2 - v1;
float rr = MathB2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = MathB2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.Fraction = t;
if (numerator > 0.0f)
{
output.Normal = -MathB2.Mul(xf.R, normal);
}
else
{
output.Normal = MathB2.Mul(xf.R, normal);
}
return(true);
}
