src/Game/Geometry.icl

   1 implementation module Geometry
   2
   3 import StdEnvExt
   4
   5 :: Metre                                =   Metre !Real
   6
   7 m                                               :: !Real -> Metre               // distance in metre
   8 m metre                                 = Metre metre
   9
  10 instance zero           Metre                  where zero                          = Metre zero
  11 instance +              Metre                  where + (Metre m1)  (Metre m2)      = Metre (m1 + m2)
  12 instance -              Metre                  where - (Metre m1)  (Metre m2)      = Metre (m1 - m2)
  13 instance scale          Metre                  where scale k       (Metre m)       = Metre (k * m)
  14 instance abs            Metre                  where abs           (Metre m)       = Metre (abs m)
  15 instance sign           Metre                  where sign          (Metre m)       = sign m
  16 instance ~              Metre                  where ~             (Metre m)       = Metre (~m)
  17 instance ==             Metre                  where == (Metre m1) (Metre m2)      = m1 == m2
  18 instance <              Metre                  where <  (Metre m1) (Metre m2)      = m1 <  m2
  19 instance toReal         Metre                  where toReal        (Metre m)       = m
  20 instance toString       Metre                  where toString      (Metre m)       = m +++> " m."
  21
  22
  23
  24 ::      Velocity                        = MetrePerSecond !Real  // velocity in metre/second
  25
  26 ms                                              :: !Real -> Velocity
  27 ms v                                    = MetrePerSecond v
  28
  29 instance zero           Velocity               where zero                                       = MetrePerSecond zero
  30 instance +              Velocity               where + (MetrePerSecond v1)  (MetrePerSecond v2) = MetrePerSecond (v1 + v2)
  31 instance -              Velocity               where - (MetrePerSecond v1)  (MetrePerSecond v2) = MetrePerSecond (v1 - v2)
  32 instance scale          Velocity               where scale k                (MetrePerSecond v)  = MetrePerSecond (k * v)
  33 instance abs            Velocity               where abs                    (MetrePerSecond v)  = MetrePerSecond (abs v)
  34 instance sign           Velocity               where sign                   (MetrePerSecond v)  = sign v
  35 instance ~              Velocity               where ~                      (MetrePerSecond v)  = MetrePerSecond (~v)
  36 instance ==             Velocity               where == (MetrePerSecond v1) (MetrePerSecond v2) = v1 == v2
  37 instance <              Velocity               where <  (MetrePerSecond v1) (MetrePerSecond v2) = v1 <  v2
  38 instance toReal         Velocity               where toReal                 (MetrePerSecond v)  = v
  39 instance toString       Velocity               where toString               (MetrePerSecond v)  = v +++> "m/s"
  40
  41 instance zero           RVector                where zero                          = {dx = zero,             dy = zero}
  42 instance +              RVector                where + v1 v2                       = {dx = v1.dx + v2.dx,    dy = v1.dy + v2.dy}
  43 instance -              RVector                where - v1 v2                       = {dx = v1.dx - v2.dx,    dy = v1.dy - v2.dy}
  44 instance one            RVector                where one                           = {dx = m 1.0,            dy = m 1.0}
  45 instance scale          RVector                where scale k {dx, dy}              = {dx = scale k dx,       dy = scale k dy}
  46 instance ~              RVector                where ~ v                           = zero - v
  47 instance coords         RVector                where coords {dx,dy}                = [dx,dy]
  48 instance zero           RVector3D              where zero                          = {dxy = zero,            dz = zero}
  49 instance +              RVector3D              where + v1 v2                       = {dxy = v1.dxy + v2.dxy, dz = v1.dz + v2.dz}
  50 instance -              RVector3D              where - v1 v2                       = {dxy = v1.dxy - v2.dxy, dz = v1.dz - v2.dz}
  51 instance one            RVector3D              where one                           = {dxy = one,             dz = m 1.0}
  52 instance scale          RVector3D              where scale k {dxy,dz}              = {dxy = scale k dxy,     dz = scale k dz}
  53 instance ~              RVector3D              where ~ v                           = zero - v
  54 instance coords         RVector3D              where coords {dxy={dx,dy},dz}       = [dx,dy,dz]
  55
  56
  57 ::      Angle                           = Radian !Real
  58
  59 pi                                              :== 3.1415926535897932384
  60
  61 rad                                             :: !Real -> Angle
  62 rad x                                   = Radian (normalize_radian x)
  63
  64 degree                                  :: !Int -> Angle
  65 degree x                                = rad (toReal x * pi / 180.0)
  66
  67 normalize_radian                :: !Real -> Real
  68 normalize_radian r
  69 | r < (~cycle)                  = normalize_radian (r + cycle)
  70 | r >   cycle                   = normalize_radian (r - cycle)
  71 | otherwise                             = r
  72 where
  73         cycle                           = 2.0 * pi
  74
  75 instance zero           Angle                  where zero                          = Radian zero
  76 instance +              Angle                  where + (Radian r1)  (Radian r2)    = Radian (normalize_radian (r1 + r2))
  77 instance -              Angle                  where - (Radian r1)  (Radian r2)    = Radian (normalize_radian (r1 - r2))
  78 instance scale          Angle                  where scale k        (Radian r)     = Radian (normalize_radian (k*r))
  79 instance abs            Angle                  where abs            (Radian r)     = Radian (abs r)
  80 instance sign           Angle                  where sign           (Radian r)     = sign r
  81 instance ~              Angle                  where ~              (Radian r)     = Radian (~r)
  82 instance ==             Angle                  where == (Radian r1) (Radian r2)    = r1 == r2
  83 instance <              Angle                  where <  (Radian r1) (Radian r2)    = r1 <  r2
  84 instance toReal         Angle                  where toReal         (Radian r)     = r
  85 instance toInt          Angle                  where toInt          (Radian r)     = toInt (r * 180.0 / pi)
  86 instance toString       Angle                  where toString       (Radian r)     = r +++> " rad"
  87 instance toRVector      Angle                  where toRVector      (Radian r)     = {dx = Metre (cos r), dy = Metre (sin r)}
  88 instance sinus          Angle                  where sinus          (Radian r)     = sin r
  89 instance cosinus        Angle                  where cosinus        (Radian r)     = cos r
  90 instance tangens        Angle                  where tangens        (Radian r)     = tan r
  91 instance arcsinus       Angle                  where arcsinus       x              = Radian (asin x)
  92 instance arccosinus     Angle                  where arccosinus     x              = Radian (acos x)
  93 instance arctangens     Angle                  where arctangens     x              = Radian (atan x)
  94
  95 instance zero           Position               where zero                          = {px  = zero, py = zero}
  96 instance ==             Position               where == p1  p2                     = p1.px == p2.px && p1.py == p2.py
  97 instance coords         Position               where coords {px,py}                = [px,py]
  98 instance toString       Position               where toString {px, py}                     = "{px=" +++ toString px +++ ",py=" +++ toString py +++ "}"
  99 instance toRVector      Position               where toRVector p                   = {dx  = p.px, dy = p.py}
 100 instance zero           Position3D             where zero                          = {pxy = zero, pz = zero}
 101 instance ==             Position3D             where == p1  p2                     = p1.pxy == p2.pxy && p1.pz == p2.pz
 102 instance coords         Position3D             where coords {pxy={px,py},pz}       = [px,py,pz]
 103 instance toString       Position3D             where toString {pxy,pz}                     = "{pxy=" +++ toString pxy +++ ",pz=" +++ toString pz +++ "}"
 104
 105 instance toPosition     (!Metre,!Metre)        where toPosition     (x,y)          = {px=x,py=y}
 106 instance toPosition     Position               where toPosition     p2D            = p2D
 107 instance toPosition     Position3D             where toPosition     p3D            = p3D.pxy
 108 instance fromPosition   (!Metre,!Metre)        where fromPosition   p2D            = (p2D.px,p2D.py)
 109 instance fromPosition   Position               where fromPosition   p2D            = p2D
 110 instance fromPosition   Position3D             where fromPosition   p2D            = {zero & pxy=p2D}
 111 instance toPosition3D   (!Metre,!Metre,!Metre) where toPosition3D   (x,y,z)        = {pxy=toPosition (x,y),pz=z}
 112 instance toPosition3D   Position               where toPosition3D   p2D            = {zero & pxy=p2D}
 113 instance toPosition3D   Position3D             where toPosition3D   p3D            = p3D
 114 instance fromPosition3D (!Metre,!Metre,!Metre) where fromPosition3D p3D            = (p3D.pxy.px,p3D.pxy.py,p3D.pz)
 115 instance fromPosition3D Position               where fromPosition3D p3D            = p3D.pxy
 116 instance fromPosition3D Position3D             where fromPosition3D p3D            = p3D
 117
 118 instance zero           Speed                  where zero                          = {direction=zero,velocity=zero}
 119 instance ==             Speed                  where == sp1 sp2                    = sp1.direction == sp2.direction && sp1.velocity == sp2.velocity
 120 instance toString       Speed                  where toString {direction,velocity} = "{direction=" +++ toString direction +++ ",velocity=" +++ toString velocity +++ "}"
 121 instance toSpeed3D      Speed                  where toSpeed3D   s                 = {zero & vxy=s}
 122 instance zero           Speed3D                where zero                          = {vxy=zero,vz=zero}
 123 instance ==             Speed3D                where == sp1 sp2                    = sp1.vxy == sp2.vxy && sp1.vz == sp2.vz
 124 instance toString       Speed3D                where toString {vxy,vz}                     = "{vxy=" +++ toString vxy +++ ",vz=" +++ toString vz +++ "}"
 125 instance toSpeed        Speed3D                where toSpeed     s                 = s.vxy
 126 instance fromSpeed      Speed3D                where fromSpeed   s                 = {zero & vxy=s}
 127 instance fromSpeed3D    Speed                  where fromSpeed3D s                 = s.vxy
 128
 129 class    coords a                               :: !a -> [Metre]
 130
 131 move_point                                              :: !RVector !Position -> Position
 132 move_point {dx,dy} {px,py}              = {px=px+dx,py=py+dy}
 133
 134 move_point3D                                    :: !RVector3D !Position3D -> Position3D
 135 move_point3D {dxy,dz} {pxy,pz}  = {pxy=move_point dxy pxy,pz=pz+dz}
 136
 137 repell                                                  :: !Metre !Position !Position -> Position
 138 repell minimum_distance base pos
 139 | d == zero                                             = move_point {zero & dx=minimum_distance} pos
 140 | d < minimum_distance                  = move_point v` base //move_point (v` - v) pos
 141 | otherwise                                             = pos
 142 where
 143         d                                                       = dist base pos
 144         v                                                       = {dx = pos.px - base.px, dy = pos.py - base.py}
 145         v`                                                      = scale ((toReal minimum_distance) / (toReal d)) v
 146
 147 attract                                                 :: !Metre !Position !Position -> Position
 148 attract maximum_distance base pos
 149 | d > maximum_distance                  = move_point v` base
 150 | otherwise                                             = pos
 151 where
 152         d                                                       = dist base pos
 153         v                                                       = {dx = pos.px - base.px, dy = pos.py - base.py}
 154         v`                                                      = scale ((toReal maximum_distance) / (toReal d)) v
 155
 156 between_points                                  :: !(!Position,!Position) !Position -> Bool
 157 between_points (a,b) c                  = point_in_rectangle (a,b) c && (toReal dcx) / (toReal dcy) == (toReal dx) / (toReal dy)
 158 where
 159         [min_x,max_x:_]                         = sort [a.px,b.px]
 160         [min_y,max_y:_]                         = sort [a.py,b.py]
 161         (dx, dy)                                        = (a.px - b.px, a.py - b.py)
 162         (dcx,dcy)                                       = (a.px - c.px, a.py - c.py)
 163
 164 point_in_rectangle                              :: !(!Position,!Position) !Position -> Bool
 165 point_in_rectangle (a,b) c              = isbetween c.px min_x max_x && isbetween c.py min_y max_y
 166 where
 167         (min_x,max_x)                           = minmax (a.px,b.px)
 168         (min_y,max_y)                           = minmax (a.py,b.py)
 169
 170 point_to_rectangle                              :: !(!Position,!Position) !Position -> Position
 171 point_to_rectangle   (a,b) c
 172 | point_in_rectangle (a,b) c    = c
 173 | otherwise                                             = toPosition c`
 174 where
 175         (min_x,max_x)                           = minmax (a.px,b.px)
 176         (min_y,max_y)                           = minmax (a.py,b.py)
 177         left                                            = c.px <= min_x
 178         right                                           = c.px >= max_x
 179         above                                           = c.py >= max_y
 180         below                                           = c.py <= min_y
 181
 182         c` | left  && above                     = (min_x,max_y)
 183            | right && above                     = (max_x,max_y)
 184            | left  && below                     = (min_x,min_y)
 185            | right && below             = (max_x,min_y)
 186            | above                                      = (c.px, max_y)
 187            | below                                      = (c.px, min_y)
 188            | left                                       = (min_x,c.py )
 189            | right                                      = (max_x,c.py )
 190            | otherwise                          = abort ("unsuspected error; please rotate with angles between pi and -pi\n")
 191
 192 size_vector                                             :: !RVector -> Metre
 193 size_vector v                                   = Metre (dist` v z)
 194 where z :: RVector; z = zero
 195
 196 size_vector3D                                   :: !RVector3D -> Metre
 197 size_vector3D v                                 = Metre (dist` v z)
 198 where z :: RVector3D; z = zero
 199
 200 dist                                                    :: !a !b -> Metre | toPosition3D a & toPosition3D b
 201 dist a b                                                = Metre (dist` (toPosition3D a) (toPosition3D b))
 202
 203 dist`                                                   :: !a !b -> Real | coords a & coords b
 204 dist` cs1 cs2                                   = sqrt (sum [ (toReal c1 - toReal c2)^2.0 \\ c1 <- coords cs1 & c2 <- coords cs2 ])
 205
 206 orthogonal                                              :: !Angle -> (!Angle,!Angle)
 207 orthogonal a                                    = (a + rad (0.25*pi), a - rad (0.25*pi))
 208
 209 px_bearing                                              :: !base !target -> Angle | toPosition base & toPosition target
 210 px_bearing base target
 211 | v.dx >= zero && v.dy >= zero  = Radian base_angle                     // 1st quadrant
 212 | v.dx <= zero && v.dy >= zero  = Radian (pi-base_angle)        // 2nd quadrant
 213 | v.dx <= zero && v.dy <= zero  = Radian (base_angle-pi)        // 3rd quadrant
 214 | v.dx >= zero && v.dy <= zero  = Radian (~base_angle)          // 4th quadrant
 215 where
 216         pbase                                           = toPosition base
 217         ptarget                                         = toPosition target
 218         v                                                       = {dx = ptarget.px - pbase.px, dy = ptarget.py - pbase.py}
 219         d                                                       = toReal (dist pbase ptarget)
 220         base_angle                                      = acos ((toReal (abs v.dx)) / d)
 221
 222 bearing                                                 :: !Angle !base !target -> Angle | toPosition base & toPosition target
 223 bearing angle base target               = px_bearing base target - angle