#! /usr/bin/python3


# This module provides functions to convert from Vicon marker
# coordinates to the geometric parameters of an articulated "matchstick"
# human figure that approximates the essential skeleton geometry of the
# human body. It computes the positions of the centers of the main
# joints and the orientations of the main skeletal parts, relative to
# the scene coordinate system.

# The input data to these functions are {X,Y,Z} coordinates (in mm) of
# reflective markers that were affixed at specific points of the
# subject's skin, next to identifiable points of the skeleton:

#   [00] coudeD     Outer knob of right elbow.
#   [01] poignetD   Knob of right wrist opposite to thumb.
#   [02] mibrasD    Middle of right forearm.
#   [03] doigtD     Tip of right middle finger.
#   [04] epauleD    Top knob of right shoulder blade.
#   [05] nuque      Base of neck, T1 vertebra; called "D1" by Vicon.
#   [06] epauleG    Top knob of left shoulder blade.
#   [07] coudeG     Outer knob of left elbow
#   [08] poignetG   Knob of left wrist opposite to thumb.
#   [09] doigtG     Tip of left middle finger.
#   [10] hancheD    Top-front corner of hip bone.
#   [11] genouD     Outer point on left knee bending axis.
#   [12] malD       Outer knob of right ankle.
#   [13] piedD      Top of right foot over base of 5th metatarsal.
#   [14] hancheG    Top front left knob of pelvis.
#   [15] genouG     Outer point on left knee bending axis.
#   [16] malG       Outer knob of left ankle.
#   [17] piedG      Top of left foot over base of 5th metatarsal.
#   [18] oeilD      Side of head, next to the eye.
#   [19] oreilleG   Side of head, next to the ear.

# HUMAN FIGURE MODEL

# The figure is conceptually defined as a /hierarchical articulate
# model/, which consists of a certain number {N} of /parts/ connected by
# /joints/ in a tree-like fashion. Each part is identified by a /name/
# string. The current model has {N=19} parts:
#
#   [00] "spine"  Approximate backbone below the neck.
#   [01] "head"   Head.
#   [02] "neck"   Part of the bachbone between torso and head.
#   [03] "scap0"  Left shoulder blade and associated structures.
#   [04] "uarm0"  Left upper arm.
#   [05] "farm0"  Left forearm.
#   [06] "hand0"  Left hand minus fingers and thumb.
#   [07] "pelv0"  Left half of the pelvis.
#   [08] "thigh0" Left thigh.
#   [09] "shin0"  Left lower leg.
#   [10] "foot0"  Left foot.
#   [11] "scap1"  Right shoulder blade and associated structures.
#   [12] "uarm1"  Right upper arm.
#   [13] "farm1"  Right forearm.
#   [14] "hand1"  Right hand minus fingers and thumb.
#   [15] "pelv1"  Right half of the pelvis.
#   [16] "thigh1" Right thigh.
#   [17] "shin1"  Right lower leg.
#   [18] "foot1"  Right foot.
#
# The suffix "0" means left, "1" means right.

# Each part also has a /parent/ that is the proximal body part to which
# it is attached, or "none". The parent links define a tree structure,
# whose root part (the "spine") is the only one that has "none" as
# parent.

# Thus, for example, the "hand0" (left hand) part has parent "farm0"
# (left lower arm), which in turn has parent "uarm0" (left upper arm),
# which in turn has parent "scap0" (left shoulder bone), which in turn
# has parent "spine" (the backbone or dorsal spine); which, being the root part,
# has parent "none".

# OUTPUT DATA FORMAT

# The complete description of the matchstick figure is a tuple of {N}
# /part placement/ specs. Each part placement spec is a tuple consisting
# of the part's name, the name of the parent part, a /nominal size/ {L}
# in mm, and four coordinate triplets: a /local origin/ {O} and three /local
# axis vectors/ comprising an orthonormal basis. The origin
# coordinates are in mm, while the axis vector coordinates are
# dimensionless.

# The origin point and the axis direction vectors comprise the part's
# /local coordinate system/. For the "spine" part the origin of the
# local system is the base of the spine, assumed to be the midpoint of
# the line that connects the two hip joints. For all other parts, the
# origin of the local system is the center of the joint that connects
# that part to its parent part.

# The axes {X,Y,Z} of the local system of each part are oriented in
# specific directions relative to it. The three axes are orthogonal. For
# example, in the local coordinate system of the "head", the {X} axis
# points in the direction from left ear to right ear, the (Y} axis points
# in the direction from occiput to nose, and the {Z} axis points up.

# The relative orientation of the {X}, {Y}, and {Z} axes is right-handed
# for all unique parts ("head", "neck", "spine") and for the right-side
# member of paired parts (such as "scap1", "hand1", "shin1"); it is
# left-handed for the left-side members (such as "scap0", "hand0",
# "shin0"). In fact the left and right members of each pair are mirror
# images of each other, including their local coordinate systems.

# The shape of each part is not specified by this model. For
# visualization, the parts may be drawn as solid objects with suitable
# degree of detail and realism. However, it is implicitly assumed that
# the shape can be determined from its placement data, and possibly from
# the placement data of the parent and children parts.

# In most cases, a part represents one major bone or rigid set of bones,
# the part origins are the centers of articulations, and the local
# axes are fixed with respect to that bone. Therefore the origin of a
# part {P}, relative to the coordinate system of the parent part {R},
# should be constant for each individual, whatever the pose and motion.
# For example, the origin for the "head" part, which is attached to
# the top of the "neck", should be {(0,0,Ln)} in the "neck"'s local
# coordinate system; where {Ln} is the length of the subject's neck.
# However, due to data errors in the capture, geometric approximations
# made in the conversion, and flexing of multi-bone structures such as
# the forearm, neck, and torso those parameters may vary slightly from
# frame to frame.

# DESCRIPTION OF THE PARTS

# The following table describes the visible shape used for each part {P},
# the origin and axes of its local coordinate system, the name of its
# parent part {R}, and where {P} is attached to {R}.

#   "spine" [00] (parent "none"): represents the spine (vertebral
#   column) minus the neck part, from the pelvis to the just above the
#   T1 vertebra. Its origin {O_spine} is the the midpoint of the centes
#   of the two hip joints. Note that this point will be a bit forward of
#   the lower end of the real spine (the sacrum). The local {Z_spine}
#   axis points from {O_spine} towards the center {O-neck} of the
#   spine-neck joint  (see below). The local {X_spine} axis lies in the plane through
#   {O_spine}, {O_neck}, and the centers of the hip joints, and is
#   directed to the right. The local {Y_spine} axis is directed towards
#   the front of the body. The nominal size {L_spine} is the distance
#   between {O_spine} and {O_neck}.

#   "neck" [01] (parent "spine"): represents the neck part of the
#   vertebral colum. The local origin {O_neck} is just above the lumen
#   of the T1 vertebra, and should be {O_spine + L_spine*Z_spine}. The
#   local axis {Z_neck} axis points from {O_neck} towards the center
#   {O_head} of the skull-neck articulation. The local {X_neck} axis
#   lies on the plance that contains those two points and is parallel
#   to the line between the centers of the two shoulder joints,
#   directed towards the subject's right side.   The local
#   {Y_neck} axis is directed towards the subject's front side.
#   Its nominal size {L_neck} is the distance from {O_neck} to
#   {O_head}.

#   "head" [02] (parent "neck"): represents the skull. The local origin
#   {O_head} is at the center of the neck-skull articulation, just below
#   the foramen; which should be {O_neck + L_neck*Z_neck}. The {X_head}
#   axis vector is parallel to the left ear to right ear vectors, the
#   {Y_head} axis vector is approximately parallel to the occiput to
#   nose vector, and the {Z_head} axis vector is approximately parallel
#   to the chin to forehead vector. Its nominal size {L_head} is the
#   approximate radius of the head, measured from the center.

#   "scap1" [03] (parent "spine"): represents the right shoulder
#   structure, including the right scapula (shoulder blade), the right
#   clavicle, the 2-3 upper ribs, and the muscles and ligaments that
#   connect them to the backbone. Its origin {O_scap1} coincides with
#   {O_neck}. Its nominal size {L_scap1} is the distance between
#   {O_scap1} and the center {O_uarm1} of the joint between the right
#   upper arm and the shoulder blade. The {X_scap1} axis points from
#   {O_scap1} to {O_uarm1}. The {Y_scap1} axis is perpendicular to
#   {Z_spine} and is directed towards the subject's front. The {Z_scap1}
#   is directed upwards.

#   "uarm1" [04] (parent "scap1"): represents the right upper arm. Its
#   origin {O_uarm1} is the center of the joint between the upper arm
#   and the scapula, and should be {O_scap1 + L_scap1*X_scap1}. The
#   {X_uarm1} axis points along the upper arm, from {O_uarm1} to the
#   center {O_farm1} of the right elbow articulation. The nominal size
#   {L_uarm1} is the distance between those points. The {Z_uarm1} axis
#   is parallel to the bending axis of the elbow joint, directed
#   outwards when the upper arm is lowered. The {Y_uarm1} axis, given by
#   the right-hand rule, is in the general direction of the forearm when
#   the elbow is bent.

#   "farm1" [05] (parent "uarm1"): represents the forearm. Its origin
#   {O_farm1} is the center of the elbow joint, which should be {O_uarm1
#   +L_uarm1*X_uarm1}. The {X_farm1} axis points along the forearm, from
#   {O_farm1} to the center {O_hand1} of the wrist joint. Its nominal
#   size {L_farm1} is the distance between those two points. The
#   {Z_farm1} axis coincides with {Z_uarm1}, namely parallel to the
#   bending axis of the elbow joint. The {Y_farm1} axis, given by the
#   right-hand rule, points in the orthogonal direction that is in the
#   general direction of the upper arm, when the elbow is bent.

#   "hand1" [06] (parent "farm1"): represents the right hand minus the
#   thumb and fingers. Its origin {O_hand1} is the center of the wrist
#   joint, which should be {O_farm1 + L_farm1*X_farm1}. The {X_hand1}
#   axis points from {O_hand1} to the point {T_hand1} midway between the
#   centers of the basal articulations of the middle and ring fingers.
#   The nominal size {L_hand1} is the distance between those two points.
#   The {Y_hand1} axis points perpendicular to the average plane of the
#   fingerless hand, directed towards the palm side. The {Z_hand1} axis
#   is directed towards the thumb side.

#   "pelv1" [07] (parent "spine"): represents the right half of the
#   pelvis. Its origin {O_pelv1} coincides with {O_spine}, by
#   definition. The {X_pelv1} axis points from {O_pelv1} to the center
#   {O_thigh1} of the right hip joint. The nominal size {L_pelv1} is the
#   distance between those two points. The {Y_pelv1} axis is
#   perpendicular to {Z_spine} and is directed forwards. The {Z_pelv1}
#   axis is directed upwards.

#   "thigh1" [08] (parent "pelv1"): represents the right thigh.
#   Its origin {O_thigh1} is the center of the hip joint,
#   which should be {O_pelv1 + L_pelv1*X_pelv1}.
#   The {X_thigh1} axis points along the
#   thigh, from {O_thigh1} to the center {O_shin1} of the knee joint.
#   Its size {L_thigh1} is the distance between those two points. The
#   {Z_thigh1} axis is parallel to the bending
#   axis of the knee, directed in the right (outwards) direction.
#   The {Y_thigh1} axis, given by the right-hand rule, points roughly forwards,
#   generally away from the lower leg when the knee is bent

#   "shin1 [09] (parent "thigh1"): represents the right lower leg. Its
#   origin {O_shin1} is the center of the knee joint, which should
#   be{O_thigh1 + L_thigh1*X_thigh1}. The {X_shin1} axis points along the
#   lower leg, from {O_shin1} to the center {O_foot1} of the ankle
#   joint. The {Z_shin1} axis coincides with the {Z_thigh1} axis, namely
#   parallel to the bending axis of the knee, directed outwards. The
#   {Y_shin1} axis, given by the right-hand rule, is directed forwards,
#   generally away from the thigh when the knee is bent.

#   "foot1" [10] (parent "shin1"): representing the skeleton of the
#   right foot below the ankle, minus the toes.  Its origin {O_foot1}
#   is the center of the ankle joint, and should be {O_shin1 + L_shin1*X_shin1}.
#   The {X_foot1} and {Y_foot1} axes are parallel to the plane of the sole
#   of the foot.  The {Y_foot1} axis points along the midline of the long axis
#   of the sole, from the center {H_foot1} of the heel just below the ankle
#   to the midpoint {T_foot1} of the centers of the basal articulations
#   of the "index" and "middle" toes.  The foot's nominal size is
#   the distance between those two points.  The {X_foot1} axis is directed outwards.
#   The {Z_foot1} axis is directed up.

# The left-side parts "scap0", "uarm0", "hand0", "shin0", etc. (indices
# [11] to [18]) are mirror images of the right-side parts; including
# their local coordinate systems.

import sys, math, re, rn, rmxn

def compute_placements(mk):
  # Given the list of marker coordinates {mk[0..M-1]}, computes and
  # returns the list {rp[0..N-1]} of absolute part placements (relative
  # to the scene coordinate system).

  M = 20 # Expected number of markers.
  assert len(mk) == M

  # Give names to the markers, as used by Vicon software:
  mk_coudeD    = mk[ 0] # Outer knob of right elbow.
  mk_poignetD  = mk[ 1] # Knob of right wrist opposite to thumb.
  mk_mibrasD   = mk[ 2] # Middle of right forearm.
  mk_doigtD    = mk[ 3] # Tip of right middle finger.
  mk_epauleD   = mk[ 4] # Top knob of right shoulder blade.
  mk_nuque     = mk[ 5] # Base of neck, T1 vertebra; called "D1" by Vicon.
  mk_epauleG   = mk[ 6] # Top knob of left shoulder blade.
  mk_coudeG    = mk[ 7] # Outer knob of left elbow
  mk_poignetG  = mk[ 8] # Knob of left wrist opposite to thumb.
  mk_doigtG    = mk[ 9] # Tip of left middle finger.
  mk_hancheD   = mk[10] # Top-front corner of hip bone.
  mk_genouD    = mk[11] # Outer point on left knee bending axis.
  mk_malD      = mk[12] # Outer knob of right ankle.
  mk_piedD     = mk[13] # Top of right foot over base of 5th metatarsal.
  mk_hancheG   = mk[14] # Top front left knob of pelvis.
  mk_genouG    = mk[15] # Outer point on left knee bending axis.
  mk_malG      = mk[16] # Outer knob of left ankle.
  mk_piedG     = mk[17] # Top of left foot over base of 5th metatarsal.
  mk_oeilD     = mk[18] # Side of head, next to the eye.
  mk_oreilleG  = mk[19] # Side of head, next to the ear.

  N = 19 # Number of figure model parts.
  rp = [None] * N

  # Compute placement of middle (unpaired) parts:
  mk_mid = (mk_oeilD, mk_oreilleG, mk_nuque, mk_epauleG, mk_epauleD, mk_hancheG, mk_hancheD)
  rp_mid = compute_placements_mid(mk_mid)
  rp_spine, rp_neck, rp_head = rp_mid

  O_spine = rp_spine[3]
  Z_spine = rp_spine[6]
  O_neck = rp_neck[3]

  # Compute placement of left arm parts:
  mk_arm0 = (mk_epauleG, mk_coudeG, mk_poignetG, mk_doigtG)
  rp_arm0 = compute_placements_arm(O_neck, Z_spine, mk_arm0, 0)
  rp_scap0, rp_uarm0, rp_farm0, rp_hand0 = rp_arm0

  # Compute placement of right arm parts:
  mk_arm1 = (mk_epauleD, mk_coudeD, mk_poignetD, mk_doigtD)
  rp_arm1 = compute_placements_arm(O_neck, Z_spine, mk_arm1, 1)
  rp_scap1, rp_uarm1, rp_farm1, rp_hand1 = rp_arm1

  # Compute placement of left leg parts:
  mk_leg0 = (mk_hancheG, mk_genouG, mk_malG, mk_piedG)
  rp_leg0 = compute_placements_leg(O_spine, Z_spine, mk_leg0, 0)
  rp_pelv0, rp_thigh0, rp_shin0, rp_foot0 = rp_leg0

  # Compute placement of right leg parts:
  mk_leg1 = (mk_hancheD, mk_genouD, mk_malD, mk_piedD)
  rp_leg1 = compute_placements_leg(O_spine, Z_spine, mk_leg1, 1)
  rp_pelv1, rp_thigh1, rp_shin1, rp_foot1 = rp_leg1

  # Pack the placements:
  rp[ 0] = rp_spine
  rp[ 1] = rp_head
  rp[ 2] = rp_neck
  rp[ 3] = rp_scap0
  rp[ 4] = rp_uarm0
  rp[ 5] = rp_farm0
  rp[ 6] = rp_hand0
  rp[ 7] = rp_pelv0
  rp[ 8] = rp_thigh0
  rp[ 9] = rp_shin0
  rp[10] = rp_foot0
  rp[11] = rp_scap1
  rp[12] = rp_uarm1
  rp[13] = rp_farm1
  rp[14] = rp_hand1
  rp[15] = rp_pelv1
  rp[16] = rp_thigh1
  rp[17] = rp_shin1
  rp[18] = rp_foot1

  return rp
  #---------------------------------------------------------------------

def compute_placements_mid(mk_mid):
  # Computes the placements of unpaired parts
  # from the relevant markers.
  #
  # The input data is a tuple {mk_mid} of coordinate triplets for the
  # markers "oeilD", "oreilleG", "nuque", "epauleG", "epauleD",
  # "hancheG", and "hancheD", relative to the scene coordinate system,
  # in that order.
  #
  # The output is a tuple with the placements of parts "spine", "neck",
  # and "head", relative to the scene system, in that order.

  # Unpack:
  mk_oeilD, mk_oreilleG, mk_nuque, mk_epauleG, mk_epauleD, mk_hancheG, mk_hancheD = mk_mid

  # --- Auxiliary pelvis coordinate system {Op,Xp,Yp,Zp} ---

  # Define {Op} as the midpoint of the two hipbone markers:
  Op = rn.mix(0.5, mk_hancheG, 0.5, mk_hancheD)

  # Compute a unit vector {Xp} parallel to the line between hip joints:
  Xp, junk = rn.dir(rn.sub(mk_hancheD, mk_hancheG))

  # Compute a unit vector {Zp0} very roughly perpendicular to {Xp}, approx towards head:
  Zp0, junk = rn.dir(rn.sub(mk_nuque, Op))

  # Compute two perp vectors {Yp,Zp} perp to {Xp} on saggital pelvis plane:
  Yp, junk = rn.dir(rn.cross2(Zp0, Xp)) # Approx forward at hips rel to spine.
  Zp, junk = rn.dir(rn.cross2(Xp, Yp))  # Approx up at hips rel to spine.

  # The "spine" origin is the midpoint of hip joints,
  # assumed to be slightly behind and below the midpoint
  # of the hip markers:
  O_spine = rn.mix(-100.0, Zp, 1.0, rn.mix(-50.0, Yp, 1.0, Op))

  # --- Auxiliary shoulder coordinate system {Oe,Xe,Ye,Ze} ---

  # Compute the midpoint {Oe} between the two shoulder-top markers:
  Oe = rn.mix(0.5, mk_epauleG, 0.5, mk_epauleD)

  # Compute a vector {Xe} parallel to the line between shoulder joints:
  Xe, junk = rn.dir(rn.sub(mk_epauleD, mk_epauleG))

  # Compute  two perp vectors {Ye,Ze} perp to {Xe} on saggital shoulder plane:
  Ye, junk = rn.dir(rn.cross2(Zp, Xe)) # Approx forward at neck base rel to spine
  Ze, junk = rn.dir(rn.cross2(Xe, Ye)) # Approx up at shoulders rel to spine.

  # Estimate the center {O_neck} of the spine-neck joint (top end of "spine" part):
  O_neck = rn.mix(30.0, Ye, 1.0, mk_nuque) # Guess: 30 mm forward of nape marker.

  # Compute size of "spine" part:
  L_spine = rn.dist(O_neck, O_spine)

  # --- Local "spine" coordinate system ---

  # Compute the axial directions of the "spine" part:
  Z_spine, junk = rn.dir(rn.sub(O_neck, O_spine))
  Y_spine, junk = rn.dir(rn.cross2(Z_spine, Xp))
  X_spine, junk = rn.dir(rn.cross2(Y_spine, Z_spine))

  # --- Auxiliary head coordinate system ---

  # Compute the the ear-eye midpoint {Oh0}:
  Oh0 = rn.mix(0.5, mk_oeilD, 0.5, mk_oreilleG)

  # Compute the direction vector {Xh0} from left ear to right eye:
  Xh0, junk0 = rn.dir(rn.sub(mk_oeilD, mk_oreilleG))

  # Complete {Xh0} to an orthonormal basis {Xh0,Yh0,Zh0}:
  Zh1, junk = rn.dir(rn.sub(Oh0, O_neck)) # A vector very roughly perpendicular to {Xh0}.
  Yh0, junk = rn.dir(rn.cross2(Zh1, Xh0))
  Zh0, junk = rn.dir(rn.cross2(Xh0, Yh0))

  # Estimate the average radius {R_head} of the head:
  R_head = 0.50*(rn.dist(mk_oreilleG, mk_oeilD) - 40.0)

  # Estimate the distance {Dhn} from the eye-ear line to the neck-head joint:
  Dhn = 0.35*(rn.dist(mk_oreilleG, mk_oeilD) - 40.0)

  # Assume that the center {O_head} of the neck-head joint is somewhere
  # on the circle on the {Yh0,Zh0} plane with center {Oh0}
  # and radius {Dhn}.  Place it on that circle, in the point
  # nearest to the neck base {O_neck}.
  Vnh0 = rn.sub(O_neck, Oh0)
  xn0 = rn.dot(Xh0, Vnh0) # Coordinate of {O_neck} on {Oh0,Xh0} axis.
  yn0 = rn.dot(Yh0, Vnh0) # Coordinate of {O_neck} on {Oh0,Yh0} axis.
  rn0 = math.hypot(xn0, yn0)
  cn0 = xn0/rn0 # Cosine of argument.
  sn0 = yn0/rn0 # Sine of argument.
  O_head = rn.mix3(Dhn*cn0, Xh0, Dhn*sn0, Yh0, 1.0, Oh0)

  # Estimate the axis {Z_head} from head-neck joint to eye-ear midpoint:
  Z_head, junk = rn.dir(rn.sub(Oh0, O_head))

  # Estimate the other head axes {X_head,Y_head}:
  Xh1 = Xh0; # Close to rightwards.
  Yh1, junk = rn.dir(rn.cross2(Z_head, Xh1)) # Close to forward
  X_head, junk = rn.dir(rn.mix(1.0, Xh1, 0.1, Yh1))
  Y_head, junk = rn.dir(rn.cross2(Z_head, X_head))

  # Estimate the center {C_head} of the head:
  C_head = rn.mix(0.75*R_head, Z_head, 1.0, rn.mix(0.25*R_head, Y_head, 1.0, O_head))

  # Nominal size of "neck":
  L_neck = rn.dist(O_head, O_neck)

  # --- Local "neck" coordinate system ---

  # {Z_neck} axis, from center of spine-neck joint to center of neck-head joint:
  Z_neck, junk = rn.dir(rn.sub(O_head, O_neck))

  # Neck {X,Y} axes {X_neck,Y_neck}, forward rel to shoulders and head:
  Yn1, junk = rn.dir(rn.add(Ye, Y_head)) # Approx forward.
  X_neck, junk = rn.dir(rn.cross2(Yn1, Z_neck))
  Y_neck, junk = rn.dir(rn.cross2(Z_neck, X_neck))

  # Package the placements:
  rp_spine = ("spine", "none",  L_spine, O_spine, X_spine, Y_spine, Z_spine)
  rp_neck =  ("neck",  "spine", L_neck,  O_neck, X_neck, Y_neck, Z_neck)
  rp_head =  ("head",  "neck",  R_head,  O_head, X_head, Y_head, Z_head)

  return (rp_spine, rp_neck, rp_head)

def compute_placements_arm(O_neck, Z_spine, mk_arm, side):
  # Computes the placements of parts of an arm, on the
  # side of the body specified by {side} (0=left, 1=right).
  #
  # The main input data is a tuple {mk_arm} of coordinate triplets for the
  # markers "epaule{X}", "coude{X}", "poignet{X}", "doigt{X}", relative
  # to the scene coordinate system, in that order; where {X} is either
  # "G" (left) or "D" (right).
  #
  # The procedure also needs the coordinates of the center {O_neck} of
  # the shoulder-neck articulation and the unit vector {Z_spine} of the
  # approximate axis of the backbone.
  #
  # The output is a tuple with the placements of parts "scap{S}", "uarm{S}",
  # "farm{S}" and "hand{S}", relative to the scene system, in that order;
  # where {S} is the value of {side} as one digit.
  #
  # The {side} parameter affects the relative orientation of the axis
  # vectors of the local coordinate systems: namely, right-handed
  # if 1, left-handed if 0.

  sgn = 2*side - 1; # {+1} for right, {-1} for left.

  # Unpack:
  mk_epaule, mk_coude, mk_poignet, mk_doigt = mk_arm

  # --- Auxiliary shoulder coordinate system {Oe,Xe,Ze} ---
  Oe = O_neck
  Xe, junk = rn.dir(rn.sub(mk_epaule, Oe))   # Rough {X_scap}.
  Ye, junk = rn.dir(rn.cross2(Z_spine, Xe))  # Rough {sgn*Y_scap}.
  Ze, junk = rn.dir(rn.cross2(Xe, Ye))       # Rough {Z_scap}.

  # Estimated position of center of joint between shoulder and upper arm
  O_uarm = rn.mix(-30.0, Ze, 1.0, mk_epaule)

  # --- Shoulder size and coordinate system ---
  O_scap = O_neck
  L_scap = rn.dist(O_uarm, O_scap)
  X_scap, junk = rn.dir(rn.sub(O_uarm, O_scap))
  Y_scap, junk = rn.dir(rn.scale(sgn, rn.cross2(Z_spine, X_scap)))
  Z_scap, junk = rn.dir(rn.scale(sgn, rn.cross2(X_scap, Y_scap)))

  # --- Auxiliary upper arm coordinate system ---
  Ou = O_uarm
  Xu, junk = rn.dir(rn.sub(mk_coude, Ou))
  Yu1, junk = rn.dir(rn.sub(mk_poignet, mk_coude)) # Rough {X_farm} and {Y_uarm}.
  Zu, junk = rn.dir(rn.scale(sgn, rn.cross2(Xu, Yu1))) # Rough {Z_uarm}
  Yu, junk = rn.dir(rn.scale(sgn, rn.cross2(Zu, Xu))) # Better {Y_uarm}.

  # Estimated center of elbow joint:
  O_farm = rn.mix(-30.0, Zu, 1.0, mk_coude)

  # --- Auxiliary forearm coordinate system ---
  Of = O_farm
  Xf, junk = rn.dir(rn.sub(mk_poignet, Ou))
  Yf1, junk = rn.dir(rn.sub(mk_epaule, mk_coude)) # Rough {Y_farm}.
  Zf = Zu # Rough elbow axis.
  Yf, junk = rn.dir(rn.scale(sgn, rn.cross2(Zf, Xf))) # Better {Y_farm}

  # Estimated center of wrist joint:
  O_hand = rn.mix(-30.0, Yf, 1.0, mk_poignet)

  # --- Upper arm size and coordinate axes ---
  L_uarm = rn.dist(O_farm, O_uarm)
  X_uarm, junk = rn.dir(rn.sub(O_farm, O_uarm))
  Z_uarm, junk = rn.dir(rn.scale(sgn, rn.cross2(X_uarm, Yu)))
  Y_uarm, junk = rn.dir(rn.scale(sgn, rn.cross2(Z_uarm, X_uarm)))

  # --- Forearm size and coordinate axes ---
  L_farm = rn.dist(O_hand, O_farm)
  X_farm, junk = rn.dir(rn.sub(O_hand, O_farm))
  Z_farm, junk = rn.dir(rn.scale(sgn, rn.cross2(X_farm, Yf)))
  Y_farm, junk = rn.dir(rn.scale(sgn, rn.cross2(Z_farm, X_farm)))

  # --- Hand size and coordinate axes ---
  L_hand = 0.5*rn.dist(mk_doigt, O_hand)
  X_hand, junk = rn.dir(rn.sub(mk_doigt, O_hand))
  Y_hand = rn.scale(-sgn, Z_farm)
  Z_hand, junk = rn.dir(rn.scale(sgn, rn.cross2(X_hand, Y_hand)))

  # Package the placements:
  suff = str(side)
  rp_scap =  ("scap" + suff,  "spine",       L_scap, O_scap, X_scap, Y_scap, Z_scap)
  rp_uarm =  ("uarm" + suff,  "scap" + suff, L_uarm, O_uarm, X_uarm, Y_uarm, Z_uarm)
  rp_farm =  ("farm" + suff,  "uarm" + suff, L_farm, O_farm, X_farm, Y_farm, Z_farm)
  rp_hand =  ("hand" + suff,  "farm" + suff, L_hand, O_hand, X_hand, Y_hand, Z_hand)

  return (rp_scap, rp_uarm, rp_farm, rp_hand)
  #---------------------------------------------------------------------

def compute_placements_leg(O_spine, Z_spine, mk_leg, side):
  # Computes the placements of parts of a leg, on the
  # side of the body specified by {side} (0=left, 1=right).
  #
  # The main input data is a tuple {mk_leg} of coordinate triplets for the
  # markers "hanche{X}", "genou{X}", "mal{X}", "pied{X}", relative
  # to the scene coordinate system, in that order; where {X} is either
  # "G" (left) or "D" (right).
  #
  # The procedure also needs the coordinates of the center {O_spine} of
  # the spine-pelvis articulation and the unit vector {Z_spine} of the
  # approximate axis of the backbone.
  #
  # The output is a tuple with the placements of parts "pelv{S}", "thigh{S}",
  # "shin{S}" and "foot{S}", relative to the scene system, in that order;
  # where {S} is the value of {side} as one digit.
  #
  # The {side} parameter affects the relative orientation of the axis
  # vectors of the local coordinate systems: namely, right-handed
  # if 1, left-footed if 0.

  sgn = 2*side - 1; # {+1} for right, {-1} for left.

  # Unpack:
  mk_hanche, mk_genou, mk_mal, mk_pied = mk_leg

  # --- Auxiliary half-pelvis coordinate system {Oe,Xe,Ze} ---
  Oe = O_spine
  Xe, junk = rn.dir(rn.sub(mk_hanche, Oe))   # Rough {X_pelv}.
  Ye, junk = rn.dir(rn.cross2(Z_spine, Xe))  # Rough {sgn*Y_pelv}.
  Ze, junk = rn.dir(rn.cross2(Xe, Ye))       # Rough {Z_pelv}.

  # Estimated position of center of joint between half-pelvis and thigh
  O_thigh = rn.mix(-100.0, Ze, 1.0, mk_hanche)

  # --- Half-Pelvis size and coordinate system ---
  O_pelv = O_spine
  L_pelv = rn.dist(O_thigh, O_pelv)
  X_pelv, junk = rn.dir(rn.sub(O_thigh, O_pelv))
  Y_pelv, junk = rn.dir(rn.scale(sgn, rn.cross2(Z_spine, X_pelv)))
  Z_pelv, junk = rn.dir(rn.scale(sgn, rn.cross2(X_pelv, Y_pelv)))

  # --- Auxiliary thigh coordinate system ---
  Ou = O_thigh
  Xu, junk = rn.dir(rn.sub(mk_genou, Ou)) # Rough {X_thigh}
  Yu1, junk = rn.dir(rn.sub(mk_genou, mk_mal)) # Rough {-X_shin}, {Y_thigh}.
  Zu, junk = rn.dir(rn.scale(sgn, rn.cross2(Xu, Yu1))) # Rough {Z_thigh}
  Yu, junk = rn.dir(rn.scale(sgn, rn.cross2(Zu, Xu))) # Better {Y_thigh}.

  # Estimated center of knee joint:
  O_shin = rn.mix(-30.0, Zu, 1.0, mk_genou)

  # --- Auxiliary "shin" (lower leg) coordinate system ---
  Of = O_shin
  Xf, junk = rn.dir(rn.sub(mk_mal, Ou))
  Yf1, junk = rn.dir(rn.sub(mk_hanche, mk_genou)) # Rough {Y_shin}.
  Zf = Zu # Rough knee axis.
  Yf, junk = rn.dir(rn.scale(sgn, rn.cross2(Zf, Xf))) # Better {Y_shin}

  # Estimated center of ankle joint:
  O_foot = rn.mix(-30.0, Yf, 1.0, mk_mal)

  # --- Thigh size and coordinate axes ---
  L_thigh = rn.dist(O_shin, O_thigh)
  X_thigh, junk = rn.dir(rn.sub(O_shin, O_thigh))
  Z_thigh, junk = rn.dir(rn.scale(sgn, rn.cross2(X_thigh, Yu)))
  Y_thigh, junk = rn.dir(rn.scale(sgn, rn.cross2(Z_thigh, X_thigh)))

  # --- Shin size and coordinate axes ---
  L_shin = rn.dist(O_foot, O_shin)
  X_shin, junk = rn.dir(rn.sub(O_foot, O_shin))
  Z_shin = Z_thigh
  Y_shin, junk = rn.dir(rn.scale(sgn, rn.cross2(Z_shin, X_shin)))

  # Auxiliary foot coordinate system. Assume zero lateral tilt of foot.:
  Xd, junk = rn.dir(rn.sub(mk_pied, O_foot)) # Vec {O_foot,mk_pied}
  Zd1 = X_shin
  Yd, junk = rn.dir(rn.scale(sgn, rn.cross2(Zd1, Xd))) # Rough outwards, perp to {Xd}.
  Zd, junk = rn.dir(rn.scale(sgn, rn.cross2(Xd, Zd1))) # Rough down, perp to {Xd}

  # --- Foot size and coordinate axes:
  L_foot = 1.5*rn.dist(mk_pied, O_foot)
  X_foot, junk = rn.dir(rn.mix3(1.0, Xd, -1.0, Yd, -1.0, Zd))
  Z_foot = Zd
  Y_foot, junk = rn.dir(rn.scale(sgn, rn.cross2(Z_foot, X_foot)))

  # Package the placements:
  suff = str(side)
  rp_pelv =  ("pelv"  + suff,  "spine",        L_pelv,  O_pelv,  X_pelv,  Y_pelv,  Z_pelv)
  rp_thigh = ("thigh" + suff,  "pelv"  + suff, L_thigh, O_thigh, X_thigh, Y_thigh, Z_thigh)
  rp_shin =  ("shin"  + suff,  "thigh" + suff, L_shin,  O_shin,  X_shin,  Y_shin,  Z_shin)
  rp_foot =  ("foot"  + suff,  "shin"  + suff, L_foot,  O_foot,  X_foot,  Y_foot,  Z_foot)

  return (rp_pelv, rp_thigh, rp_shin, rp_foot)
  #---------------------------------------------------------------------