\section{Joints} Joints are a basic essential element of the RSAGL inverse kinematics subsystem. \begin{code} 
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.Orthagonal
import RSAGL.Types
\end{code} \texttt{Joint} is the result of computing a joint. It provides AffineTransformations that describe the orientations of the bases of the components of the joint. \texttt{joint\_arm\_upper} is the affine transformation to the position of the upper arm, where the origin is the shoulder (or base). \texttt{joint\_arm\_lower} is the affine transformation to the lower arm, where the origin is the elbow. \texttt{joint\_arm\_hand} is the affine transformation where the origin is the hand. \texttt{joint\_shoulder}, \texttt{joint\_hand}, and \texttt{joint\_elbow} refer to the positions of those endpoints of the joint. \begin{code} 
data Joint = Joint { joint_shoulder :: Point3D,
                     joint_hand :: Point3D,
                     joint_elbow :: Point3D,
                     joint_arm_lower :: AffineTransformation,
                     joint_arm_upper :: AffineTransformation,
		     joint_arm_hand :: AffineTransformation }
\end{code} Compute a joint where given a bend vector, two end points, and the total length of them limb. \begin{code} 
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
              (orthagonalFrame (forward $ vectorToFrom hand shoulder) (down bend)) (Vector3D 0 (-1) 0)
          elbow = translate joint_offset_vector $ lerp 0.5 (shoulder,hand)
\end{code}