collision WIP

Assets/Scripts/ObjectMovement/abstract/FlatlandMovement.cs

@@ -94,7 +94,7 @@ public class FlatlandMovement : MonoBehaviour
         {
 
             Vector3 tmp = new Vector3(transform.position.x, 0, transform.position.z);
-            if (SpaceLength(nowDest, transform.position) < 0.1f || SpaceInnerPorduct(nowDest - tmp, v) < 0)
+            if (SpaceLength(nowDest, transform.position) < 0.1f || SpaceInnerProduct(nowDest - tmp, v) < 0)
             {
                 //목적지 근접 or 넘어가면
                 //Debug.Log("dest" + nowDest);
@@ -148,7 +148,7 @@ public class FlatlandMovement : MonoBehaviour
     /// <param name="a">xy가 공간</param>
     /// <param name="b">xz가 공간</param>
     /// <returns></returns>
-    protected float SpaceInnerPorduct(Vector3 a, Vector3 b)
+    protected float SpaceInnerProduct(Vector3 a, Vector3 b)
     {
         return a.x * b.x + a.z * b.z;
     }
@@ -386,17 +386,40 @@ public class FlatlandMovement : MonoBehaviour
 
         finalcollisionforcedirection = finalcollisionforcedirection.normalized;
 
-        var a = mass + other.mass * finalcollisionvelocitygamma;
-        var b = other.mass * finalcollisionvelocitygamma * SpaceInnerPorduct((finalcollisionvelocity/((float)Constants.c)), finalcollisionforcedirection);
-        var c = other.mass * finalcollisionvelocitygamma;
-
-        var resultvgamma = ((2.0 * a * c) + Math.Sqrt((4 * a * a * c * c ) - 4.0 * (a * a - b * b) * (b * b + c * c))) / (2.0 * (a * a - b * b));
-
-        if(resultvgamma < 1.0)
+        var cosine = SpaceInnerProduct(finalcollisionvelocity.normalized,finalcollisionforcedirection);
+        var totalenergy = mass * Constants.c * Constants.c + other.mass * Constants.Gamma(finalcollisionvelocity.magnitude) * Constants.c * Constants.c;
+        var initialmomentum = finalcollisionvelocitygamma * other.mass * finalcollisionvelocity.magnitude;
+
+        var a = 4 * (
+            (initialmomentum * Constants.c * cosine) * (initialmomentum * Constants.c * cosine)
+             - totalenergy * totalenergy);
+        var b = 4 * (
+            (totalenergy * totalenergy * totalenergy)
+            - (initialmomentum * Constants.c) * (initialmomentum * Constants.c) * totalenergy
+            + (mass * mass * Constants.c * Constants.c * Constants.c * Constants.c) * totalenergy
+            - (other.mass * Constants.c * Constants.c) * (other.mass * Constants.c * Constants.c) * totalenergy
+            );
+        var c = (2 * initialmomentum * initialmomentum * Constants.c * Constants.c * totalenergy * totalenergy)
+            - (2 * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * totalenergy * totalenergy)
+            + (2 * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * initialmomentum * initialmomentum * Constants.c * Constants.c)
+            + (2 * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * totalenergy * totalenergy)
+            - (2 * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * initialmomentum * initialmomentum * Constants.c * Constants.c)
+            + (2 * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c)
+            - (totalenergy * totalenergy * totalenergy * totalenergy)
+            - (initialmomentum * initialmomentum * initialmomentum * initialmomentum * Constants.c * Constants.c * Constants.c * Constants.c)
+            - (mass * mass * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c)
+            - (other.mass * other.mass * other.mass * other.mass * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c * Constants.c)
+            - (4 * mass * mass * Constants.c * Constants.c * Constants.c * Constants.c * initialmomentum * initialmomentum * Constants.c * Constants.c * cosine * cosine);
+
+        var resultEnergy = (-b + Math.Sqrt((b * b) - 4*(a)*(c)))/(2*a);
+
+        if(resultEnergy < 0)
         {
-            resultvgamma = -resultvgamma;
+            resultEnergy = (-b - Math.Sqrt((b * b) - 4 * (a) * (c))) / (2 * a); ;
         }
 
+        var resultvgamma = resultEnergy / (mass * Constants.c * Constants.c);
+
         var resultvbetamagnitude = Math.Sqrt(1.0 - (1.0/resultvgamma));
 
         StartCoroutine(_AcceleratetoConstantVelocityWithoutCorrection(finalcollisionforcedirection,(float)resultvbetamagnitude));