diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/WiimotePlayer.hs | 82 |
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 |