Add collisionMargin

GuidewireSimulation/GuidewireSimulation/Assets/Guidewire_Assets/CollisionSolvingStep.cs

@@ -14,7 +14,7 @@ namespace GuidewireSim
         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
+        float collisionMargin = 0.01f; /**< A margin by which a colliding element of the guidewire is set away from the object colliding with
                                         *   in the direction of the normal.
                                         *   @note Without this margin, the colliding element of the guidewire (e.g. a sphere) is corrected such that
                                         *   its surface exactly touches the object colliding with, which results in the guidewire still
@@ -185,7 +185,7 @@ namespace GuidewireSim
                                               Vector3 closestSurfacePoint, Vector3 normalVector)
         {
             Debug.DrawLine(closestSurfacePoint, closestSurfacePoint + 20f * normalVector, Color.blue, 2f);
-            Debug.DrawLine(otherPosition, spherePositionPrediction, Color.yellow, 2f);
+            // Debug.DrawLine(otherPosition, spherePositionPrediction, Color.yellow, 2f);
 
             DebugExtension.DrawPoint(spherePositionPrediction, Color.white);
 
@@ -195,7 +195,8 @@ namespace GuidewireSim
 
         protected Vector3 CalculateDeltaPosition(Vector3 spherePositionPrediction, Vector3 closestSurfacePoint, Vector3 normalVector)
         {
-            return - (spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint);
+            // return - (spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint);
+            return - (spherePositionPrediction - sphereRadius * normalVector - closestSurfacePoint - collisionMargin * normalVector);
         }
 
         private void CorrectCollisionPredictions(int sphereIndex, Vector3[] spherePositionPredictions, int solverStep, int constraintSolverSteps)

GuidewireSimulation/GuidewireSimulation/Assets/Guidewire_Assets/CollisionSolvingStep_UnityVariant.cs

@@ -13,7 +13,8 @@ namespace GuidewireSim
             Vector3 otherPosition = contactPoint.otherCollider.transform.position;
 
             Vector3 closestSurfacePoint = contactPoint.point;
-            Vector3 normalVector = (closestSurfacePoint - otherPosition).normalized;
+            // Vector3 normalVector = (closestSurfacePoint - otherPosition).normalized;
+            Vector3 normalVector = contactPoint.normal;
 
             if (solverStep == 0)
             {