private bool OverlapTest(ref CapsuleShape capsule, ref TSVector p1, ref TSVector p2,
ref SphereShape sphere, ref TSVector sphereCenter,
ref TSVector pa, ref TSVector pb, ref TSVector normal, ref FP penetration)
{
SegmentShape cap = SegmentShape.Pool.GetNew();
cap.P1 = p1;
cap.P2 = p2;
TSVector v;
FP r2 = capsule.Radius + sphere.Radius;
r2 *= r2;
FP sb;
cap.ClosestPointTo(ref sphereCenter, out sb, out pb);
SegmentShape.Pool.GiveBack(cap);
TSVector.Subtract(ref sphereCenter, ref pb, out normal);
if (normal.sqrMagnitude - r2 >= TSMath.Epsilon)
{
return(false);
}
penetration = (capsule.radius + sphere.radius) - normal.magnitude;
normal.Normalize();
TSVector.Multiply(ref normal, -sphere.Radius, out v);
TSVector.Add(ref sphereCenter, ref v, out pa);
TSVector.Multiply(ref normal, capsule.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Negate(ref normal, out normal);
return(true);
}
///// <summary>
///// Transform the segment using the specified transformation.
///// </summary>
///// <param name="s">The segment to transform.</param>
///// <param name="transform">the transform to apply.</param>
///// <param name="output">Returns the transformed segment.</param>
//public static void Transform(ref SegmentShape s, ref Transform transform, out SegmentShape output)
//{
// TSVector.Transform(ref s.P1, ref transform.Combined, out output.P1);
// TSVector.Transform(ref s.P2, ref transform.Combined, out output.P2);
//}
/// <summary>
/// Intersects two line segments.
/// </summary>
/// <param name="sa">The first line segment.</param>
/// <param name="sb">The second line segment.</param>
/// <param name="scalarA">Returns a value between 0 and 1 indicating the point of intersection on the first segment.</param>
/// <param name="scalarB">Returns a value between 0 and 1 indicating the point of intersection on the second segment.</param>
/// <param name="p">Returns the point of intersection, common to both segments.</param>
/// <returns>Returns a value indicating whether there was an intersection.</returns>
public static bool Intersect(ref SegmentShape sa, ref SegmentShape sb, out FP scalarA, out FP scalarB, out TSVector p)
{
TSVector pa;
FP dist;
SegmentShape.ClosestPoints(ref sa, ref sb, out scalarA, out pa, out scalarB, out p);
dist = TSVector.Subtract(pa, p).sqrMagnitude;
return(dist < TSMath.Epsilon);
}
private bool DoOverlapTest(ref CapsuleShape a, ref TSVector aP1, ref TSVector aP2,
ref CapsuleShape b, ref TSVector bP1, ref TSVector bP2, TSVector offset, ref TSVector axisA, ref TSVector axisB,
ref TSVector pa, ref TSVector pb, ref TSVector normal, ref FP penetration)
{
capa.P1 = aP1;
capa.P2 = aP2;
TSVector.Add(ref aP1, ref offset, out capa.P1);
TSVector.Add(ref aP2, ref offset, out capa.P2);
capb.P1 = bP1;
capb.P2 = bP2;
TSVector v;
FP sa, sb, r2 = a.Radius + b.Radius;
r2 *= r2;
// find the closest point between the two capsules
SegmentShape.ClosestPoints(ref capa, ref capb, out sa, out pa, out sb, out pb);
TSVector.Subtract(ref pa, ref pb, out normal);
FP sqrPaPb = normal.sqrMagnitude;
if (sqrPaPb - r2 >= TSMath.Epsilon)
{
return(false);
}
else if (sqrPaPb < TSMath.Epsilon)
{
normal = TSVector.forward;
}
else
{
normal.Normalize();
}
TSVector.Negate(ref normal, out normal);
penetration = TSVector.Subtract(pa, pb).magnitude - (a.radius + b.radius);
TSVector.Multiply(ref normal, -a.Radius, out v);
TSVector.Add(ref pa, ref v, out pa);
TSVector.Multiply(ref normal, b.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Subtract(ref pa, ref offset, out pa);
// if the two capsules are nearly parallel, an additional support point provides stability
if (sa == FP.Zero || sa == FP.One)
{
pa = sa == FP.Zero ? capa.P2 : capa.P1;
capb.ClosestPointTo(ref pa, out sa, out pb);
}
else if (sb == FP.Zero || sb == FP.One)
{
pb = sb == FP.Zero ? capb.P2 : capb.P1;
capa.ClosestPointTo(ref pb, out sb, out pa);
}
else
{
return(true);
}
FP dist = TSVector.Subtract(pa, pb).sqrMagnitude - r2;
penetration = TSVector.Subtract(pa, pb).magnitude - (a.radius + b.radius);
if (dist < TSMath.Epsilon)
{
TSVector.Multiply(ref normal, -a.Radius, out v);
TSVector.Add(ref pa, ref v, out pa);
TSVector.Multiply(ref normal, b.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Subtract(ref pa, ref offset, out pa);
}
TSVector.Negate(ref normal, out normal);
return(true);
}
/// <summary>
/// Gets the closest two points on two line segments.
/// </summary>
/// <remarks>
/// If the line segments are parallel and overlap in their common direction, then the midpoint of the overlapped portion of line segments
/// is returned.
/// </remarks>
/// <param name="sa">The first line segment.</param>
/// <param name="sb">The second line segment.</param>
/// <param name="scalarA">Returns a value between 0 and 1 indicating the position of the closest point on the first segment.</param>
/// <param name="pa">Returns the closest point on the first segment.</param>
/// <param name="scalarB">Returns a value between 0 and 1 indicating the position of the closest point on the second segment.</param>
/// <param name="pb">Returns the closest point on the second segment.</param>
public static void ClosestPoints(ref SegmentShape sa, ref SegmentShape sb,
out FP scalarA, out TSVector pa, out FP scalarB, out TSVector pb)
{
TSVector d1, d2, r;
TSVector.Subtract(ref sa.P2, ref sa.P1, out d1);
TSVector.Subtract(ref sb.P2, ref sb.P1, out d2);
TSVector.Subtract(ref sa.P1, ref sb.P1, out r);
FP a, e, f;
a = TSVector.Dot(ref d1, ref d1);
e = TSVector.Dot(ref d2, ref d2);
f = TSVector.Dot(ref d2, ref r);
if (a < TSMath.Epsilon && e < TSMath.Epsilon)
{
// segment a and b are both points
scalarA = scalarB = FP.Zero;
pa = sa.P1;
pb = sb.P1;
return;
}
if (a < TSMath.Epsilon)
{
// segment a is a point
scalarA = FP.Zero;
scalarB = TSMath.Clamp(f / e, FP.Zero, FP.One);
}
else
{
FP c = TSVector.Dot(ref d1, ref r);
if (e < TSMath.Epsilon)
{
// segment b is a point
scalarB = FP.Zero;
scalarA = TSMath.Clamp(-c / a, FP.Zero, FP.One);
}
else
{
FP b = TSVector.Dot(ref d1, ref d2);
FP denom = a * e - b * b;
if (denom < TSMath.Epsilon)
{
// segments are parallel
FP a1, a2, b1, b2;
a1 = TSVector.Dot(ref d2, ref sa.P1);
a2 = TSVector.Dot(ref d2, ref sa.P2);
b1 = TSVector.Dot(ref d2, ref sb.P1);
b2 = TSVector.Dot(ref d2, ref sb.P2);
if (a1 <= b1 && a2 <= b1)
{
// segment A is completely "before" segment B
scalarA = a2 > a1 ? FP.One : FP.Zero;
scalarB = FP.Zero;
}
else if (a1 >= b2 && a2 >= b2)
{
// segment B is completely "before" segment A
scalarA = a2 > a1 ? FP.Zero : FP.One;
scalarB = FP.One;
}
else
{
// segments A and B overlap, use midpoint of shared length
if (a1 > a2)
{
f = a1; a1 = a2; a2 = f;
}
f = (TSMath.Min(a2, b2) + TSMath.Max(a1, b1)) / 2;
scalarB = (f - b1) / e;
TSVector.Multiply(ref d2, scalarB, out pb);
TSVector.Add(ref sb.P1, ref pb, out pb);
sa.ClosestPointTo(ref pb, out scalarA, out pa);
return;
}
}
else
{
// general case
scalarA = TSMath.Clamp((b * f - c * e) / denom, FP.Zero, FP.One);
scalarB = (b * scalarA + f) / e;
if (scalarB < FP.Zero)
{
scalarB = FP.Zero;
scalarA = TSMath.Clamp(-c / a, FP.Zero, FP.One);
}
else if (scalarB > FP.One)
{
scalarB = FP.One;
scalarA = TSMath.Clamp((b - c) / a, FP.Zero, FP.One);
}
}
}
}
TSVector.Multiply(ref d1, scalarA, out d1);
TSVector.Multiply(ref d2, scalarB, out d2);
TSVector.Add(ref sa.P1, ref d1, out pa);
TSVector.Add(ref sb.P1, ref d2, out pb);
}