--- /dev/null
+implementation module Geometry
+
+import StdEnvExt
+
+:: Metre = Metre !Real
+
+m :: !Real -> Metre // distance in metre
+m metre = Metre metre
+
+instance zero Metre where zero = Metre zero
+instance + Metre where + (Metre m1) (Metre m2) = Metre (m1 + m2)
+instance - Metre where - (Metre m1) (Metre m2) = Metre (m1 - m2)
+instance scale Metre where scale k (Metre m) = Metre (k * m)
+instance abs Metre where abs (Metre m) = Metre (abs m)
+instance sign Metre where sign (Metre m) = sign m
+instance ~ Metre where ~ (Metre m) = Metre (~m)
+instance == Metre where == (Metre m1) (Metre m2) = m1 == m2
+instance < Metre where < (Metre m1) (Metre m2) = m1 < m2
+instance toReal Metre where toReal (Metre m) = m
+instance toString Metre where toString (Metre m) = m +++> " m."
+
+
+
+:: Velocity = MetrePerSecond !Real // velocity in metre/second
+
+ms :: !Real -> Velocity
+ms v = MetrePerSecond v
+
+instance zero Velocity where zero = MetrePerSecond zero
+instance + Velocity where + (MetrePerSecond v1) (MetrePerSecond v2) = MetrePerSecond (v1 + v2)
+instance - Velocity where - (MetrePerSecond v1) (MetrePerSecond v2) = MetrePerSecond (v1 - v2)
+instance scale Velocity where scale k (MetrePerSecond v) = MetrePerSecond (k * v)
+instance abs Velocity where abs (MetrePerSecond v) = MetrePerSecond (abs v)
+instance sign Velocity where sign (MetrePerSecond v) = sign v
+instance ~ Velocity where ~ (MetrePerSecond v) = MetrePerSecond (~v)
+instance == Velocity where == (MetrePerSecond v1) (MetrePerSecond v2) = v1 == v2
+instance < Velocity where < (MetrePerSecond v1) (MetrePerSecond v2) = v1 < v2
+instance toReal Velocity where toReal (MetrePerSecond v) = v
+instance toString Velocity where toString (MetrePerSecond v) = v +++> "m/s"
+
+instance zero RVector where zero = {dx = zero, dy = zero}
+instance + RVector where + v1 v2 = {dx = v1.dx + v2.dx, dy = v1.dy + v2.dy}
+instance - RVector where - v1 v2 = {dx = v1.dx - v2.dx, dy = v1.dy - v2.dy}
+instance one RVector where one = {dx = m 1.0, dy = m 1.0}
+instance scale RVector where scale k {dx, dy} = {dx = scale k dx, dy = scale k dy}
+instance ~ RVector where ~ v = zero - v
+instance coords RVector where coords {dx,dy} = [dx,dy]
+instance zero RVector3D where zero = {dxy = zero, dz = zero}
+instance + RVector3D where + v1 v2 = {dxy = v1.dxy + v2.dxy, dz = v1.dz + v2.dz}
+instance - RVector3D where - v1 v2 = {dxy = v1.dxy - v2.dxy, dz = v1.dz - v2.dz}
+instance one RVector3D where one = {dxy = one, dz = m 1.0}
+instance scale RVector3D where scale k {dxy,dz} = {dxy = scale k dxy, dz = scale k dz}
+instance ~ RVector3D where ~ v = zero - v
+instance coords RVector3D where coords {dxy={dx,dy},dz} = [dx,dy,dz]
+
+
+:: Angle = Radian !Real
+
+pi :== 3.1415926535897932384
+
+rad :: !Real -> Angle
+rad x = Radian (normalize_radian x)
+
+degree :: !Int -> Angle
+degree x = rad (toReal x * pi / 180.0)
+
+normalize_radian :: !Real -> Real
+normalize_radian r
+| r < (~cycle) = normalize_radian (r + cycle)
+| r > cycle = normalize_radian (r - cycle)
+| otherwise = r
+where
+ cycle = 2.0 * pi
+
+instance zero Angle where zero = Radian zero
+instance + Angle where + (Radian r1) (Radian r2) = Radian (normalize_radian (r1 + r2))
+instance - Angle where - (Radian r1) (Radian r2) = Radian (normalize_radian (r1 - r2))
+instance scale Angle where scale k (Radian r) = Radian (normalize_radian (k*r))
+instance abs Angle where abs (Radian r) = Radian (abs r)
+instance sign Angle where sign (Radian r) = sign r
+instance ~ Angle where ~ (Radian r) = Radian (~r)
+instance == Angle where == (Radian r1) (Radian r2) = r1 == r2
+instance < Angle where < (Radian r1) (Radian r2) = r1 < r2
+instance toReal Angle where toReal (Radian r) = r
+instance toInt Angle where toInt (Radian r) = toInt (r * 180.0 / pi)
+instance toString Angle where toString (Radian r) = r +++> " rad"
+instance toRVector Angle where toRVector (Radian r) = {dx = Metre (cos r), dy = Metre (sin r)}
+instance sinus Angle where sinus (Radian r) = sin r
+instance cosinus Angle where cosinus (Radian r) = cos r
+instance tangens Angle where tangens (Radian r) = tan r
+instance arcsinus Angle where arcsinus x = Radian (asin x)
+instance arccosinus Angle where arccosinus x = Radian (acos x)
+instance arctangens Angle where arctangens x = Radian (atan x)
+
+instance zero Position where zero = {px = zero, py = zero}
+instance == Position where == p1 p2 = p1.px == p2.px && p1.py == p2.py
+instance coords Position where coords {px,py} = [px,py]
+instance toString Position where toString {px, py} = "{px=" +++ toString px +++ ",py=" +++ toString py +++ "}"
+instance toRVector Position where toRVector p = {dx = p.px, dy = p.py}
+instance zero Position3D where zero = {pxy = zero, pz = zero}
+instance == Position3D where == p1 p2 = p1.pxy == p2.pxy && p1.pz == p2.pz
+instance coords Position3D where coords {pxy={px,py},pz} = [px,py,pz]
+instance toString Position3D where toString {pxy,pz} = "{pxy=" +++ toString pxy +++ ",pz=" +++ toString pz +++ "}"
+
+instance toPosition (!Metre,!Metre) where toPosition (x,y) = {px=x,py=y}
+instance toPosition Position where toPosition p2D = p2D
+instance toPosition Position3D where toPosition p3D = p3D.pxy
+instance fromPosition (!Metre,!Metre) where fromPosition p2D = (p2D.px,p2D.py)
+instance fromPosition Position where fromPosition p2D = p2D
+instance fromPosition Position3D where fromPosition p2D = {zero & pxy=p2D}
+instance toPosition3D (!Metre,!Metre,!Metre) where toPosition3D (x,y,z) = {pxy=toPosition (x,y),pz=z}
+instance toPosition3D Position where toPosition3D p2D = {zero & pxy=p2D}
+instance toPosition3D Position3D where toPosition3D p3D = p3D
+instance fromPosition3D (!Metre,!Metre,!Metre) where fromPosition3D p3D = (p3D.pxy.px,p3D.pxy.py,p3D.pz)
+instance fromPosition3D Position where fromPosition3D p3D = p3D.pxy
+instance fromPosition3D Position3D where fromPosition3D p3D = p3D
+
+instance zero Speed where zero = {direction=zero,velocity=zero}
+instance == Speed where == sp1 sp2 = sp1.direction == sp2.direction && sp1.velocity == sp2.velocity
+instance toString Speed where toString {direction,velocity} = "{direction=" +++ toString direction +++ ",velocity=" +++ toString velocity +++ "}"
+instance toSpeed3D Speed where toSpeed3D s = {zero & vxy=s}
+instance zero Speed3D where zero = {vxy=zero,vz=zero}
+instance == Speed3D where == sp1 sp2 = sp1.vxy == sp2.vxy && sp1.vz == sp2.vz
+instance toString Speed3D where toString {vxy,vz} = "{vxy=" +++ toString vxy +++ ",vz=" +++ toString vz +++ "}"
+instance toSpeed Speed3D where toSpeed s = s.vxy
+instance fromSpeed Speed3D where fromSpeed s = {zero & vxy=s}
+instance fromSpeed3D Speed where fromSpeed3D s = s.vxy
+
+class coords a :: !a -> [Metre]
+
+move_point :: !RVector !Position -> Position
+move_point {dx,dy} {px,py} = {px=px+dx,py=py+dy}
+
+move_point3D :: !RVector3D !Position3D -> Position3D
+move_point3D {dxy,dz} {pxy,pz} = {pxy=move_point dxy pxy,pz=pz+dz}
+
+repell :: !Metre !Position !Position -> Position
+repell minimum_distance base pos
+| d == zero = move_point {zero & dx=minimum_distance} pos
+| d < minimum_distance = move_point v` base //move_point (v` - v) pos
+| otherwise = pos
+where
+ d = dist base pos
+ v = {dx = pos.px - base.px, dy = pos.py - base.py}
+ v` = scale ((toReal minimum_distance) / (toReal d)) v
+
+attract :: !Metre !Position !Position -> Position
+attract maximum_distance base pos
+| d > maximum_distance = move_point v` base
+| otherwise = pos
+where
+ d = dist base pos
+ v = {dx = pos.px - base.px, dy = pos.py - base.py}
+ v` = scale ((toReal maximum_distance) / (toReal d)) v
+
+between_points :: !(!Position,!Position) !Position -> Bool
+between_points (a,b) c = point_in_rectangle (a,b) c && (toReal dcx) / (toReal dcy) == (toReal dx) / (toReal dy)
+where
+ [min_x,max_x:_] = sort [a.px,b.px]
+ [min_y,max_y:_] = sort [a.py,b.py]
+ (dx, dy) = (a.px - b.px, a.py - b.py)
+ (dcx,dcy) = (a.px - c.px, a.py - c.py)
+
+point_in_rectangle :: !(!Position,!Position) !Position -> Bool
+point_in_rectangle (a,b) c = isbetween c.px min_x max_x && isbetween c.py min_y max_y
+where
+ (min_x,max_x) = minmax (a.px,b.px)
+ (min_y,max_y) = minmax (a.py,b.py)
+
+point_to_rectangle :: !(!Position,!Position) !Position -> Position
+point_to_rectangle (a,b) c
+| point_in_rectangle (a,b) c = c
+| otherwise = toPosition c`
+where
+ (min_x,max_x) = minmax (a.px,b.px)
+ (min_y,max_y) = minmax (a.py,b.py)
+ left = c.px <= min_x
+ right = c.px >= max_x
+ above = c.py >= max_y
+ below = c.py <= min_y
+
+ c` | left && above = (min_x,max_y)
+ | right && above = (max_x,max_y)
+ | left && below = (min_x,min_y)
+ | right && below = (max_x,min_y)
+ | above = (c.px, max_y)
+ | below = (c.px, min_y)
+ | left = (min_x,c.py )
+ | right = (max_x,c.py )
+ | otherwise = abort ("unsuspected error; please rotate with angles between pi and -pi\n")
+
+size_vector :: !RVector -> Metre
+size_vector v = Metre (dist` v z)
+where z :: RVector; z = zero
+
+size_vector3D :: !RVector3D -> Metre
+size_vector3D v = Metre (dist` v z)
+where z :: RVector3D; z = zero
+
+dist :: !a !b -> Metre | toPosition3D a & toPosition3D b
+dist a b = Metre (dist` (toPosition3D a) (toPosition3D b))
+
+dist` :: !a !b -> Real | coords a & coords b
+dist` cs1 cs2 = sqrt (sum [ (toReal c1 - toReal c2)^2.0 \\ c1 <- coords cs1 & c2 <- coords cs2 ])
+
+orthogonal :: !Angle -> (!Angle,!Angle)
+orthogonal a = (a + rad (0.25*pi), a - rad (0.25*pi))
+
+px_bearing :: !base !target -> Angle | toPosition base & toPosition target
+px_bearing base target
+| v.dx >= zero && v.dy >= zero = Radian base_angle // 1st quadrant
+| v.dx <= zero && v.dy >= zero = Radian (pi-base_angle) // 2nd quadrant
+| v.dx <= zero && v.dy <= zero = Radian (base_angle-pi) // 3rd quadrant
+| v.dx >= zero && v.dy <= zero = Radian (~base_angle) // 4th quadrant
+where
+ pbase = toPosition base
+ ptarget = toPosition target
+ v = {dx = ptarget.px - pbase.px, dy = ptarget.py - pbase.py}
+ d = toReal (dist pbase ptarget)
+ base_angle = acos ((toReal (abs v.dx)) / d)
+
+bearing :: !Angle !base !target -> Angle | toPosition base & toPosition target
+bearing angle base target = px_bearing base target - angle
repositories / fp1415-soccerfun.git / blobdiff