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 HWiidPlayer ( HWiidPlayer
, newHWiidPlayer
) 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 HWiidPlayer = HWiidPlayer Wiimote WiimoteAccCal [(Float, Float)]
deriving (Typeable, Show)
instance Player HWiidPlayer where
playerUpdate (HWiidPlayer wiimote cal oldaims) tank = do
state <- getState wiimote
messages <- getMesg wiimote
let buttons = stateButtons state
shoot = any (\m -> (mesgType m == mesgTypeButton) && (test (mesgButtons m) buttonB)) $ messages
foo = any (\m -> (mesgType m == mesgTypeButton) && (test (mesgButtons m) buttonA)) $ messages
x = (if (test buttons buttonLeft) then (-1) else 0) + (if (test buttons buttonRight) then 1 else 0)
y = (if (test buttons buttonDown) then (-1) else 0) + (if (test buttons buttonUp) then 1 else 0)
ext = stateExt state
(mx, my) = if (extType ext) /= extNunchuk
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 (HWiidPlayer wiimote cal aims, moveangle, move, aangle, shoot)
renderPlayer (HWiidPlayer _ _ []) = return ()
renderPlayer (HWiidPlayer _ _ 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 irMaxX)/2
hMaxY = (fromIntegral irMaxY)/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
newHWiidPlayer :: IO HWiidPlayer
newHWiidPlayer = do
wiimote <- open bdAddrAny (flagMesgInterface .|. flagNonblock)
when (wiimote == nullWiimote) $ fail "Wiimote error"
setReportMode wiimote (reportButtons .|. reportAcc .|. reportIR .|. reportNunchuk)
cal <- getAccCal wiimote extNone
return $ HWiidPlayer wiimote cal []
test :: (Bits a) => a -> a -> Bool
test field bits = (field .&. bits) == bits
|
