diff options
| -rw-r--r-- | src/WiimotePlayer.hs | 77 |
1 files changed, 64 insertions, 13 deletions
diff --git a/src/WiimotePlayer.hs b/src/WiimotePlayer.hs index 3907615..7804b26 100644 --- a/src/WiimotePlayer.hs +++ b/src/WiimotePlayer.hs @@ -1,11 +1,13 @@ {-# LANGUAGE DeriveDataTypeable #-} -module WiimotePlayer ( WiimotePlayer(..) +module WiimotePlayer ( WiimotePlayer , newWiimotePlayer ) where import Control.Monad import Data.Bits +import Data.Function (on) +import Data.List (sortBy) import Data.Ratio ((%)) import Data.Typeable import HWiid @@ -28,30 +30,79 @@ instance Player WiimotePlayer where y = (if (test buttons hwiidButtonDown) then (-1) else 0) + (if (test buttons hwiidButtonUp) then 1 else 0) ext = stateExt state - (mx, my) <- if (extType ext) /= hwiidExtNunchuk - then return (x, y) - else do - let nx = ((fromIntegral . extNunchukStickX $ ext) - 0x80)/0x80 - ny = ((fromIntegral . extNunchukStickY $ ext) - 0x80)/0x80 - return $ if (nx*nx + ny*ny) < 0.4 then (x, y) else (x+nx, y+ny) + (mx, my) = if (extType ext) /= hwiidExtNunchuk + then (x, y) + else + let nx = ((fromIntegral . extNunchukStickX $ ext) - 0x80)/0x80 + ny = ((fromIntegral . extNunchukStickY $ ext) - 0x80)/0x80 + in if (nx*nx + ny*ny) < 0.4 then (x, y) else (x+nx, y+ny) --ax = aimx - (fromRational . toRational . tankX $ tank) --ay = aimy - (fromRational . toRational . tankY $ tank) - let move = (mx /= 0 || my /= 0) - angle = if move then Just $ fromRational $ round ((atan2 my mx)*1000000*180/pi)%1000000 else Nothing + move = (mx /= 0 || my /= 0) + angle = atan2 my mx + moveangle = if move then Just $ fromRational $ round ((angle - (sin $ 8*x)/8)*1000000*180/pi)%1000000 else Nothing --aangle = if (ax /= 0 || ay /= 0) then Just $ fromRational $ round ((atan2 ay ax)*1000000*180/pi)%1000000 else Nothing - when foo $ print state - return (WiimotePlayer wiimote, angle, move, Nothing, shoot) + when foo $ print $ handleIR state + return (WiimotePlayer wiimote, moveangle, move, Nothing, shoot) +irXScale :: Float +irXScale = 1 + +irXTranslate :: Float +irXTranslate = 0 + +irYScale :: Float +irYScale = 1 + +irYTranslate :: Float +irYTranslate = 0 + +handleIR :: WiimoteState -> Maybe (Float, Float) +handleIR state = handle $ sortIRSourcesByPos $ take 2 $ sortIRSourcesBySize $ stateIRSources state + where + handle [ira,irb] = let pa = pos ira + pb = pos irb + p = negV pa + b = pb `subV` pa + x = (p `dotV` b)/(lengthV b) + y = (sqrt ((lengthSqV p) - x*x)) * (signum $ sinV b p) + in Just ((x - (lengthV b)/2)*irXScale + irXTranslate, y*irYScale + irYTranslate) + + handle _ = Nothing + + 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 + 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)) + newWiimotePlayer :: IO WiimotePlayer newWiimotePlayer = do wiimote <- hwiidOpen bdAddrAny (hwiidFlagMesgInterface .|. hwiidFlagNonblock) when (wiimote == nullWiimote) $ fail "Wiimote error" - hwiidSetReportMode wiimote (hwiidReportButtons .|. hwiidReportIR .|. hwiidReportNunchuk) + hwiidSetReportMode wiimote (hwiidReportButtons .|. hwiidReportAcc .|. hwiidReportIR .|. hwiidReportNunchuk) return $ WiimotePlayer wiimote test :: (Bits a) => a -> a -> Bool -test field bits = (field .&. bits) == bits
\ No newline at end of file +test field bits = (field .&. bits) == bits |