WiimotePlayer: Smooth IR aiming

1 files changed, 49 insertions, 33 deletions

diff --git a/src/WiimotePlayer.hs b/src/WiimotePlayer.hs
index 75f923b..6a93f22 100644
--- a/src/WiimotePlayer.hs
+++ b/src/WiimotePlayer.hs
@@ -22,11 +22,11 @@ import Player
 import Tank
 
 
-data WiimotePlayer = WiimotePlayer Wiimote (Float, Float)
+data WiimotePlayer = WiimotePlayer Wiimote WiimoteAccCal [(Float, Float)]
                      deriving (Typeable, Show)
 
 instance Player WiimotePlayer where
-    playerUpdate (WiimotePlayer wiimote oldaim) tank = do
+    playerUpdate (WiimotePlayer wiimote cal oldaims) tank = do
                                 state <- hwiidGetState wiimote
                                 messages <- hwiidGetMesg wiimote
                                 
@@ -44,8 +44,13 @@ instance Player WiimotePlayer where
                                                      ny = ((fromIntegral . extNunchukStickY $ ext) - 0x80)/0x80
                                                  in if (nx*nx + ny*ny) < 0.4 then (x, y) else (x+nx, y+ny)
                                     
-                                    iraim = handleIR state
-                                    (aimx, aimy) = if isJust iraim then fromJust iraim else oldaim
+                                    iraim = handleIR state cal
+                                    newaims = if isJust iraim then take irSmooth ((fromJust iraim):oldaims) else oldaims
+                                    newaim = if null newaims then (0, 0) else mulV (1/(fromIntegral $ length newaims)) (foldr addV (0, 0) newaims)
+                                    aims = if not (null newaims) && (lengthV $ subV (head newaims) newaim) > irSkip
+                                           then take irSkipSmooth newaims
+                                           else newaims
+                                    (aimx, aimy) = if null aims then (0, 0) else mulV (1/(fromIntegral $ length aims)) (foldr addV (0, 0) aims)
                                     ax = aimx - (fromRational . toRational . tankX $ tank)
                                     ay = aimy - (fromRational . toRational . tankY $ tank)
                                     aangle = if (ax /= 0 || ay /= 0) then Just $ fromRational $ round ((atan2 ay ax)*1000000*180/pi)%1000000 else Nothing
@@ -54,24 +59,31 @@ instance Player WiimotePlayer where
                                     angle = atan2 my mx
                                     moveangle = if move then Just $ fromRational $ round ((angle - (sin $ 8*x)/8)*1000000*180/pi)%1000000 else Nothing
                                 when foo $ print $ state
-                                return (WiimotePlayer wiimote (aimx, aimy), moveangle, move, aangle, shoot)
+                                return (WiimotePlayer wiimote cal aims, moveangle, move, aangle, shoot)
     
-    renderPlayer (WiimotePlayer _ (x, y)) = unsafePreservingMatrix $ do
-                                translate $ Vector3 x y (0 :: GLfloat)
+    renderPlayer (WiimotePlayer _ _ [])   = return ()
+    renderPlayer (WiimotePlayer _ _ aims) = unsafePreservingMatrix $ do
+                                                  let (x, y) = mulV (1/(fromIntegral $ length aims)) $ foldr addV (0, 0) aims
+                                                  
+                                                  translate $ Vector3 x y (0 :: GLfloat)
                                        
-                                unsafeRenderPrimitive Quads $ do
-                                           texCoord $ TexCoord2 (0 :: GLfloat) (0 :: GLfloat)
-                                           vertex $ Vertex2 (-0.2 :: GLfloat) (-0.2 :: GLfloat)
-                                           
-                                           texCoord $ TexCoord2 (0 :: GLfloat) (1 :: GLfloat)
-                                           vertex $ Vertex2 (-0.2 :: GLfloat) (0.2 :: GLfloat)
-                                           
-                                           texCoord $ TexCoord2 (1 :: GLfloat) (1 :: GLfloat)
-                                           vertex $ Vertex2 (0.2 :: GLfloat) (0.2 :: GLfloat)
-                                           
-                                           texCoord $ TexCoord2 (1 :: GLfloat) (0 :: GLfloat)
-                                           vertex $ Vertex2 (0.2 :: GLfloat) (-0.2 :: GLfloat)
-           
+                                                  unsafeRenderPrimitive Quads $ do
+                                                                     texCoord $ TexCoord2 (0 :: GLfloat) (0 :: GLfloat)
+                                                                     vertex $ Vertex2 (-0.2 :: GLfloat) (-0.2 :: GLfloat)
+                                                                     
+                                                                     texCoord $ TexCoord2 (0 :: GLfloat) (1 :: GLfloat)
+                                                                     vertex $ Vertex2 (-0.2 :: GLfloat) (0.2 :: GLfloat)
+                                                                     
+                                                                     texCoord $ TexCoord2 (1 :: GLfloat) (1 :: GLfloat)
+                                                                     vertex $ Vertex2 (0.2 :: GLfloat) (0.2 :: GLfloat)
+                                                                     
+                                                                     texCoord $ TexCoord2 (1 :: GLfloat) (0 :: GLfloat)
+                                                                     vertex $ Vertex2 (0.2 :: GLfloat) (-0.2 :: GLfloat)
+
+
+irSmooth = 10
+irSkip = 0.2
+irSkipSmooth = 4
 
 irXScale :: Float
 irXScale = 20
@@ -85,8 +97,8 @@ irYScale = 20
 irYTranslate :: Float
 irYTranslate = -10 + 8
 
-handleIR :: WiimoteState -> Maybe (Float, Float)
-handleIR state = handle $ sortIRSourcesByPos $ take 2 $ sortIRSourcesBySize $ stateIRSources state
+handleIR :: WiimoteState -> WiimoteAccCal -> Maybe (Float, Float)
+handleIR state cal = handle $ sortIRSourcesByPos $ take 2 $ sortIRSourcesBySize $ stateIRSources state
     where
       handle [ira,irb] = let pa = pos ira
                              pb = pos irb
@@ -100,33 +112,37 @@ handleIR state = handle $ sortIRSourcesByPos $ take 2 $ sortIRSourcesBySize $ st
       
       pos src = (((fromIntegral $ irPosX src) - hMaxX)/hMaxX, ((fromIntegral $ irPosY src) - hMaxY)/hMaxY)
       
-      rot (x, y) = let s = (fromIntegral . stateAccX $ state) - 0x80
-                       c = (fromIntegral . stateAccZ $ state) - 0x80
+      rot (x, y) = let fi = fromIntegral
+                       s = ((fi . stateAccX $ state)-(fi . accCalZeroX $ cal))/((fi . accCalOneX $ cal)-(fi . accCalZeroX $ cal))
+                       c = ((fi . stateAccZ $ state)-(fi . accCalZeroZ $ cal))/((fi . accCalOneZ $ cal)-(fi . accCalZeroZ $ cal))
                    in (c*x + s*y, -s*x + c*y)
       
       hMaxX = (fromIntegral hwiidIRMaxX)/2
       hMaxY = (fromIntegral hwiidIRMaxY)/2
       
-      negV (a1, a2) = (-a1, -a2)
-      subV (a1, a2) (b1, b2) = (a1-b1, a2-b2)
-      dotV (a1, a2) (b1, b2) = a1*b1 + a2*b2
-      mulV x (a1, a2) = (x*a1, x*a2)
-      sinV (a1, a2) (b1, b2) = (a1 * b2 - b1 * a2)
-      lengthSqV a = dotV a a
-      lengthV a = sqrt $ lengthSqV a
-      
       sortIRSourcesBySize :: [WiimoteIRSource] -> [WiimoteIRSource]
       sortIRSourcesBySize = sortBy (flip compare `on` irSize)
       
       sortIRSourcesByPos :: [WiimoteIRSource] -> [WiimoteIRSource]
       sortIRSourcesByPos = sortBy (compare `on` (fst . rot . pos))
 
+negV (a1, a2) = (-a1, -a2)
+addV (a1, a2) (b1, b2) = (a1+b1, a2+b2)
+subV (a1, a2) (b1, b2) = (a1-b1, a2-b2)
+dotV (a1, a2) (b1, b2) = a1*b1 + a2*b2
+mulV x (a1, a2) = (x*a1, x*a2)
+sinV (a1, a2) (b1, b2) = (a1 * b2 - b1 * a2)
+lengthSqV a = dotV a a
+lengthV a = sqrt $ lengthSqV a
+      
+
 newWiimotePlayer :: IO WiimotePlayer
 newWiimotePlayer = do
   wiimote <- hwiidOpen bdAddrAny (hwiidFlagMesgInterface .|. hwiidFlagNonblock)
   when (wiimote == nullWiimote) $ fail "Wiimote error"
   hwiidSetReportMode wiimote (hwiidReportButtons .|. hwiidReportAcc .|. hwiidReportIR .|. hwiidReportNunchuk)
-  return $ WiimotePlayer wiimote (0, 0)
+  cal <- hwiidGetAccCal wiimote hwiidExtNone
+  return $ WiimotePlayer wiimote cal []
 
 test :: (Bits a) => a -> a -> Bool
 test field bits = (field .&. bits) == bits