From 1abac4460dc4d88dde44fb1770a47344a5f5f763 Mon Sep 17 00:00:00 2001
From: Felix Klenner <felix@zuim.de>
Date: Thu, 30 Nov 2023 09:20:46 +0100
Subject: [PATCH] Fix angle calculation

---
 Assets/CableJointsAlgorithm.cs | 13 ++++++++++---
 Assets/Controller.cs           | 11 ++---------
 Assets/Extensions.cs           |  6 ++----
 3 files changed, 14 insertions(+), 16 deletions(-)

diff --git a/Assets/CableJointsAlgorithm.cs b/Assets/CableJointsAlgorithm.cs
index 13cbb0f..c85961e 100644
--- a/Assets/CableJointsAlgorithm.cs
+++ b/Assets/CableJointsAlgorithm.cs
@@ -3,6 +3,7 @@ using System.Collections.Generic;
 using UnityEngine;
 using static System.Math;
 
+/** Cable Joints algorithm based on Paper by Matthias Müller, Nuttapong Chentanez, Stefan Jeschke and Miles Macklin*/
 public static class CableJointsAlgorithm {
     public static void TimeStep(List<Cable> cables) {
         //handle each cable individually
@@ -15,6 +16,7 @@ public static class CableJointsAlgorithm {
                     var dist = go.GetComponent<DistanceJoint2D>();
                     var rp = go.GetComponent<RollerProperties>();
                     var (left, right) = rp.updateDistanceJoints();
+                    rp.actualDistance = dist.ActualDistanceInWorld();
 
                     if (left.HasValue) {
                         dist.distance -= left.Value;
@@ -56,6 +58,7 @@ public static class CableJointsAlgorithm {
 
             float phi = (float)Asin(rollerRadius / dLen);
 
+            //TODO: check if should be roller.clockwise
             if (fixedObject.clockwise) {
                 alpha = (float)(alpha - PI / 2 - phi);
             }
@@ -136,11 +139,11 @@ public static class CableJointsAlgorithm {
         var angle1 = Math.Atan2(p1diff.y, p1diff.x);
         var angle2 = Math.Atan2(p2diff.y, p2diff.x);
 
-        if (angle2 < PI/2 && angle1 > PI/2) {
+        if (angle2 < -PI/2 && angle1 > PI/2) {
             angle2 += 2 * PI;
         }
-        if (angle2 > PI/2 && angle1 < PI/2) {
-            //angle2 += 2 * PI;
+        else if (angle1 < -PI/2 && angle2 > PI/2) {
+            angle1 += 2 * PI;
         }
         
         var delta = angle2 - angle1;
@@ -148,6 +151,10 @@ public static class CableJointsAlgorithm {
 
         var dist = circumference * delta;
 
+        if (!c.GetComponent<RollerProperties>().clockwise) {
+            dist *= -1;
+        }
+
         //Debug.Log(c.name+": angle1: "+angle1+" angle2: "+angle2+" -> delta: "+delta+"  circumferecne: "+circumference+" dist: "+dist);
         return (float) dist;
     }
diff --git a/Assets/Controller.cs b/Assets/Controller.cs
index 65c8a0e..b61650f 100644
--- a/Assets/Controller.cs
+++ b/Assets/Controller.cs
@@ -1,13 +1,6 @@
-using System.Collections;
 using System.Collections.Generic;
-using NUnit.Framework;
 using TMPro;
-using Unity.Collections;
-using Unity.VisualScripting;
-using UnityEditor;
 using UnityEngine;
-using UnityEngine.UI;
-using FontWeight = TMPro.FontWeight;
 
 public class Controller : MonoBehaviour {
     private List<Cable> cables = new();
@@ -86,7 +79,7 @@ public class Controller : MonoBehaviour {
                     //update roller tangent attachments
                     rp.updateDistanceJoints();
                     
-                    rp.currentDistance = dist.distance = dist.DistanceInWorld(dist.anchor, dist.connectedAnchor);
+                    rp.actualDistance = dist.distance = dist.ActualDistanceInWorld();
                 }
 
                 if (rp.movement is RollerProperties.RotationType.Rotatable or RollerProperties.RotationType.Powered) {
@@ -94,7 +87,7 @@ public class Controller : MonoBehaviour {
                     wheel.anchor = Vector2.zero;
                     if (rp.movement == RollerProperties.RotationType.Powered) {
                         wheel.useMotor = true;
-                        wheel.motor = new JointMotor2D() { maxMotorTorque = 10000, motorSpeed = 10 };
+                        wheel.motor = new JointMotor2D() { maxMotorTorque = 10000, motorSpeed = rp.clockwise?10:-10 };
                     }
                 }
 
diff --git a/Assets/Extensions.cs b/Assets/Extensions.cs
index 02106eb..9ac42d0 100644
--- a/Assets/Extensions.cs
+++ b/Assets/Extensions.cs
@@ -1,5 +1,3 @@
-using System.Collections;
-using System.Collections.Generic;
 using UnityEngine;
 
 public static class Extensions
@@ -16,8 +14,8 @@ public static class Extensions
         return new Vector2(pos.x, pos.y);
     }
 
-    public static float DistanceInWorld(this DistanceJoint2D dist, Vector2 pointInSelf, Vector2 pointInConnected) {
+    public static float ActualDistanceInWorld(this DistanceJoint2D dist) {
         return (dist.transform.TransformPoint(dist.anchor) -
-                dist.connectedBody.transform.TransformPoint(pointInConnected)).magnitude;
+                dist.connectedBody.transform.TransformPoint(dist.connectedAnchor)).magnitude;
     }
 }