Move hands to touch walls

src/Dodge/Creature/HandPos.hs

@@ -14,6 +14,9 @@ module Dodge.Creature.HandPos (
     rightHandPQ,
 ) where
 
+import Control.Monad
+import qualified Data.IntMap.Strict as IM
+import Linear
 import Control.Lens
 import Dodge.Creature.Test
 import Dodge.Data.Creature
@@ -61,11 +64,44 @@ rightHandPQ cr
     | oneH cr = (V3 11 (-3) 20, Q.qID)
     | twists cr = (V3 0 5 20, Q.qz (-1)) `Q.comp` (V3 4 (-10) 0, Q.qID)
     | twoFlat cr = (V3 4 (-8) 12, Q.qID)
---    | wl <- crRightHandWall cr = (0, Q.qID)
+    | Just p <- crRightHandWall cr = (20 & _xy .~ p, Q.qID)
     | otherwise = (handWalkingPos LeftForward (-8) cr, Q.qID)
 
---crRightHandWall :: IM.IntMap Wall -> Creature -> Wall
---crRightHandWall wls cr
+crRightHandWall :: Creature -> Maybe Point2
+crRightHandWall cr = do
+    (_,p) <- IM.lookupMin (cr ^. crWallTouch)
+    let wp = p - cr ^. crPos . _xy 
+        dp = argV wp
+        ang = nearZeroAngle (cd - dp)
+        cdp = unitVectorAtAngle cd
+        x   | ang < 0 = 0.8 - (0.5 * ang / (pi / 4))
+            | otherwise =  0.8* dotV cdp (signorm wp)
+        y   | ang < 0 = 2 - (5 * ang / (pi / 4))
+            | otherwise = 2
+        ep = wp - y *^ signorm wp + x *^ vNormal wp
+    guard $ ang > negate (pi / 8)
+    return $ ep & _xy %~ rot
+  where
+    cd = cr ^. crDir
+    rot = rotateV (negate cd)
+
+crLeftHandWall :: Creature -> Maybe Point2
+crLeftHandWall cr = do
+    (_,p) <- IM.lookupMin (cr ^. crWallTouch)
+    let wp = p - cr ^. crPos . _xy 
+        dp = argV wp
+        ang = nearZeroAngle (cd - dp)
+        cdp = unitVectorAtAngle cd
+        x   | ang > 0 = 0.8 + (0.5 * ang / (pi / 4))
+            | otherwise =  0.8* dotV cdp (signorm wp)
+        y   | ang > 0 = 2 + (5 * ang / (pi / 4))
+            | otherwise = 2
+        ep = wp - y *^ signorm wp - x *^ vNormal wp
+    guard $ ang < (pi / 8)
+    return $ ep & _xy %~ rot
+  where
+    cd = cr ^. crDir
+    rot = rotateV (negate cd)
 
 translateToRightHand :: Creature -> SPic -> SPic
 translateToRightHand = overPosSP . translatePointToRightHand
@@ -75,9 +111,10 @@ rightWristPQ cr = rightHandPQ cr `Q.comp` (V3 0 (-4) (-4), Q.qID)
 
 leftHandPQ :: Creature -> Point3Q
 leftHandPQ cr
-    | oneH cr = (V3 0 8 10, Q.qz 0.4)
     | twists cr = (V3 0 5 20, Q.qz (-1)) `Q.comp` (V3 12 4 0, Q.qz 0.4)
     | twoFlat cr = (V3 4 8 12, Q.qID)
+    | Just p <- crLeftHandWall cr = (20 & _xy .~ p, Q.qID)
+    | oneH cr = (V3 0 8 10, Q.qz 0.4)
     | otherwise = (handWalkingPos RightForward 8 cr, Q.qID)
 
 translatePointToLeftHand :: Creature -> Point3 -> Point3