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
data Joint = Joint { joint_shoulder :: Point3D,
joint_hand :: Point3D,
joint_elbow :: Point3D,
joint_arm_lower :: AffineTransformation,
joint_arm_upper :: AffineTransformation,
joint_arm_hand :: AffineTransformation
}
joint :: Vector3D -> Point3D -> RSdouble -> Point3D -> Joint
joint bend shoulder joint_length hand | distanceBetween shoulder hand > joint_length =
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)