1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
|
{-# LANGUAGE DeriveDataTypeable #-}
module WiimotePlayer ( WiimotePlayer
, newWiimotePlayer
) where
import Control.Monad
import Data.Bits
import Data.Function (on)
import Data.List (sortBy)
import Data.Maybe
import Data.Ratio ((%))
import Data.Typeable
import HWiid
import Graphics.Rendering.OpenGL.GL (GLfloat, Vector3(..))
import Graphics.Rendering.OpenGL.GL.BeginEnd (unsafeRenderPrimitive, PrimitiveMode(..))
import Graphics.Rendering.OpenGL.GL.CoordTrans (unsafePreservingMatrix, translate)
import Graphics.Rendering.OpenGL.GL.VertexSpec
import Player
import Tank
data WiimotePlayer = WiimotePlayer Wiimote WiimoteAccCal [(Float, Float)]
deriving (Typeable, Show)
instance Player WiimotePlayer where
playerUpdate (WiimotePlayer wiimote cal oldaims) tank = do
state <- hwiidGetState wiimote
messages <- hwiidGetMesg wiimote
let buttons = stateButtons state
shoot = any (\m -> (mesgType m == hwiidMesgTypeButton) && (test (mesgButtons m) hwiidButtonB)) $ messages
foo = any (\m -> (mesgType m == hwiidMesgTypeButton) && (test (mesgButtons m) hwiidButtonA)) $ messages
x = (if (test buttons hwiidButtonLeft) then (-1) else 0) + (if (test buttons hwiidButtonRight) then 1 else 0)
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 (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)
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
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
when foo $ print $ state
return (WiimotePlayer wiimote cal aims, moveangle, move, aangle, shoot)
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)
irSmooth = 10
irSkip = 0.2
irSkipSmooth = 4
irXScale :: Float
irXScale = 20
irXTranslate :: Float
irXTranslate = 0 + 7
irYScale :: Float
irYScale = 20
irYTranslate :: Float
irYTranslate = -10 + 8
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
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 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
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)
cal <- hwiidGetAccCal wiimote hwiidExtNone
return $ WiimotePlayer wiimote cal []
test :: (Bits a) => a -> a -> Bool
test field bits = (field .&. bits) == bits
|