summaryrefslogtreecommitdiffstats
path: root/src/HWiidPlayer.hs
blob: db3587116ff2027b4fcbc700a9555ed0981f1868 (plain)
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
149
{-# LANGUAGE DeriveDataTypeable #-}

module HWiidPlayer ( HWiidPlayer
                   , newHWiidPlayer
                   ) where

import Control.Monad
import Data.Bits hiding (rotate)
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, rotate)
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.2 :: GLfloat)
                                                  rotate 30 $ Vector3 1 0 (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