From cdb46263f8386761e1e840b0d615cda9143ab338 Mon Sep 17 00:00:00 2001
From: Leni Aniva <v@leni.sh>
Date: Tue, 16 Jul 2024 12:03:51 -0700
Subject: [PATCH] fix: Ambiguous rotation in Hirth Joint

---
 nhf/parts/joints.py               | 28 ++++++++++++++++++----------
 nhf/touhou/houjuu_nue/__init__.py | 19 ++++++++++---------
 nhf/touhou/houjuu_nue/wing.py     |  9 ++++-----
 3 files changed, 32 insertions(+), 24 deletions(-)

diff --git a/nhf/parts/joints.py b/nhf/parts/joints.py
index 77dcc94..3ff76e9 100644
--- a/nhf/parts/joints.py
+++ b/nhf/parts/joints.py
@@ -101,7 +101,12 @@ class HirthJoint:
         (
             result
             .polyline([(0, 0, 0), (1, 0, 0)], forConstruction=True)
-            .tag("dir")
+            .tag("dirX")
+        )
+        (
+            result
+            .polyline([(0, 0, 0), (0, 1, 0)], forConstruction=True)
+            .tag("dirY")
         )
         return result
 
@@ -109,12 +114,15 @@ class HirthJoint:
                         assembly: Cq.Assembly,
                         parent: str,
                         child: str,
-                        angle: int = 0):
+                        offset: int = 0):
+        angle = offset * self.tooth_angle
         (
             assembly
             .constrain(f"{parent}?mate", f"{child}?mate", "Plane")
-            .constrain(f"{parent}?dir", f"{child}?dir",
-                       "Axis", param=angle * self.tooth_angle)
+            .constrain(f"{parent}?dirX", f"{child}?dirX",
+                       "Axis", param=angle)
+            .constrain(f"{parent}?dirY", f"{child}?dirX",
+                       "Axis", param=90 - angle)
         )
 
     def assembly(self, offset: int = 1):
@@ -138,17 +146,17 @@ class HirthJoint:
             self.generate(is_mated=True)
             .union(tab)
         )
-        angle = offset * self.tooth_angle
         result = (
             Cq.Assembly()
             .addS(obj1, name="obj1", role=Role.PARENT)
             .addS(obj2, name="obj2", role=Role.CHILD)
-            .constrain("obj1", "Fixed")
-            .constrain("obj1?mate", "obj2?mate", "Plane")
-            .constrain("obj1?dir", "obj2?dir", "Axis", param=angle)
-            .solve()
         )
-        return result
+        self.add_constraints(
+            result,
+            parent="obj1",
+            child="obj2",
+            offset=offset)
+        return result.solve()
 
 def comma_joint(radius=30,
                 shaft_radius=10,
diff --git a/nhf/touhou/houjuu_nue/__init__.py b/nhf/touhou/houjuu_nue/__init__.py
index ea07fad..0a05a35 100644
--- a/nhf/touhou/houjuu_nue/__init__.py
+++ b/nhf/touhou/houjuu_nue/__init__.py
@@ -30,6 +30,7 @@ s1, s2, s3. The joints are named (from root to tip)
 shoulder, elbow, wrist in analogy with human anatomy.
 """
 from dataclasses import dataclass, field
+from typing import Optional
 import cadquery as Cq
 from nhf.build import Model, TargetKind, target, assembly
 from nhf.parts.joints import HirthJoint, TorsionJoint
@@ -147,11 +148,11 @@ class Parameters(Model):
 
 
     @assembly()
-    def wing_r1_assembly(self) -> Cq.Assembly:
-        return self.wing_profile.assembly()
+    def wing_r1_assembly(self, parts: Optional[list[str]] = None) -> Cq.Assembly:
+        return self.wing_profile.assembly(parts)
 
     @assembly()
-    def wings_harness_assembly(self) -> Cq.Assembly:
+    def wings_harness_assembly(self, parts: Optional[list[str]] = None) -> Cq.Assembly:
         """
         Assembly of harness with all the wings
         """
@@ -160,13 +161,13 @@ class Parameters(Model):
         result = (
             Cq.Assembly()
             .add(self.harness_assembly(), name="harness", loc=Cq.Location((0, 0, 0)))
-            .add(self.wing_r1_assembly(), name="wing_r1")
-            .add(self.wing_r1_assembly(), name="wing_r2")
-            .add(self.wing_r1_assembly(), name="wing_r3")
+            .add(self.wing_r1_assembly(parts), name="wing_r1")
+            .add(self.wing_r1_assembly(parts), name="wing_r2")
+            .add(self.wing_r1_assembly(parts), name="wing_r3")
         )
-        self.hs_hirth_joint.add_constraints(result, "harness/r1", "wing_r1/s0/hs", angle=9)
-        self.hs_hirth_joint.add_constraints(result, "harness/r2", "wing_r2/s0/hs", angle=8)
-        self.hs_hirth_joint.add_constraints(result, "harness/r3", "wing_r3/s0/hs", angle=7)
+        self.hs_hirth_joint.add_constraints(result, "harness/r1", "wing_r1/s0/hs", offset=9)
+        self.hs_hirth_joint.add_constraints(result, "harness/r2", "wing_r2/s0/hs", offset=8)
+        self.hs_hirth_joint.add_constraints(result, "harness/r3", "wing_r3/s0/hs", offset=7)
         return result.solve()
 
     @assembly(collision_check=False)
diff --git a/nhf/touhou/houjuu_nue/wing.py b/nhf/touhou/houjuu_nue/wing.py
index 9d84552..b520384 100644
--- a/nhf/touhou/houjuu_nue/wing.py
+++ b/nhf/touhou/houjuu_nue/wing.py
@@ -192,7 +192,7 @@ class WingProfile(Model):
                 .constrain(f"{tag}?top", f"top?{tag}", "Plane")
                 .constrain(f"{tag}?dir", f"top?{tag}_dir", "Axis")
             )
-        hirth = self.base_joint.generate()
+        hirth = self.base_joint.generate(is_mated=True)
         (
             result
             .addS(hirth, name="hs", role=Role.CHILD, material=self.mat_hs_joint)
@@ -614,8 +614,7 @@ class WingProfile(Model):
                 .constrain("s3/wrist_bot?conn0", "wrist/child/bot?conn0", "Plane")
                 .constrain("s3/wrist_bot?conn1", "wrist/child/bot?conn1", "Plane")
             )
+        if len(parts) > 1:
+            result.solve()
 
-        return result.solve()
-
-
-
+        return result