```\section{Joints}

Joints are a basic essential element of the RSAGL inverse kinematics subsystem.

\begin{code}
module RSAGL.Joint
(Joint(..),
joint) where

import RSAGL.Vector
import RSAGL.Affine
import RSAGL.Interpolation
import RSAGL.CoordinateSystems
import RSAGL.Orthagonal
\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 -> Double -> 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}
```