neocturne/htanks
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

WiimotePlayer: Smooth IR aiming

Browse source
This commit is contained in:
Matthias Schiffer 2010-04-10 15:17:55 +02:00
parent 546da85814
commit f78f24af9b
1 changed files with 49 additions and 33 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

74
src/WiimotePlayer.hs
View file

@ -22,11 +22,11 @@ import Player
import Tank import Tank
data WiimotePlayer = WiimotePlayer Wiimote (Float, Float) data WiimotePlayer = WiimotePlayer Wiimote WiimoteAccCal [(Float, Float)]
deriving (Typeable, Show) deriving (Typeable, Show)
instance Player WiimotePlayer where instance Player WiimotePlayer where
playerUpdate (WiimotePlayer wiimote oldaim) tank = do playerUpdate (WiimotePlayer wiimote cal oldaims) tank = do
state <- hwiidGetState wiimote state <- hwiidGetState wiimote
messages <- hwiidGetMesg wiimote messages <- hwiidGetMesg wiimote
@ -44,8 +44,13 @@ instance Player WiimotePlayer where
ny = ((fromIntegral . extNunchukStickY $ 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) in if (nx*nx + ny*ny) < 0.4 then (x, y) else (x+nx, y+ny)
iraim = handleIR state iraim = handleIR state cal
(aimx, aimy) = if isJust iraim then fromJust iraim else oldaim 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) ax = aimx - (fromRational . toRational . tankX $ tank)
ay = aimy - (fromRational . toRational . tankY $ 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 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 angle = atan2 my mx
moveangle = if move then Just $ fromRational $ round ((angle - (sin $ 8*x)/8)*1000000*180/pi)%1000000 else Nothing moveangle = if move then Just $ fromRational $ round ((angle - (sin $ 8*x)/8)*1000000*180/pi)%1000000 else Nothing
when foo $ print $ state 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 renderPlayer (WiimotePlayer _ _ []) = return ()
translate $ Vector3 x y (0 :: GLfloat) renderPlayer (WiimotePlayer _ _ aims) = unsafePreservingMatrix $ do
let (x, y) = mulV (1/(fromIntegral $ length aims)) $ foldr addV (0, 0) aims
unsafeRenderPrimitive Quads $ do translate $ Vector3 x y (0 :: GLfloat)
texCoord $ TexCoord2 (0 :: GLfloat) (0 :: GLfloat)
vertex $ Vertex2 (-0.2 :: GLfloat) (-0.2 :: GLfloat)
texCoord $ TexCoord2 (0 :: GLfloat) (1 :: GLfloat) unsafeRenderPrimitive Quads $ do
vertex $ Vertex2 (-0.2 :: GLfloat) (0.2 :: GLfloat) texCoord $ TexCoord2 (0 :: GLfloat) (0 :: GLfloat)
vertex $ Vertex2 (-0.2 :: GLfloat) (-0.2 :: GLfloat)
texCoord $ TexCoord2 (1 :: GLfloat) (1 :: GLfloat) texCoord $ TexCoord2 (0 :: GLfloat) (1 :: GLfloat)
vertex $ Vertex2 (0.2 :: GLfloat) (0.2 :: GLfloat) vertex $ Vertex2 (-0.2 :: GLfloat) (0.2 :: GLfloat)
texCoord $ TexCoord2 (1 :: GLfloat) (0 :: GLfloat) texCoord $ TexCoord2 (1 :: GLfloat) (1 :: GLfloat)
vertex $ Vertex2 (0.2 :: GLfloat) (-0.2 :: 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 :: Float
irXScale = 20 irXScale = 20
@ -85,8 +97,8 @@ irYScale = 20
irYTranslate :: Float irYTranslate :: Float
irYTranslate = -10 + 8 irYTranslate = -10 + 8
handleIR :: WiimoteState -> Maybe (Float, Float) handleIR :: WiimoteState -> WiimoteAccCal -> Maybe (Float, Float)
handleIR state = handle $ sortIRSourcesByPos $ take 2 $ sortIRSourcesBySize $ stateIRSources state handleIR state cal = handle $ sortIRSourcesByPos $ take 2 $ sortIRSourcesBySize $ stateIRSources state
where where
handle [ira,irb] = let pa = pos ira handle [ira,irb] = let pa = pos ira
pb = pos irb 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) pos src = (((fromIntegral $ irPosX src) - hMaxX)/hMaxX, ((fromIntegral $ irPosY src) - hMaxY)/hMaxY)
rot (x, y) = let s = (fromIntegral . stateAccX $ state) - 0x80 rot (x, y) = let fi = fromIntegral
c = (fromIntegral . stateAccZ $ state) - 0x80 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) in (c*x + s*y, -s*x + c*y)
hMaxX = (fromIntegral hwiidIRMaxX)/2 hMaxX = (fromIntegral hwiidIRMaxX)/2
hMaxY = (fromIntegral hwiidIRMaxY)/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 :: [WiimoteIRSource] -> [WiimoteIRSource]
sortIRSourcesBySize = sortBy (flip compare `on` irSize) sortIRSourcesBySize = sortBy (flip compare `on` irSize)
sortIRSourcesByPos :: [WiimoteIRSource] -> [WiimoteIRSource] sortIRSourcesByPos :: [WiimoteIRSource] -> [WiimoteIRSource]
sortIRSourcesByPos = sortBy (compare `on` (fst . rot . pos)) 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 :: IO WiimotePlayer
newWiimotePlayer = do newWiimotePlayer = do
wiimote <- hwiidOpen bdAddrAny (hwiidFlagMesgInterface .|. hwiidFlagNonblock) wiimote <- hwiidOpen bdAddrAny (hwiidFlagMesgInterface .|. hwiidFlagNonblock)
when (wiimote == nullWiimote) $ fail "Wiimote error" when (wiimote == nullWiimote) $ fail "Wiimote error"
hwiidSetReportMode wiimote (hwiidReportButtons .|. hwiidReportAcc .|. hwiidReportIR .|. hwiidReportNunchuk) 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 :: (Bits a) => a -> a -> Bool
test field bits = (field .&. bits) == bits test field bits = (field .&. bits) == bits