From 3aa4a592f0a9bd281a1a2b5ba44ce6482850fb9f Mon Sep 17 00:00:00 2001
From: Leni Aniva <v@leni.sh>
Date: Wed, 17 Jul 2024 13:20:06 -0700
Subject: [PATCH] fix: Collision between spring and track

---
 nhf/checks.py                   | 27 +++++++++++++++++++++++++++
 nhf/parts/joints.py             |  1 +
 nhf/parts/test.py               |  4 ++--
 nhf/touhou/houjuu_nue/joints.py |  1 +
 nhf/touhou/houjuu_nue/test.py   |  4 ++++
 5 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/nhf/checks.py b/nhf/checks.py
index a5a413d..1f1f4c3 100644
--- a/nhf/checks.py
+++ b/nhf/checks.py
@@ -6,6 +6,33 @@ def binary_intersection(a: Cq.Assembly) -> Cq.Shape:
     obj1, obj2 = objs[:2]
     return obj1.intersect(obj2)
 
+def visualize_intersection(assembly: Cq.Assembly, tol: float=1e-6) -> Cq.Shape:
+    """
+    Given an assembly, test the pairwise intersection volume of its components.
+    Return the pairs whose intersection volume exceeds `tol`.
+    """
+    m = {name: (i, shape.moved(loc))
+         for i, (shape, name, loc, _)
+         in enumerate(assembly)}
+    for name, (i1, sh1) in m.items():
+        for name2, (i2, sh2) in m.items():
+            if name == name2:
+                assert i1 == i2
+                continue
+            if i2 <= i1:
+                # Remove the upper diagonal
+                continue
+            head = name.split('/', 2)[1]
+            head2 = name2.split('/', 2)[1]
+            if head == head2:
+                # Do not test into subassemblies
+                continue
+
+            isect = sh1.intersect(sh2)
+            vol = isect.Volume()
+            if vol > tol:
+                return isect
+    return None
 
 def pairwise_intersection(assembly: Cq.Assembly, tol: float=1e-6) -> list[(str, str, float)]:
     """
diff --git a/nhf/parts/joints.py b/nhf/parts/joints.py
index f80b1b6..20fd507 100644
--- a/nhf/parts/joints.py
+++ b/nhf/parts/joints.py
@@ -218,6 +218,7 @@ class TorsionJoint:
         assert self.radius_rider > self.groove_radius_outer > self.groove_radius_inner + self.groove_inner_gap
         assert self.groove_radius_inner > self.spring.radius > self.radius_axle
         assert self.spring.height > self.groove_depth, "Groove is too deep"
+        assert self.groove_depth < self.spring.height - self.spring.thickness * 2
 
     @property
     def total_height(self):
diff --git a/nhf/parts/test.py b/nhf/parts/test.py
index b711efa..6e01420 100644
--- a/nhf/parts/test.py
+++ b/nhf/parts/test.py
@@ -1,7 +1,7 @@
 import unittest
 import cadquery as Cq
 from nhf.checks import binary_intersection, pairwise_intersection
-from nhf.parts import joints, handle, metric_threads
+from nhf.parts import joints, handle, metric_threads, springs
 
 class TestJoints(unittest.TestCase):
 
@@ -36,7 +36,7 @@ class TestJoints(unittest.TestCase):
             with self.subTest(slot=slot, right_handed=False):
                 self.torsion_joint_case(j, slot)
     def test_torsion_joint_right_handed(self):
-        j = joints.TorsionJoint(right_handed=True)
+        j = joints.TorsionJoint(springs.TorsionSpring(right_handed=True))
         for slot in range(j.rider_n_slots):
             with self.subTest(slot=slot, right_handed=True):
                 self.torsion_joint_case(j, slot)
diff --git a/nhf/touhou/houjuu_nue/joints.py b/nhf/touhou/houjuu_nue/joints.py
index 2dcd131..a01bb14 100644
--- a/nhf/touhou/houjuu_nue/joints.py
+++ b/nhf/touhou/houjuu_nue/joints.py
@@ -18,6 +18,7 @@ class ShoulderJoint(Model):
     torsion_joint: TorsionJoint = field(default_factory=lambda: TorsionJoint(
         radius_track=18,
         radius_rider=18,
+        groove_depth=4.8,
         groove_radius_outer=16,
         groove_radius_inner=13,
         track_disk_height=5.0,
diff --git a/nhf/touhou/houjuu_nue/test.py b/nhf/touhou/houjuu_nue/test.py
index 0fcda84..01fcc74 100644
--- a/nhf/touhou/houjuu_nue/test.py
+++ b/nhf/touhou/houjuu_nue/test.py
@@ -6,6 +6,10 @@ from nhf.checks import pairwise_intersection
 
 class TestJoints(unittest.TestCase):
 
+    def test_shoulder_collision_of_torsion_joint(self):
+        j = MJ.ShoulderJoint()
+        assembly = j.torsion_joint.rider_track_assembly()
+        self.assertEqual(pairwise_intersection(assembly), [])
     def test_shoulder_collision_0(self):
         j = MJ.ShoulderJoint()
         assembly = j.assembly()