/* Last edited on 2011-09-13 03:23:43 by stolfilocal */
/* _*_ latin-1 _*_  */
/* Bezier model of human body */
/* Coordinate axes are X = forwards, Y = right, Z = up. */

#include <bzb_patch.h>
#include <bzb_limb.h>


void bzb_joint_bend
  ( r3_t *q, 
    double x0, 
    double y0, 
    double z0, 
    double z1, 
    double frac,
    r3_t u, 
    r3_t w
  )
  {
    if (frac <= 0) { return; }

    /* Make sure {u,w} are orthonormal, and compute the third frame vector {v}: */
    double wm = r3_dir(&w, &w); demand(wm != 0, "invalid {w}");
    r3_t v;
    r3_cross(&w, &u, &v);
    double vm = r3_dir(&v, &v); demand(vm != 0, "invalid {u}");
    r3_cross(&v, &w, &u);
    double um = r3_dir(&u, &u); assert(um != 0);
    
    /* Compute the amount of twisting needed: */
    ???
    double bxy = hypot(, ay);
    if ((frac <= 0) || (axy == 0)) { return; }
    
    !!! Do it with H3 !!!
    
    double qx = q->c[0];
    double qy = q->c[1];
    double qz = q->c[2];
    
    /* Apply twist:*/
    double tz = az*frac;
    double ctz = cos(tz);
    double stz = sin(tz);
    
    double qxt = x0 + (qx - x0)*ctz - (qy - y0)*stz;
    double qyt = y0 + (qx - x0)*stz + (qy - y0)*ctz;
    double qzt = qz;
    
    /* Apply twist to angle too: */
    double axt = ax*ctz - ay*stz;
    double ayt = ax*stz + ay*ctz;
   
    /* Compute bending center: */
    double R = (z1 - z0)/axy;                         /* Radius of bent axis. */
    double uxt = axt/axy, uyt = ayt/axy;              /* Direction vector of axis bending. */
    double xc = x0 + R*uxt, yc = y0 + R*uyt, zc = z1; /* Center of bent axis. */
    
    /* Translate {q} so that center is at origin: */
    double qxr = qxt - xc;
    double qyr = qyt - yc;
    double qzr = qzt - zc;
    
    /* Apply bending: */
    ???


    double qr = (qxt - x0)*uxt + (qyt - y0)*uyt; /* Radius of point bending. */
    
    if ((px <= pm->r) && (pz >= z0))
          {
          }
        else
          { 
          }