public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
CapsuleShape capsule = this.Shape1 as CapsuleShape;
SphereShape sphere = this.Shape2 as SphereShape;
// Get the center of capsule in world coordinates -> center1
capsule.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere in world coordinates -> center2
sphere.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector aP1, aP2, axisA;
TSVector halfAxisVecA = TSVector.up * FP.Half * capsule.length;
TSVector.Transform(ref halfAxisVecA, ref orientation1, out axisA);
TSVector.Add(ref center1, ref axisA, out aP1);
TSVector.Negate(ref axisA, out axisA);
TSVector.Add(ref center1, ref axisA, out aP2);
return(OverlapTest(ref capsule, ref aP1, ref aP2, ref sphere, ref center2,
ref point1, ref point2, ref normal, ref penetration));
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
CapsuleShape capsule1 = this.Shape1 as CapsuleShape;
CapsuleShape capsule2 = this.Shape2 as CapsuleShape;
// Get the center of box1 in world coordinates -> center1
capsule1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of box2 in world coordinates -> center2
capsule2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector aP1, aP2, axisA;
TSVector halfAxisVecA = TSVector.up * FP.Half * capsule1.length;
TSVector.Transform(ref halfAxisVecA, ref orientation1, out axisA);
TSVector.Add(ref center1, ref axisA, out aP1);
TSVector.Negate(ref axisA, out axisA);
TSVector.Add(ref center1, ref axisA, out aP2);
//UnityEngine.Debug.DrawLine(aP1.ToVector(), aP2.ToVector(), UnityEngine.Color.red, 1);
TSVector bP1, bP2, axisB;
TSVector halfAxisVecB = TSVector.up * FP.Half * capsule2.length;
TSVector.Transform(ref halfAxisVecB, ref orientation2, out axisB);
TSVector.Add(ref center2, ref axisB, out bP1);
TSVector.Negate(ref axisB, out axisB);
TSVector.Add(ref center2, ref axisB, out bP2);
//UnityEngine.Debug.DrawLine(bP1.ToVector(), bP2.ToVector(), UnityEngine.Color.blue, 1);
return(DoOverlapTest(ref capsule1, ref aP1, ref aP2,
ref capsule2, ref bP1, ref bP2, TSVector.zero, ref axisA, ref axisB,
ref point1, ref point2, ref normal, ref penetration));
}
