module RSAGL.Animation.Joint
    (Joint(..),
     joint) where

import RSAGL.Math.Vector
import RSAGL.Math.Affine
import RSAGL.Math.Interpolation
import RSAGL.Scene.CoordinateSystems
import RSAGL.Math.Orthogonal
import RSAGL.Math.Types

-- | The result of computing a joint.  It provides AffineTransformations that
-- describe the orientations of the components of the joint.
-- All affine transformations reorient the +Z axis to aim in the direction
-- of the far point.  For example, in @joint_arm_lower@ the +Z axis aims
-- at the position of the hand.
data Joint = Joint { joint_shoulder :: Point3D,
                     -- ^ The fixed point of the joint.
                     joint_hand :: Point3D,
                     -- ^ The far end point of the joint.
                     joint_elbow :: Point3D,
                     -- ^ The articulated point of the joint.
                     joint_arm_lower :: AffineTransformation,
                     -- ^ The affine transformation to the lower
                     -- arm, where the origin is the elbow.
                     joint_arm_upper :: AffineTransformation,
                     -- ^ The affine transformation to the upper
                     -- arm, where the origin is the shoulder.
                     joint_arm_hand :: AffineTransformation
                     -- ^ The affine transformation where the origin
                     -- is the hand.  Oriented to preserve as much as
                     -- possible the +Y axis.
                     }

-- | Compute a joint where given a bend vector, describing the direction
-- in which the articulated point (elbow) will try to move when the
-- arm is retracted, and shoulder or base of the joint, the total
-- length of the joint, and ideal position of the hand.
joint :: Vector3D -> Point3D -> RSdouble -> Point3D -> Joint
joint bend shoulder joint_length hand | distanceBetween shoulder hand > joint_length = -- if the end is out of range, constrict it to within range
    joint bend shoulder joint_length (translate (vectorScaleTo (0.99 * joint_length) $ vectorToFrom hand shoulder) shoulder)
joint bend shoulder joint_length hand = Joint {
        joint_shoulder = shoulder,
        joint_hand = hand,
        joint_elbow = elbow,
        joint_arm_lower = modelLookAt elbow (forward $ Left hand) (down $ Right bend),
        joint_arm_upper = modelLookAt shoulder (forward $ Left elbow) (down $ Right bend),
        joint_arm_hand = modelLookAt hand (backward $ Left elbow) (up $ Right (Vector3D 0 1 0)) }
    where joint_offset = sqrt (joint_length^2 - (distanceBetween shoulder hand)^2) / 2
          joint_offset_vector = vectorScaleTo joint_offset $ transformation
              (orthogonalFrame (forward $ vectorToFrom hand shoulder) (down bend)) (Vector3D 0 (-1) 0)
          elbow = translate joint_offset_vector $ lerp 0.5 (shoulder,hand)