
  0 | module Data.NumIdr.Transform.Rigid
  1 | 
  2 | import Data.Vect
  3 | import Data.NumIdr.Interfaces
  4 | import Data.NumIdr.Array
  5 | import Data.NumIdr.Vector
  6 | import Data.NumIdr.Matrix
  7 | import Data.NumIdr.Homogeneous
  8 | import Data.NumIdr.Transform.Point
  9 | import Data.NumIdr.Transform.Transform
 10 | import Data.NumIdr.Transform.Rotation
 11 | 
 12 | %default total
 13 | 
 14 | 
 15 | ||| A rigid transform encodes a (proper) rigid transformation, also known as a
 16 | ||| rototranslation.
 17 | public export
 18 | Rigid : Nat -> Type -> Type
 19 | Rigid = Transform TRigid
 20 | 
 21 | -- TODO: Add Rigid constructors
 22 | 
 23 | ||| Determine if a homogeneous matrix represents a rigid transform.
 24 | export
 25 | isRigid : FieldCmp a => HMatrix' n a -> Bool
 26 | isRigid mat = isHMatrix mat && isRotation' (getMatrix mat)
 27 | 
 28 | export
 29 | fromHMatrix : FieldCmp a => HMatrix' n a -> Maybe (Rigid n a)
 30 | fromHMatrix mat = if isRigid mat then Just (unsafeMkTrans mat) else Nothing
 31 | 
 32 | 
 33 | export
 34 | rigid2D : Vector 2 Double -> Double -> Rigid 2 Double
 35 | rigid2D v a = setTranslation v (rotate2D a)
 36 | 
 37 | 
 38 | ||| Construct a 3D rigid transform representing an observer standing at `orig`
 39 | ||| and facing towards `target`.
 40 | |||
 41 | ||| @ orig The origin point, i.e. the point that the origin is mapped to.
 42 | ||| @ target The point that the z-axis is directed towards.
 43 | ||| @ up The vertical direction, the direction that the y-axis faces.
 44 | export
 45 | faceTowards : (orig, target : Point 3 Double) -> (up : Vector 3 Double)
 46 |                 -> Rigid 3 Double
 47 | faceTowards orig target up = setTranslation (toVector orig)
 48 |                                             (faceTowards (orig -. target) up)
 49 |