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              = normalize r 100
  69 where
  70     normalize r 0               = abort "Loop in normalize_radian. Mail p.achten@cs.ru.nl"
  71         normalize r n
  72         | r < ~pi                       = normalize (r + 2.0*pi) (n-1)
  73         | r >  pi                       = normalize (r - 2.0*pi) (n-1)
  74         | otherwise                     = r
  75
  76
  77 instance zero           Angle                  where zero                          = Radian zero
  78 instance +              Angle                  where + (Radian r1)  (Radian r2)    = Radian (normalize_radian (r1 + r2))
  79 instance -              Angle                  where - (Radian r1)  (Radian r2)    = Radian (normalize_radian (r1 - r2))
  80 instance scale          Angle                  where scale k        (Radian r)     = Radian (normalize_radian (k*r))
  81 instance abs            Angle                  where abs            (Radian r)     = Radian (abs r)
  82 instance sign           Angle                  where sign           (Radian r)     = sign r
  83 instance ~              Angle                  where ~              (Radian r)     = Radian (~r)
  84 instance ==             Angle                  where == (Radian r1) (Radian r2)    = r1 == r2
  85 instance <              Angle                  where <  (Radian r1) (Radian r2)    = r1 <  r2
  86 instance toReal         Angle                  where toReal         (Radian r)     = r
  87 instance toInt          Angle                  where toInt          (Radian r)     = toInt (r * 180.0 / pi)
  88 instance toString       Angle                  where toString       (Radian r)     = r +++> " rad"
  89 instance toRVector      Angle                  where toRVector      (Radian r)     = {dx = Metre (cos r), dy = Metre (sin r)}
  90 instance sinus          Angle                  where sinus          (Radian r)     = sin r
  91 instance cosinus        Angle                  where cosinus        (Radian r)     = cos r
  92 instance tangens        Angle                  where tangens        (Radian r)     = tan r
  93 instance arcsinus       Angle                  where arcsinus       x              = Radian (asin x)
  94 instance arccosinus     Angle                  where arccosinus     x              = Radian (acos x)
  95 instance arctangens     Angle                  where arctangens     x              = Radian (atan x)
  96
  97 instance zero           Position               where zero                          = {px  = zero, py = zero}
  98 instance ==             Position               where == p1  p2                     = p1.px == p2.px && p1.py == p2.py
  99 instance coords         Position               where coords {px,py}                = [px,py]
 100 instance toString       Position               where toString {px, py}                     = "{px=" +++ toString px +++ ",py=" +++ toString py +++ "}"
 101 instance toRVector      Position               where toRVector p                   = {dx  = p.px, dy = p.py}
 102 instance zero           Position3D             where zero                          = {pxy = zero, pz = zero}
 103 instance ==             Position3D             where == p1  p2                     = p1.pxy == p2.pxy && p1.pz == p2.pz
 104 instance coords         Position3D             where coords {pxy={px,py},pz}       = [px,py,pz]
 105 instance toString       Position3D             where toString {pxy,pz}                     = "{pxy=" +++ toString pxy +++ ",pz=" +++ toString pz +++ "}"
 106
 107 instance toPosition     (!Metre,!Metre)        where toPosition     (x,y)          = {px=x,py=y}
 108 instance toPosition     Position               where toPosition     p2D            = p2D
 109 instance toPosition     Position3D             where toPosition     p3D            = p3D.pxy
 110 instance fromPosition   (!Metre,!Metre)        where fromPosition   p2D            = (p2D.px,p2D.py)
 111 instance fromPosition   Position               where fromPosition   p2D            = p2D
 112 instance fromPosition   Position3D             where fromPosition   p2D            = {zero & pxy=p2D}
 113 instance toPosition3D   (!Metre,!Metre,!Metre) where toPosition3D   (x,y,z)        = {pxy=toPosition (x,y),pz=z}
 114 instance toPosition3D   Position               where toPosition3D   p2D            = {zero & pxy=p2D}
 115 instance toPosition3D   Position3D             where toPosition3D   p3D            = p3D
 116 instance fromPosition3D (!Metre,!Metre,!Metre) where fromPosition3D p3D            = (p3D.pxy.px,p3D.pxy.py,p3D.pz)
 117 instance fromPosition3D Position               where fromPosition3D p3D            = p3D.pxy
 118 instance fromPosition3D Position3D             where fromPosition3D p3D            = p3D
 119
 120 instance zero           Speed                  where zero                          = {direction=zero,velocity=zero}
 121 instance ==             Speed                  where == sp1 sp2                    = sp1.direction == sp2.direction && sp1.velocity == sp2.velocity
 122 instance toString       Speed                  where toString {direction,velocity} = "{direction=" +++ toString direction +++ ",velocity=" +++ toString velocity +++ "}"
 123 instance toSpeed3D      Speed                  where toSpeed3D   s                 = {zero & vxy=s}
 124 instance zero           Speed3D                where zero                          = {vxy=zero,vz=zero}
 125 instance ==             Speed3D                where == sp1 sp2                    = sp1.vxy == sp2.vxy && sp1.vz == sp2.vz
 126 instance toString       Speed3D                where toString {vxy,vz}                     = "{vxy=" +++ toString vxy +++ ",vz=" +++ toString vz +++ "}"
 127 instance toSpeed        Speed3D                where toSpeed     s                 = s.vxy
 128 instance fromSpeed      Speed3D                where fromSpeed   s                 = {zero & vxy=s}
 129 instance fromSpeed3D    Speed                  where fromSpeed3D s                 = s.vxy
 130
 131 class    coords a                               :: !a -> [Metre]
 132
 133 move_point                                              :: !RVector !Position -> Position
 134 move_point {dx,dy} {px,py}              = {px=px+dx,py=py+dy}
 135
 136 move_point3D                                    :: !RVector3D !Position3D -> Position3D
 137 move_point3D {dxy,dz} {pxy,pz}  = {pxy=move_point dxy pxy,pz=pz+dz}
 138
 139 repell                                                  :: !Metre !Position !Position -> Position
 140 repell minimum_distance base pos
 141 | d == zero                                             = move_point {zero & dx=minimum_distance} pos
 142 | d < minimum_distance                  = move_point v` base //move_point (v` - v) pos
 143 | otherwise                                             = pos
 144 where
 145         d                                                       = dist base pos
 146         v                                                       = {dx = pos.px - base.px, dy = pos.py - base.py}
 147         v`                                                      = scale ((toReal minimum_distance) / (toReal d)) v
 148
 149 attract                                                 :: !Metre !Position !Position -> Position
 150 attract maximum_distance base pos
 151 | d > maximum_distance                  = move_point v` base
 152 | otherwise                                             = pos
 153 where
 154         d                                                       = dist base pos
 155         v                                                       = {dx = pos.px - base.px, dy = pos.py - base.py}
 156         v`                                                      = scale ((toReal maximum_distance) / (toReal d)) v
 157
 158 between_points                                  :: !(!Position,!Position) !Position -> Bool
 159 between_points (a,b) c                  = point_in_rectangle (a,b) c && (toReal dcx) / (toReal dcy) == (toReal dx) / (toReal dy)
 160 where
 161         [min_x,max_x:_]                         = sort [a.px,b.px]
 162         [min_y,max_y:_]                         = sort [a.py,b.py]
 163         (dx, dy)                                        = (a.px - b.px, a.py - b.py)
 164         (dcx,dcy)                                       = (a.px - c.px, a.py - c.py)
 165
 166 point_in_rectangle                              :: !(!Position,!Position) !Position -> Bool
 167 point_in_rectangle (a,b) c              = isbetween c.px min_x max_x && isbetween c.py min_y max_y
 168 where
 169         (min_x,max_x)                           = minmax (a.px,b.px)
 170         (min_y,max_y)                           = minmax (a.py,b.py)
 171
 172 point_to_rectangle                              :: !(!Position,!Position) !Position -> Position
 173 point_to_rectangle   (a,b) c
 174 | point_in_rectangle (a,b) c    = c
 175 | otherwise                                             = toPosition c`
 176 where
 177         (min_x,max_x)                           = minmax (a.px,b.px)
 178         (min_y,max_y)                           = minmax (a.py,b.py)
 179         left                                            = c.px <= min_x
 180         right                                           = c.px >= max_x
 181         above                                           = c.py >= max_y
 182         below                                           = c.py <= min_y
 183
 184         c` | left  && above                     = (min_x,max_y)
 185            | right && above                     = (max_x,max_y)
 186            | left  && below                     = (min_x,min_y)
 187            | right && below             = (max_x,min_y)
 188            | above                                      = (c.px, max_y)
 189            | below                                      = (c.px, min_y)
 190            | left                                       = (min_x,c.py )
 191            | right                                      = (max_x,c.py )
 192            | otherwise                          = abort ("unsuspected error; please rotate with angles between pi and -pi\n")
 193
 194 size_vector                                             :: !RVector -> Metre
 195 size_vector v                                   = Metre (dist` v z)
 196 where z :: RVector; z = zero
 197
 198 size_vector3D                                   :: !RVector3D -> Metre
 199 size_vector3D v                                 = Metre (dist` v z)
 200 where z :: RVector3D; z = zero
 201
 202 dist                                                    :: !a !b -> Metre | toPosition3D a & toPosition3D b
 203 dist a b                                                = Metre (dist` (toPosition3D a) (toPosition3D b))
 204
 205 dist`                                                   :: !a !b -> Real | coords a & coords b
 206 dist` cs1 cs2                                   = sqrt (sum [ (toReal c1 - toReal c2)^2.0 \\ c1 <- coords cs1 & c2 <- coords cs2 ])
 207
 208 orthogonal                                              :: !Angle -> (!Angle,!Angle)
 209 orthogonal a                                    = (a + rad (0.25*pi), a - rad (0.25*pi))
 210
 211 px_bearing                                              :: !base !target -> Angle | toPosition base & toPosition target
 212 px_bearing base target
 213 | abs ratio > 1.0                               = Radian zero                           // Corner-cases: d==0 and "large values of 1.0"
 214 | v.dx >= zero && v.dy >= zero  = Radian base_angle                     // 1st quadrant
 215 | v.dx <= zero && v.dy >= zero  = Radian (pi-base_angle)        // 2nd quadrant
 216 | v.dx <= zero && v.dy <= zero  = Radian (base_angle-pi)        // 3rd quadrant
 217 | v.dx >= zero && v.dy <= zero  = Radian (~base_angle)          // 4th quadrant
 218 where
 219         pbase                                           = toPosition base
 220         ptarget                                         = toPosition target
 221         v                                                       = {dx = ptarget.px - pbase.px, dy = ptarget.py - pbase.py}
 222         d                                                       = toReal (dist pbase ptarget)
 223     ratio                                               = toReal (abs v.dx) / d
 224         base_angle                                      = acos ratio
 225
 226 bearing                                                 :: !Angle !base !target -> Angle | toPosition base & toPosition target
 227 bearing angle base target               = px_bearing base target - angle