Add CollisionSolvingStep_UnityVariant.cs

GuidewireSimulation/GuidewireSimulation/Assets/Guidewire_Assets/CollisionSolvingStep.cs

@@ -11,6 +11,8 @@ namespace GuidewireSim
         CollisionHandler collisionHandler; //!< The component CollisionHandler that tracks all collisions.
 
         Vector3 deltaPosition = new Vector3(); //!< The correction of @p spherePositionPrediction in method SolveCollisionConstraint().
+        Vector3 initialPositionPrediction = new Vector3();
+
         float sphereRadius = 5f; //!< The radius of a sphere of the guidewire.
         float collisionMargin = 0.001f; /**< A margin by which a colliding element of the guidewire is set away from the object colliding with
                                         *   in the direction of the normal.
@@ -29,7 +31,7 @@ namespace GuidewireSim
             Assert.IsNotNull(collisionHandler);
         }
 
-        public void SolveCollisionConstraints(Vector3[] spherePositionPredictions)
+        public void SolveCollisionConstraints(Vector3[] spherePositionPredictions, int solverStep, int constraintSolverSteps)
         {
             // Debug.Log("Collision Count " + collisionHandler.registeredCollisions.Count);
             for (int collisionIndex = 0; collisionIndex < collisionHandler.registeredCollisions.Count; collisionIndex++)
@@ -47,19 +49,21 @@ namespace GuidewireSim
                 // Debug.Log("deviation" + deviation.ToString("e3"));
 
 
-                SolveCollisionConstraint(spherePositionPrediction, collisionPair, out deltaPosition);
-                CorrectCollisionPredictions(sphereID, spherePositionPredictions);
+                SolveCollisionConstraint(spherePositionPrediction, collisionPair, solverStep, out deltaPosition);
+                CorrectCollisionPredictions(sphereID, spherePositionPredictions, solverStep, constraintSolverSteps);
+            }
+
+            // Pauses the simulation after the first collision has been resolved.
+            if (collisionHandler.registeredCollisions.Count >= 1)
+            {
+                Time.timeScale = 0f;
             }
         }
 
         // @attention Current calculation of the normal only works for spheres.
-        // TODO could use Vector3.Reflect
-        private void SolveCollisionConstraint(Vector3 spherePositionPrediction, CollisionPair collisionPair, out Vector3 deltaPosition)
+        protected virtual void SolveCollisionConstraint(Vector3 spherePositionPrediction, CollisionPair collisionPair, int solverStep,
+                                                        out Vector3 deltaPosition)
         {
-            Vector3 prnq = new Vector3();
-            Vector3 normalVector = new Vector3();
-            Vector3 closestSurfacePoint = new Vector3();
-
             int sphereID = collisionPair.sphereID;
 
             ContactPoint contactPoint = collisionPair.vesselCollider.GetContact(0);
@@ -73,8 +77,8 @@ namespace GuidewireSim
 
             // solution two
             float otherRadius = 15f;
-            closestSurfacePoint = otherPosition + otherRadius * (spherePositionPrediction - otherPosition).normalized;
-            normalVector = (spherePositionPrediction - otherPosition).normalized;
+            Vector3 closestSurfacePoint = otherPosition + otherRadius * (spherePositionPrediction - otherPosition).normalized;
+            Vector3 normalVector = (spherePositionPrediction - otherPosition).normalized;
 
 
             //solution three
@@ -82,28 +86,12 @@ namespace GuidewireSim
             // normalVector = (closestSurfacePoint - otherPosition).normalized;
 
 
-            Debug.DrawLine(otherPosition, otherPosition + 20f * normalVector, Color.blue, 2f);
-            Debug.DrawLine(otherPosition, closestSurfacePoint, Color.yellow, 2f);
-
-            DebugExtension.DrawPoint(spherePositionPrediction, Color.white);
-
-            DebugExtension.DrawPoint(contactPoint.point, Color.black);
-            DebugExtension.DrawPoint(closestSurfacePoint, Color.yellow);
-
-            // Debug.Log("closestSurfacePoint" + closestSurfacePoint);
-
-
-
-            prnq = spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint;
-            // Debug.Log("prnq " + prnq.ToString("e2"));
-
-            deltaPosition = - prnq;
-
-
-
-
-
+            if (solverStep == 0)
+            {
+                DrawCollisionInformation(spherePositionPrediction, contactPoint, otherPosition, closestSurfacePoint, normalVector);
+            }
 
+            deltaPosition = CalculateDeltaPosition(spherePositionPrediction, closestSurfacePoint, normalVector);
 
 
 
@@ -124,7 +112,7 @@ namespace GuidewireSim
             // Debug.Log("closestSurfacePoint " + closestSurfacePoint.ToString("e3"));
             // Debug.Log("closestSurfacePoint2 " + closestSurfacePoint2.ToString("e3"));
 
-            
+
             // Debug.Log("spherePosition" + collisionPair.sphere.position);
             // Debug.Log("spherePositionPrediction" + spherePositionPrediction);
             // Debug.Log("closestSurfacePoint" + closestSurfacePoint);
@@ -193,17 +181,41 @@ namespace GuidewireSim
             // deltaPosition = factor * normalVector;
         }
 
-        private void CorrectCollisionPredictions(int sphereIndex, Vector3[] spherePositionPredictions)
+        protected void DrawCollisionInformation(Vector3 spherePositionPrediction, ContactPoint contactPoint, Vector3 otherPosition,
+                                              Vector3 closestSurfacePoint, Vector3 normalVector)
+        {
+            Debug.DrawLine(closestSurfacePoint, closestSurfacePoint + 20f * normalVector, Color.blue, 2f);
+            Debug.DrawLine(otherPosition, spherePositionPrediction, Color.yellow, 2f);
+
+            DebugExtension.DrawPoint(spherePositionPrediction, Color.white);
+
+            DebugExtension.DrawPoint(contactPoint.point, Color.black);
+            DebugExtension.DrawPoint(closestSurfacePoint, Color.yellow);
+        }
+
+        protected Vector3 CalculateDeltaPosition(Vector3 spherePositionPrediction, Vector3 closestSurfacePoint, Vector3 normalVector)
+        {
+            return - (spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint);
+        }
+
+        private void CorrectCollisionPredictions(int sphereIndex, Vector3[] spherePositionPredictions, int solverStep, int constraintSolverSteps)
         {
             Assert.IsTrue(sphereIndex >= 0);
 
-            Vector3 start = spherePositionPredictions[sphereIndex];
+            if (solverStep == 0)
+            {
+                initialPositionPrediction = spherePositionPredictions[sphereIndex];
+            }
+
             // spherePositionPredictions[sphereIndex] += Mathf.Pow(1 - (1 - collisionStiffness), 1 / 1000f) * deltaPosition;
             spherePositionPredictions[sphereIndex] += collisionStiffness * deltaPosition;
             // spherePositionPredictions[sphereIndex] += deltaPosition;
 
-            Debug.DrawLine(start, spherePositionPredictions[sphereIndex], Color.red, 2f);
-            DebugExtension.DrawPoint(spherePositionPredictions[sphereIndex], Color.red);
+            if (solverStep == constraintSolverSteps - 1)
+            {
+                Debug.DrawLine(initialPositionPrediction, spherePositionPredictions[sphereIndex], Color.red, 2f);
+                DebugExtension.DrawPoint(spherePositionPredictions[sphereIndex], Color.red);
+            }
         }
     }
 }
\ No newline at end of file

GuidewireSimulation/GuidewireSimulation/Assets/Guidewire_Assets/CollisionSolvingStep_UnityVariant.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace GuidewireSim
+{
+    public class CollisionSolvingStep_UnityVariant : CollisionSolvingStep
+    {
+        protected override void SolveCollisionConstraint(Vector3 spherePositionPrediction, CollisionPair collisionPair, int solverStep,
+                                                         out Vector3 deltaPosition)
+        {
+            ContactPoint contactPoint = collisionPair.vesselCollider.GetContact(0);
+            Vector3 otherPosition = contactPoint.otherCollider.transform.position;
+
+            Vector3 closestSurfacePoint = contactPoint.point;
+            Vector3 normalVector = (closestSurfacePoint - otherPosition).normalized;
+
+            if (solverStep == 0)
+            {
+                DrawCollisionInformation(spherePositionPrediction, contactPoint, otherPosition, closestSurfacePoint, normalVector);
+            }
+
+            deltaPosition = CalculateDeltaPosition(spherePositionPrediction, closestSurfacePoint, normalVector);
+        }
+    }
+}
\ No newline at end of file

GuidewireSimulation/GuidewireSimulation/Assets/Guidewire_Assets/CollisionSolvingStep_UnityVariant.cs.meta

+fileFormatVersion: 2
+guid: f02fb1d019bf486469f64a89aa7a9ec1
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

GuidewireSimulation/GuidewireSimulation/Assets/Guidewire_Assets/CollisionTestScene_2Spheres_PrimitiveSphere_UnityVariant.unity.meta

+fileFormatVersion: 2
+guid: e67e959b05666694eabac253d7731dfb
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

GuidewireSimulation/GuidewireSimulation/Assets/Guidewire_Assets/SimulationLoop.cs

@@ -247,7 +247,7 @@ public class SimulationLoop : MonoBehaviour
 
             if (solveCollisionConstraints)
             {
-                collisionSolvingStep.SolveCollisionConstraints(spherePositionPredictions);
+                collisionSolvingStep.SolveCollisionConstraints(spherePositionPredictions, solverStep, ConstraintSolverSteps);
             }
         }
 

GuidewireSimulation/GuidewireSimulation/UserSettings/EditorUserSettings.asset

@@ -6,34 +6,34 @@ EditorUserSettings:
   serializedVersion: 4
   m_ConfigSettings:
     RecentlyUsedScenePath-0:
-      value: 224247031146467c18070827072a4d1529360b39293c357f1d3b1227edf42d28e7d435ece93f006f0a12e739163b1271e704001fef
+      value: 224247031146467c18070827072a4d1529360b39293c357f1d3b1227edf42d28e7d435ece93f006e0a12e739163b1271e704001fef
       flags: 0
     RecentlyUsedScenePath-1:
-      value: 224247031146467c18070827072a4d1529360b39293c357f1d3b1227edf42d28e7d435ece93f006e0a12e739163b1271e704001fef
+      value: 224247031146467f080c192534315e071f191f0f343c233e3e20123dadc52c39eff73aeca81f273d3412e3394a2b0f36e613
       flags: 0
     RecentlyUsedScenePath-2:
-      value: 224247031146467f080c192534315e071f191f0f343c233e3e20123dadc52c39eff73aeca81f273d3412e3394a2b0f36e613
+      value: 224247031146467c18070827072a4d1529360b39293c357f1d3b1227edf42d28e7d435ece93f006f6931ff34012c042cbc1f0702e212
       flags: 0
     RecentlyUsedScenePath-3:
-      value: 224247031146467c18070827072a4d1529360b39293c357f1d3b1227edf42d28e7d435ece93f006f6931ff34012c042cbc1f0702e212
+      value: 224247031146467c18070827072a4d1529360b39293c357f1d3b1227edf42d28e7d435ece93f006d6931ff34012c042cbc1f0702e212
       flags: 0
     RecentlyUsedScenePath-4:
-      value: 224247031146467c18070827072a4d1529360b39293c357f1d3b1227edf42d28e7d435ece93f006d6931ff34012c042cbc1f0702e212
+      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a34d51e191f1e03cd0701fa5e061fcc0cdc
       flags: 0
     RecentlyUsedScenePath-5:
-      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a34d51e191f1e03cd0701fa5e061fcc0cdc
+      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06c56701431fb1e10
       flags: 0
     RecentlyUsedScenePath-6:
-      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06c56701431fb1e10
+      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a45e305031f08
       flags: 0
     RecentlyUsedScenePath-7:
-      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a45e305031f08
+      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a34c6190306181af41814d2150019f511d51f860a12c20d05
       flags: 0
     RecentlyUsedScenePath-8:
       value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a34c6190306181af41814cc001b14d71d8b0fc61608d2
       flags: 0
     RecentlyUsedScenePath-9:
-      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a34c6190306181af41814d2150019f511d51f860a12c20d05
+      value: 224247031146467c18070827072a4d1529360b39293c357f0e26113febf33d37ecd333faf3093c393707d06e372e093ae00f1a34c6190306181af41814cc001b14d71dfa2fc61608d22f1ddcc41cdff79bc3dae3ccf2
       flags: 0
     vcSharedLogLevel:
       value: 0d5e400f0650