module SoccerFun.Geometry where
import Prelude.Unicode
import SoccerFun.Prelude
import Data.List (sort)
type Metre = Float
type Length = Metre
type XPos = Metre
type YPos = Metre
type ZPos = Metre
data Position = Position
{px ∷ XPos,
py ∷ YPos
} deriving (Show,Eq)
data Position3D = Position3D {pxy ∷ Position, pz ∷ ZPos} deriving (Show,Eq)
coordinates ∷ Position3D → (XPos,YPos,ZPos)
coordinates pos = (px $ pxy pos, py $ pxy pos, pz pos)
type XRadius = Metre
type YRadius = Metre
type ZRadius = Metre
inRadiusOfPosition ∷ Position3D → XRadius → YRadius → ZRadius → Position → Bool
inRadiusOfPosition pos1 xr yr zr pos2 = let
(x1,y1,z1) = coordinates pos1
Position x2 y2 = pos2
in x2+xr >= x1 && x1 >= x2xr && y2+yr >= y1 && y1 >= y2yr && z1 <= zr
type Angle = Radian
type Radian = Float
angleHowFarFromPi ∷ Angle → Angle
angleHowFarFromPi a = toRadian (if (a' > 180) then (360 a') else a')
where
a' = fromRadian (abs a)
angleHowFarFromAngle ∷ Angle → Angle → Angle
angleHowFarFromAngle a b
| a' > b' = if a' b' > 180 then toRadian (b' a' + 360) else toRadian (a' b')
| otherwise = if b' a' > 180 then toRadian (a' b' + 360) else toRadian (b' a')
where
a' = fromRadian (abs a)
b' = fromRadian (abs b)
movePoint ∷ RVector → Position → Position
movePoint (RVector dx dy) (Position px py) = Position (px+dx) (py+dy)
movePoint3D ∷ RVector3D → Position3D → Position3D
movePoint3D (RVector3D dxy dz) (Position3D pxy pz) = Position3D (movePoint dxy pxy) (pz+dz)
pointToRectangle ∷ (Position,Position) → Position → Position
pointToRectangle (a,b) c
| pointInRectangle (a,b) c = c
| otherwise = let (x,y) = c' in Position x y
where
(minX,maxX) = minmax ((px a),(px b))
(minY,maxY) = minmax ((py a),(py b))
left = (px c) <= minX
right = (px c) >= maxX
above = (py c) <= minY
below = (py c) >= maxY
c' | left && above = (minX,minY)
| right && above = (maxX,minY)
| left && below = (minX,maxY)
| right && below = (maxX,maxY)
| above = ((px c), minY)
| below = ((px c), maxY)
| left = (minX,(py c) )
| right = (maxX,(py c) )
| otherwise = error ("unsuspected error; please rotate with angles between pi and -pi\n")
pointInRectangle ∷ (Position,Position) → Position → Bool
pointInRectangle (a,b) c = minX ≤ px c ∧ px c ≤ maxX ∧ minY ≤ py c ∧ py c ≤ maxY where
(minX,maxX) = minmax ((px a),(px b))
(minY,maxY) = minmax ((py a),(py b))
inCircleRadiusOfPosition ∷ Position3D → XRadius → ZRadius → Position → Bool
inCircleRadiusOfPosition (Position3D pxy pz) r zr pos
= dist pxy pos <= r && pz <= zr
data RVector = RVector
{dx ∷ Metre,
dy ∷ Metre
} deriving (Show,Eq)
data RVector3D = RVector3D {dxy ∷ RVector, dz ∷ Metre} deriving (Show,Eq)
data Speed = Speed
{direction ∷ Angle,
velocity ∷ Velocity
} deriving (Show,Eq)
data Speed3D = Speed3D
{vxy ∷ Speed,
vz ∷ Velocity
} deriving (Show,Eq)
type Velocity = Float
class ToSpeed a where toSpeed ∷ a → Speed
class FromSpeed a where fromSpeed ∷ Speed → a
class ToSpeed3D a where toSpeed3D ∷ a → Speed3D
class FromSpeed3D a where fromSpeed3D ∷ Speed3D → a
instance Num RVector3D where
p1 + p2 = RVector3D {dxy = dxy p1 + dxy p2, dz = dz p1 + dz p2}
p1 * p2 = RVector3D {dxy = dxy p1 * dxy p2, dz = dz p1 * dz p2}
p1 p2 = RVector3D {dxy = dxy p1 dxy p2, dz = dz p1 dz p2}
abs p = RVector3D {dxy = abs $ dxy p, dz = abs $ dz p}
signum p = RVector3D {dxy = signum $ dxy p, dz = signum $ dz p}
fromInteger i = RVector3D {dxy = fromInteger i, dz = 0}
instance Num Speed3D where
p1 + p2 = Speed3D {vxy = vxy p1 + vxy p2, vz = vz p1 + vz p2}
p1 * p2 = Speed3D {vxy = vxy p1 * vxy p2, vz = vz p1 * vz p2}
p1 p2 = Speed3D {vxy = vxy p1 vxy p2, vz = vz p1 vz p2}
abs p = Speed3D {vxy = abs $ vxy p, vz = abs $ vz p}
signum p = Speed3D {vxy = signum $ vxy p, vz = signum $ vz p}
fromInteger i = Speed3D {vxy = fromInteger i, vz = 0}
instance Num Position3D where
p1 + p2 = Position3D {pxy = pxy p1 + pxy p2, pz = pz p1 + pz p2}
p1 * p2 = Position3D {pxy = pxy p1 * pxy p2, pz = pz p1 * pz p2}
p1 p2 = Position3D {pxy = pxy p1 pxy p2, pz = pz p1 pz p2}
abs p = Position3D {pxy = abs $ pxy p, pz = abs $ pz p}
signum p = Position3D {pxy = signum $ pxy p, pz = signum $ pz p}
fromInteger i = Position3D {pxy = fromInteger i, pz = 0}
instance Num RVector where
p1 + p2 = RVector {dx = dx p1 + dx p2, dy = dy p1 + dy p2}
p1 * p2 = RVector {dx = dx p1 * dx p2, dy = dy p1 * dy p2}
p1 p2 = RVector {dx = dx p1 dx p2, dy = dy p1 dy p2}
abs p = RVector {dx = abs $ dx p, dy = abs $ dy p}
signum p = RVector {dx = signum $ dx p, dy = signum $ dy p}
fromInteger i = RVector {dx = fromInteger i, dy = 0}
instance Num Position where
p1 + p2 = Position {px = px p1 + px p2, py = py p1 + py p2}
p1 * p2 = Position {px = px p1 * px p2, py = py p1 * py p2}
p1 p2 = Position {px = px p1 px p2, py = py p1 py p2}
abs p = Position {px = abs $ px p, py = abs $ py p}
signum p = Position {px = signum $ px p, py = signum $ py p}
fromInteger i = Position {px = fromInteger i, py = 0}
instance Num Speed where
p1 + p2 = Speed {direction = direction p1 + direction p2, velocity = velocity p1 + velocity p2}
p1 * p2 = Speed {direction = direction p1 * direction p2, velocity = velocity p1 * velocity p2}
p1 p2 = Speed {direction = direction p1 direction p2, velocity = velocity p1 velocity p2}
abs p = Speed {direction = abs $ direction p, velocity = abs $ velocity p}
signum p = Speed {direction = signum $ direction p, velocity = signum $ velocity p}
fromInteger i = Speed {direction = fromInteger i, velocity = 0}
class ToRVector a where toRVector ∷ a → RVector
class ToPosition a where toPosition ∷ a → Position
class FromPosition a where fromPosition ∷ Position → a
class ToPosition3D a where toPosition3D ∷ a → Position3D
class FromPosition3D a where fromPosition3D ∷ Position3D → a
type Degrees = Int
scaleVector ∷ Float → RVector → RVector
scaleVector k (RVector dx dy) = RVector (k*dx) (k*dy)
scaleVector3D ∷ Float → RVector3D → RVector3D
scaleVector3D k (RVector3D dxy dz) = RVector3D (scaleVector k dxy) (k*dz)
fromRadian ∷ Radian → Degrees
fromRadian a = (round (a * (360.0 / (2.0*pi)))) `mod` 360
toRadian ∷ Degrees → Radian
toRadian a = (fromIntegral a) * pi / 180.0
betweenPoints ∷ (Position,Position) → Position → Bool
betweenPoints (a,b) c
= pointInRectangle (a,b) c && dcx / dcy == dx / dy
where
(minX:maxX:_) = sort [(px a),(px b)]
(minY:maxY:_) = sort [(py a),(py b)]
(dx, dy) = ((px a) (px b), (py a) (py b))
(dcx,dcy) = ((px a) (px c), (py a) (py c))
instance ToRVector Float where toRVector angle = RVector {dx=cos angle,dy=sin angle}
instance ToRVector Position where toRVector p = RVector {dx=(px p),dy=(py p)}
sizeVector ∷ RVector → Float
sizeVector (RVector dx dy) = sqrt (dx*82.0 + dy**2.0)
sizeVector3D ∷ RVector3D → Float
sizeVector3D (RVector3D dxy dz) = sqrt ((dx dxy)**2.0 + (dy dxy)**2.0 + dz**2.0)
class Dist a b where dist ∷ a → b → Float
instance Dist Float Float where dist a b = abs (ab)
instance Dist Position Position where dist a b = sqrt $ (abs $ px a px b)**2.0 + (abs $ py a py b)**2.0
instance Dist Position3D Position3D where dist a b = sqrt ((abs (px (pxy a)px(pxy b)))**2.0 + (abs (py (pxy a)py (pxy b)))**2.0 + (abs ((pz a)(pz b)))**2.0)
instance Dist Position Position3D where dist a b = dist (Position3D a 0) b
instance Dist Position3D Position where dist a b = dist a (Position3D b 0)
orthogonal ∷ Angle → (Angle,Angle)
orthogonal a = (a+pi/4.0, api/4.0)
instance ToPosition (Float,Float) where toPosition (x,y) = Position {px=x,py=y}
instance ToPosition Position3D where toPosition p3D = pxy p3D
instance FromPosition (Float,Float) where fromPosition p2D = (px p2D, py p2D)
instance FromPosition Position3D where fromPosition p2D = 0 {pxy=p2D}
instance ToPosition3D (Float,Float,Float) where toPosition3D (x,y,z) = Position3D {pxy=toPosition (x,y),pz=z}
instance ToPosition3D Position where toPosition3D p2D = 0 {pxy=p2D}
instance FromPosition3D (Float,Float,Float) where fromPosition3D p3D = (px $ pxy p3D, py $ pxy p3D, pz p3D)
instance FromPosition3D Position where fromPosition3D p3D = pxy p3D
instance ToSpeed Speed3D where toSpeed s = (vxy s)
instance FromSpeed Speed3D where fromSpeed s = 0 {vxy=s}
instance ToSpeed3D Speed where toSpeed3D s = 0 {vxy=s}
instance FromSpeed3D Speed where fromSpeed3D s = (vxy s)
oppositeAngle ∷ Angle → Angle
oppositeAngle a
| newAngle < (pi) = newAngle + 2.0*pi
| otherwise = newAngle
where
newAngle = a pi
angleWithObject ∷ Position → Position → Angle
angleWithObject base target
| a >= (0.5*pi) && b >= 0 = ((pi)+a)
| a <= (0.5*pi) && b >= 0 = (pi)+b
| a <= (0.5*pi) && b <= 0 = pi+b
| a >= (0.5*pi) && b <= 0 = (b)
where
v = RVector (px basepx target) (py basepy target)
d = dist base target
a = acos (max1 ((dx v) / d))
b = asin (max1 ((dy v) / d))
max1 ∷ Float → Float
max1 r
| r < 1.0 = 1.0
| r > 1.0 = 1.0
| otherwise = r
angleWithObjectForRun ∷ (Position,Angle) → Position → Angle
angleWithObjectForRun (base,angle) target
| newAngle > pi = newAngle 2.0*pi
| newAngle < (pi) = newAngle + 2.0*pi
| otherwise = newAngle
where
newAngle = angleWithObject base target angle