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)); }