From 7f9d837bf0769a772b4c7b84120e2bbf4ce0b8aa Mon Sep 17 00:00:00 2001
From: justin <justin.g.kruger@gmail.com>
Date: Sat, 9 Aug 2025 10:07:55 +0100
Subject: [PATCH] Add quaternions to body positions

---
 src/Dodge/Creature/HandPos.hs | 69 +++++++++++++++++++++++++----------
 1 file changed, 49 insertions(+), 20 deletions(-)

diff --git a/src/Dodge/Creature/HandPos.hs b/src/Dodge/Creature/HandPos.hs
index a176c36a7..a7759f2ec 100644
--- a/src/Dodge/Creature/HandPos.hs
+++ b/src/Dodge/Creature/HandPos.hs
@@ -160,27 +160,53 @@ translateToLeftWrist cr = overPosSP
 --    f i = negate 2 + negate 6 * (sLen - i) / sLen
 --    g i = negate 2 + negate 6 * (i) / sLen
 
-translateToLeftLeg :: Creature -> SPic -> SPic
-translateToLeftLeg cr =
-    rotateSP (_crMvDir cr - _crDir cr) . case cr ^? crStance . carriage of
-        Just (Walking sa LeftForward) -> translateSPxy (f sa) off
-        Just (Walking sa RightForward) -> translateSPxy (- f sa) off
-        _ -> translateSPxy 0 off
+leftLegPQ :: Creature -> Point3Q
+leftLegPQ cr = Q.comp (0,Q.qz (_crMvDir cr - _crDir cr))
+    (V3 x off 0,Q.qID)
   where
+    x = case cr ^? crStance . carriage of
+        Just (Walking sa LeftForward) -> f sa
+        Just (Walking sa RightForward) -> - f sa
+        _ -> 0
     off = 5
     sLen = _strideLength $ _crStance cr
     f i = 8 * (sLen - i) / sLen
+    
+
+translateToLeftLeg :: Creature -> SPic -> SPic
+translateToLeftLeg cr = overPosSP (\p -> fst (leftLegPQ cr `Q.comp` (p,Q.qID)))
+--    rotateSP (_crMvDir cr - _crDir cr) . case cr ^? crStance . carriage of
+--        Just (Walking sa LeftForward) -> translateSPxy (f sa) off
+--        Just (Walking sa RightForward) -> translateSPxy (- f sa) off
+--        _ -> translateSPxy 0 off
+--  where
+--    off = 5
+--    sLen = _strideLength $ _crStance cr
+--    f i = 8 * (sLen - i) / sLen
+
+rightLegPQ :: Creature -> Point3Q
+rightLegPQ cr = Q.comp (0,Q.qz (_crMvDir cr - _crDir cr))
+    (V3 x off 0,Q.qID)
+  where
+    x = case cr ^? crStance . carriage of
+        Just (Walking sa LeftForward) -> -f sa
+        Just (Walking sa RightForward) -> f sa
+        _ -> 0
+    off = -5
+    sLen = _strideLength $ _crStance cr
+    f i = 8 * (sLen - i) / sLen
 
 translateToRightLeg :: Creature -> SPic -> SPic
-translateToRightLeg cr =
-    rotateSP (_crMvDir cr - _crDir cr) . case cr ^? crStance . carriage of
-        Just (Walking sa LeftForward) -> translateSPxy (- f sa) (- off)
-        Just (Walking sa RightForward) -> translateSPxy (f sa) (- off)
-        _ -> translateSPxy 0 (- off)
-  where
-    off = 5
-    sLen = _strideLength $ _crStance cr
-    f i = 8 * (sLen - i) / sLen
+translateToRightLeg cr = overPosSP (\p -> fst (rightLegPQ cr `Q.comp` (p,Q.qID)))
+--translateToRightLeg cr =
+--    rotateSP (_crMvDir cr - _crDir cr) . case cr ^? crStance . carriage of
+--        Just (Walking sa LeftForward) -> translateSPxy (- f sa) (- off)
+--        Just (Walking sa RightForward) -> translateSPxy (f sa) (- off)
+--        _ -> translateSPxy 0 (- off)
+--  where
+--    off = 5
+--    sLen = _strideLength $ _crStance cr
+--    f i = 8 * (sLen - i) / sLen
 
 translateToHead :: Creature -> SPic -> SPic
 translateToHead cr = overPosSP (\p -> fst $ (headPQ cr `Q.comp` (p,Q.qID)))
@@ -208,12 +234,15 @@ translatePointToHead cr p = fst (headPQ cr `Q.comp` (p,Q.qID))
 --            . rotate3 (negate 0.5)
 --    | otherwise = (+.+.+ V3 2.5 0 20)
 
+chestPQ :: Creature -> Point3Q
+chestPQ cr = backPQ cr `Q.comp` (0,Q.qz pi)
+
 translateToChest :: Creature -> SPic -> SPic
-translateToChest cr
-    | oneH cr = rotateSP 0.5
-    --    | twists cr = translateSPxy 0 5 . rotateSP (-1)
-    | twists cr = rotateSP (-1)
-    | otherwise = id
+translateToChest cr = overPosSP (\p -> fst $ chestPQ cr `Q.comp` (p,Q.qID))
+--    | oneH cr = rotateSP 0.5
+--    --    | twists cr = translateSPxy 0 5 . rotateSP (-1)
+--    | twists cr = rotateSP (-1)
+--    | otherwise = id
 
 backPQ :: Creature -> Point3Q
 backPQ cr