the walk manifold is alive

[carveJwlIkooP6JGAAIwe30JlM.git] / character.h

#ifndef CHARACTER_H
#define CHARACTER_H

#include "common.h"
#include "model.h"
#include "scene.h"
#include "ik.h"
#include "rigidbody.h"
#include "shaders/character.h"

vg_tex2d tex_pallet = { .path = "textures/ch_gradient.qoi" };

static void character_register(void)
{
   shader_character_register();
}

static void character_init(void)
{
   vg_tex2d_init( (vg_tex2d *[]){ &tex_pallet }, 1 );
}

#define FOREACH_PART(FN) \
   FN( foot_l ) \
   FN( foot_r ) \
   FN( sock_l ) \
   FN( sock_r ) \
   FN( body0 ) \
   FN( body1 ) \
   FN( neck ) \
   FN( head ) \
   FN( arm_l0 ) \
   FN( arm_l1 ) \
   FN( hand_l ) \
   FN( arm_r0 ) \
   FN( arm_r1 ) \
   FN( hand_r ) \
   FN( leg_l0 ) \
   FN( leg_l1 ) \
   FN( leg_r0 ) \
   FN( leg_r1 ) \
   FN( wf ) \
   FN( wb ) \
   FN( board ) \

#define MAKE_ENUM(ENUM) k_chpart_##ENUM,
#define MAKE_STRING(STR) #STR,
#define ADD_ONE(_) +1
#define PART_COUNT FOREACH_PART(ADD_ONE)


enum character_part
{
   FOREACH_PART( MAKE_ENUM )
};

static const char *character_part_strings[] =
{
   FOREACH_PART( MAKE_STRING )
};

struct character
{
   glmesh mesh;
   
   submodel parts[ PART_COUNT ];
   m4x3f matrices[ PART_COUNT ];
   
   /* Auxillary information */
   v3f offsets[ PART_COUNT ];
   rigidbody ragdoll[ PART_COUNT ];

   /* 
    * Controls
    *  note: - base nodes of IK structures will be filled automatically 
    *        - all positions are in local space to the mroot
    */
   struct ik_basic ik_arm_l, ik_arm_r,
                   ik_leg_l, ik_leg_r,
                   ik_body;
   v3f cam_pos;

   v4f qhead;
   float rhip, rcollar, /* twist of hip and collar controls,
                           these act in the local +y axis of the part */
         rhead,
         rfootl, rfootr,
         rhandl, rhandr;

   v3f ground_normal;   /* Feet will be aligned to this */
   m4x3f mroot;

   int shoes[2];
};

static void character_offset( struct character *ch, enum character_part parent,
      enum character_part child )
{
   v3_sub( ch->parts[ child ].pivot, ch->parts[ parent ].pivot, 
         ch->offsets[ child ] );
}

static int character_load( struct character *ch, const char *name )
{
   char buf[64];

   snprintf( buf, sizeof(buf)-1, "models/%s.mdl", name );
   model *src = vg_asset_read( buf );

   if( !src )
   {
      vg_error( "Could not open 'models/%s.mdl'", name );
      return 0;
   }
   
   int error_count = 0;

   for( int i=0; i<PART_COUNT; i++ )
   {
      snprintf( buf, sizeof(buf)-1, "%s_%s", name, character_part_strings[i] );
      submodel *sm = submodel_get( src, buf );

      if( !sm )
      {
         vg_warn( "Character file does not contain an '_%s' part.\n",
               character_part_strings[i] );
         error_count ++;
         
         memset( &ch->parts[i], 0, sizeof(submodel) );
         continue;
      }

      ch->parts[i] = *sm;
   }

   model_unpack( src, &ch->mesh );
   
   if( !error_count )
      vg_success( "Loaded character file '%s' with no errors\n", name );

   /* Create the offsets */
   character_offset( ch, k_chpart_body0, k_chpart_body1 );
   character_offset( ch, k_chpart_body1, k_chpart_neck );
   character_offset( ch, k_chpart_neck, k_chpart_head );

   character_offset( ch, k_chpart_body1, k_chpart_arm_l0 );
   character_offset( ch, k_chpart_arm_l0, k_chpart_arm_l1 );
   character_offset( ch, k_chpart_arm_l1, k_chpart_hand_l );

   character_offset( ch, k_chpart_body1, k_chpart_arm_r0 );
   character_offset( ch, k_chpart_arm_r0, k_chpart_arm_r1 );
   character_offset( ch, k_chpart_arm_r1, k_chpart_hand_r );

   character_offset( ch, k_chpart_body0, k_chpart_leg_l0 );
   character_offset( ch, k_chpart_leg_l0, k_chpart_leg_l1 );
   character_offset( ch, k_chpart_leg_l1, k_chpart_foot_l );

   character_offset( ch, k_chpart_body0, k_chpart_leg_r0 );
   character_offset( ch, k_chpart_leg_r0, k_chpart_leg_r1 );
   character_offset( ch, k_chpart_leg_r1, k_chpart_foot_r );
   
   character_offset( ch, k_chpart_board, k_chpart_wb );
   character_offset( ch, k_chpart_board, k_chpart_wf );

   ch->ik_arm_l.l1 = v3_length( ch->offsets[ k_chpart_arm_l1 ] );
   ch->ik_arm_l.l2 = v3_length( ch->offsets[ k_chpart_hand_l ] );
   ch->ik_arm_r.l1 = v3_length( ch->offsets[ k_chpart_arm_r1 ] );
   ch->ik_arm_r.l2 = v3_length( ch->offsets[ k_chpart_hand_r ] );

   ch->ik_leg_l.l1 = v3_length( ch->offsets[ k_chpart_leg_l1 ] );
   ch->ik_leg_l.l2 = v3_length( ch->offsets[ k_chpart_foot_l ] );
   ch->ik_leg_r.l1 = v3_length( ch->offsets[ k_chpart_leg_r1 ] );
   ch->ik_leg_r.l2 = v3_length( ch->offsets[ k_chpart_foot_r ] );

   ch->ik_body.l1 = v3_length( ch->offsets[ k_chpart_body1 ] );
   ch->ik_body.l2 = v3_length( ch->offsets[ k_chpart_neck ] );

   free( src );
   return 1;
}

static void align_to_board( struct character *ch, enum character_part id )
{
   /* Calculate rotation between board and feet */
   m4x3f *mats = ch->matrices;

   v3f foot_pos, foot_fwd, foot_target, board_norm, board_origin;
   v3_copy( mats[id][3], foot_pos );
   m3x3_mulv( mats[id], (v3f){1.0f,0.0f,0.0f}, foot_fwd );
   v3_add( foot_fwd, foot_pos, foot_target );

   m3x3_mulv( mats[k_chpart_board], (v3f){0.0f,1.0f,0.0f}, board_norm );
   m4x3_mulv( mats[k_chpart_board], (v3f){0.0f,0.13f,0.0f}, board_origin ); 

   vg_line( foot_pos, foot_target, 0xff00ff00 );

   v3f v0;
   v3_sub( board_origin, foot_target, v0 );
   float t = v3_dot( v0, board_norm ) / board_norm[1];
   foot_target[1] += t;
   vg_line( foot_pos, foot_target, 0xff00ffff );
   
   v3_sub( foot_target, foot_pos, foot_target );
   v3_normalize( foot_target );
   float ang = acosf( v3_dot( foot_target, foot_fwd ) );

   v4f qcorrection; m3x3f correction;
   q_axis_angle( qcorrection, (v3f){0.0f,0.0f,1.0f}, -ang );
   q_m3x3( qcorrection, correction );
   m3x3_mul( mats[id], correction, mats[id] );
}

static void character_eval( struct character *ch )
{
   m4x3f *mats = ch->matrices;
   v3f *offs = ch->offsets;

   ik_basic( &ch->ik_body, mats[k_chpart_body0], mats[k_chpart_body1], 
         k_ikY, k_ikX );
   
   m3x3f temp;
   v4f body_rotation;
   /* TODO: Do this directly via m3x3 */

   q_axis_angle( body_rotation, (v3f){0.0f,1.0f,0.0f}, ch->rhip );
   q_m3x3( body_rotation, temp );
   m3x3_mul( mats[k_chpart_body0], temp, mats[k_chpart_body0] );

   q_axis_angle( body_rotation, (v3f){0.0f,1.0f,0.0f}, ch->rcollar );
   q_m3x3( body_rotation, temp );
   m3x3_mul( mats[k_chpart_body1], temp, mats[k_chpart_body1] );
   
   /* Setup aux */
   m4x3_mulv( mats[k_chpart_body0], offs[k_chpart_leg_l0], ch->ik_leg_l.base );
   m4x3_mulv( mats[k_chpart_body0], offs[k_chpart_leg_r0], ch->ik_leg_r.base );
   m4x3_mulv( mats[k_chpart_body1], offs[k_chpart_arm_l0], ch->ik_arm_l.base );
   m4x3_mulv( mats[k_chpart_body1], offs[k_chpart_arm_r0], ch->ik_arm_r.base );

   /* IK for arms and legs */
   ik_basic( &ch->ik_arm_l, mats[k_chpart_arm_l0], mats[k_chpart_arm_l1], 
         k_ikZ, k_ikY );
   ik_basic( &ch->ik_arm_r, mats[k_chpart_arm_r0], mats[k_chpart_arm_r1],
         k_iknZ, k_ikY );
   ik_basic( &ch->ik_leg_l, mats[k_chpart_leg_l0], mats[k_chpart_leg_l1],
         k_ikY, k_iknX );
   ik_basic( &ch->ik_leg_r, mats[k_chpart_leg_r0], mats[k_chpart_leg_r1],
         k_ikY, k_iknX );
   
   /* Hands */
   m3x3_copy( mats[k_chpart_arm_l1], mats[k_chpart_hand_l] );
   m3x3_copy( mats[k_chpart_arm_r1], mats[k_chpart_hand_r] );
   m4x3_mulv( mats[k_chpart_arm_l1], offs[k_chpart_hand_l], 
         mats[k_chpart_hand_l][3] );
   m4x3_mulv( mats[k_chpart_arm_r1], offs[k_chpart_hand_r], 
         mats[k_chpart_hand_r][3] );
   
   /* Neck / Head */
   m3x3_copy( mats[k_chpart_body1], mats[k_chpart_neck] );
   m4x3_mulv( mats[k_chpart_body1], offs[k_chpart_neck], 
         mats[k_chpart_neck][3] );

   v4f qhead;
   q_axis_angle( qhead, (v3f){ 0.0f,1.0f,0.0f }, ch->rhead );
   q_m3x3( qhead, mats[k_chpart_head] );
   //m3x3_mul( mats[k_chpart_neck], mats[k_chpart_head], mats[k_chpart_head] );
   m4x3_mulv( mats[k_chpart_neck], offs[k_chpart_head], mats[k_chpart_head][3]);

   /* Feet */
   m3x3_copy( mats[k_chpart_leg_l1], mats[k_chpart_foot_l] );
   m3x3_copy( mats[k_chpart_leg_r1], mats[k_chpart_foot_r] );
   m4x3_mulv( mats[k_chpart_leg_l1], offs[k_chpart_foot_l], 
         mats[k_chpart_foot_l][3] );
   m4x3_mulv( mats[k_chpart_leg_r1], offs[k_chpart_foot_r], 
         mats[k_chpart_foot_r][3] );

   align_to_board( ch, k_chpart_foot_l );
   align_to_board( ch, k_chpart_foot_r );
   
   for( int i=0; i<PART_COUNT; i++ )
      m4x3_mul( ch->mroot, ch->matrices[i], ch->matrices[i] );
}

#define B3D_CO( X, Y, Z ) (v3f){ X, Z, -Y }

typedef struct character_pose character_pose;
struct character_pose
{
   v3f b0, b1, p, fr, fl, pl, pr, hl, hr, apl, apr, cam;
};

static character_pose pose_aero =
{
  .b0 =  {0.0721f, 0.8167f, 0.1365f},
  .b1 =  {-0.0773f, 1.1559f, -0.1699f},
  .p =   {0.0421f, 1.1430f, 0.2803f},
  .fr =  {0.0535f, 0.1312f, -0.3647f},
  .fl =  {-0.0605f, 0.1464f, 0.2917f},
  .pl =  {-0.1704f, 0.6889f, -0.4017f},
  .pr =  {0.0672f, 0.7598f, -0.5963f},
  .hl =  {-0.2153f, 0.7195f, -0.1345f},
  .hr =  {0.1974f, 0.7940f, -0.3522f},
  .apl = {-0.2008f, 0.9546f, 0.3687f},
  .apr = {0.3133f, 0.9299f, 0.0181f},
  .cam = {-0.3394f, 1.2661f, 0.2936f}
};

static character_pose pose_slide =
{
  .b0 =  {0.6732f, 0.5565f, -0.0000f},
  .b1 =  {0.8116f, 1.0547f, 0.0613f},
  .p =   {1.0404f, 0.7907f, 0.0186f},
  .fr =  {-0.0030f, 0.1366f, -0.4461f},
  .fl =  {-0.0030f, 0.1366f, 0.3480f},
  .pl =  {-0.0887f, 0.8229f, 0.3826f},
  .pr =  {-0.0887f, 0.8229f, -0.4621f},
  .hl =  {0.7749f, 0.5545f, 0.5310f},
  .hr =  {0.5844f, 1.2445f, -0.5456f},
  .apl = {1.0999f, 0.5390f, 0.2398f},
  .apr = {0.9816f, 0.9536f, -0.5463f},
  .cam = {0.9888f, 1.4037f, 0.6081f}
};

static character_pose pose_slide1 =
{
  .b0 =  {-0.2385f, 0.6403f, 0.1368f},
  .b1 =  {-0.5151f, 1.1351f, 0.1380f},
  .p =   {-0.1158f, 1.2118f, 0.3895f},
  .fr =  {-0.0030f, 0.1323f, -0.3190f},
  .fl =  {-0.0030f, 0.1323f, 0.5797f},
  .pl =  {-0.6568f, 0.4305f, 0.2069f},
  .pr =  {-0.6850f, 0.2740f, -0.2969f},
  .hl =  {-0.7029f, 0.6132f, 0.2972f},
  .hr =  {-0.2572f, 1.0104f, -0.4770f},
  .apl = {-0.4808f, 0.8480f, 0.3731f},
  .apr = {-0.0836f, 1.0480f, -0.1201f},
  .cam = {-1.0508f, 1.0769f, 0.0528f}
};

static character_pose pose_aero_reverse =
{
  .b0 =  {0.0616f, 0.8167f, -0.1415f},
  .b1 =  {0.0148f, 1.1559f, 0.1861f},
  .p =   {0.0558f, 1.1430f, -0.2779f},
  .fr =  {0.0535f, 0.1312f, -0.3647f},
  .fl =  {0.0730f, 0.1464f, 0.2917f},
  .pl =  {-0.2073f, 0.6889f, 0.3839f},
  .pr =  {-0.3584f, 0.4069f, 0.1032f},
  .hl =  {0.1567f, 0.7195f, 0.1997f},
  .hr =  {-0.3055f, 0.7940f, 0.2639f},
  .apl = {0.3143f, 0.9546f, -0.2784f},
  .apr = {-0.2885f, 0.9299f, -0.1236f},
  .cam = {-0.3394f, 1.2661f, -0.2936f}
};

static character_pose pose_stand =
{
  .b0 =  {0.1877f, 1.0663f, 0.0063f},
  .b1 =  {0.0499f, 1.5564f, -0.0584f},
  .p =   {0.5982f, 1.2810f, 0.0842f},
  .fr =  {0.0535f, 0.1312f, -0.3647f},
  .fl =  {0.0354f, 0.1464f, 0.2917f},
  .pl =  {-0.4325f, 0.6889f, 0.1823f},
  .pr =  {-0.4794f, 0.7598f, -0.3610f},
  .hl =  {0.0498f, 1.0058f, 0.2317f},
  .hr =  {0.0188f, 0.9786f, -0.2725f},
  .apl = {0.2898f, 1.3453f, 0.2303f},
  .apr = {0.5273f, 1.2876f, -0.1848f},
  .cam = {-0.3477f, 1.5884f, -0.0019f}
};

static character_pose pose_fly =
{
  .b0 =  {0.2995f, 0.6819f, -0.1369f},
  .b1 =  {0.1618f, 1.1720f, -0.2016f},
  .p =   {0.7477f, 0.9173f, -0.1885f},
  .fr =  {0.0535f, 0.1312f, -0.3647f},
  .fl =  {0.0354f, 0.1464f, 0.2917f},
  .pl =  {-0.2930f, 0.4849f, 0.5307f},
  .pr =  {-0.4754f, 0.4124f, -0.4874f},
  .hl =  {0.2650f, 1.1897f, 0.4626f},
  .hr =  {0.2494f, 1.2059f, -0.7985f},
  .apl = {0.5165f, 1.0990f, 0.1655f},
  .apr = {0.6759f, 1.0661f, -0.6014f},
  .cam = {-0.2727f, 1.2606f, 0.3564f}
};

static 
void character_pose_blend( struct character *ch, character_pose *pose, float q )
{
   v3_muladds( ch->ik_body.base, pose->b0, q, ch->ik_body.base );
   v3_muladds( ch->ik_body.end, pose->b1, q, ch->ik_body.end );
   v3_muladds( ch->ik_body.pole, pose->p, q, ch->ik_body.pole );
   v3_muladds( ch->ik_leg_l.end, pose->fl, q, ch->ik_leg_l.end );
   v3_muladds( ch->ik_leg_l.pole, pose->pl, q, ch->ik_leg_l.pole );
   v3_muladds( ch->ik_leg_r.end, pose->fr, q, ch->ik_leg_r.end );
   v3_muladds( ch->ik_leg_r.pole, pose->pr, q, ch->ik_leg_r.pole );
   v3_muladds( ch->ik_arm_l.pole, pose->apl, q, ch->ik_arm_l.pole );
   v3_muladds( ch->ik_arm_r.pole, pose->apr, q, ch->ik_arm_r.pole );
   v3_muladds( ch->ik_arm_l.end, pose->hl, q, ch->ik_arm_l.end );
   v3_muladds( ch->ik_arm_r.end, pose->hr, q, ch->ik_arm_r.end );
   v3_muladds( ch->cam_pos, pose->cam, q, ch->cam_pos );
}

static 
void character_final_pose( struct character *ch, v3f cog,
      character_pose *pose, float q )
{
   character_pose npose;
   float dip = vg_clampf(cog[1], -1.0f, 0.3f) * 0.35f,
         tilt = vg_clampf(cog[2], -1.0f, 1.0f) * 0.3f;

   v4f rz; m4x3f tr;
   q_axis_angle( rz, (v3f){0.0f,0.0f,1.0f}, -cog[0]*0.6f );
   q_m3x3( rz, tr );
   v3_copy( (v3f){0.0f,dip,tilt}, tr[3] );

   m4x3_mulv( tr, pose->b0, npose.b0 );
   m4x3_mulv( tr, pose->b1, npose.b1 );
   m4x3_mulv( tr, pose->p, npose.p );
   m4x3_mulv( tr, pose->pl, npose.pl );
   m4x3_mulv( tr, pose->pr, npose.pr );
   m4x3_mulv( tr, pose->hl, npose.hl );
   m4x3_mulv( tr, pose->hr, npose.hr );
   m4x3_mulv( tr, pose->apl, npose.apl );
   m4x3_mulv( tr, pose->apr, npose.apr );

   v3_copy( pose->fr, npose.fr );
   v3_copy( pose->fl, npose.fl );
   v3_copy( pose->cam, npose.cam );
   
   character_pose_blend( ch, &npose, q );
}

static void zero_ik_basic( struct ik_basic *ik )
{
   v3_zero( ik->base );
   v3_zero( ik->end );
   v3_zero( ik->pole );
}

static void character_pose_reset( struct character *ch )
{
   zero_ik_basic( &ch->ik_body );
   zero_ik_basic( &ch->ik_leg_l );
   zero_ik_basic( &ch->ik_leg_r );
   zero_ik_basic( &ch->ik_arm_l );
   zero_ik_basic( &ch->ik_arm_r );
   v3_zero( ch->cam_pos );
}

static void character_testpose( struct character *ch, float t )
{
   /* Body */
   float *hips = ch->ik_body.base,
         *collar = ch->ik_body.end,
         *pole = ch->ik_body.pole;

   hips[0] = cosf(t*1.325f)*0.25f;
   hips[1] = (sinf(t)*0.2f+0.6f) * ch->parts[ k_chpart_body0 ].pivot[1];
   hips[2] = 0.0f;
   
   collar[0] = hips[0];
   collar[1] = hips[1] + (ch->ik_body.l1+ch->ik_body.l2)*(sinf(t)*0.05f+0.94f);
   collar[2] = hips[2] + cosf(t*0.42f)*0.01f;
   
   v3_add( hips, collar, pole );
   v3_muls( pole, 0.5f, pole );
   v3_add( pole, (v3f){ 1.0f, 0.0f, 0.0f }, pole );

   /* Legs */
   float *footl = ch->ik_leg_l.end,
         *footr = ch->ik_leg_r.end,
         *polel = ch->ik_leg_l.pole,
         *poler = ch->ik_leg_r.pole;

   footl[0] = sinf(t*0.563f);
   footl[1] = 0.0f;
   footl[2] = 0.0f;

   footr[0] = 0.0f;
   footr[1] = 0.0f;
   footr[2] = cosf(t*0.672f);
   
   v3_add( hips, footl, polel );
   v3_muls( polel, 0.4f, polel );
   v3_add( polel, (v3f){ -1.0f,0.0f,0.0f }, polel );

   v3_add( hips, footr, poler );
   v3_muls( poler, 0.4f, poler );
   v3_add( poler, (v3f){ -1.0f,0.0f,0.0f }, poler );
   
   /* Arms */
   float *arml = ch->ik_arm_l.end,
         *armr = ch->ik_arm_r.end;
   polel = ch->ik_arm_l.pole;
   poler = ch->ik_arm_r.pole;

   v3_copy( (v3f){ 0.0f, 0.0f, 1.0f }, arml );
   v3_copy( (v3f){ 0.0f, 0.0f,-1.0f }, armr );
   v3_copy( (v3f){ 1.0f, 1.0f, 0.5f }, polel );
   v3_copy( (v3f){ 1.0f, 1.0f,-0.5f }, poler );

   /* Other */
   ch->rhip = sinf(t*0.2f);
   ch->rcollar = sinf(t*0.35325f);
   q_identity( ch->qhead );

   m4x3_identity( ch->matrices[k_chpart_board] );
   m4x3_identity( ch->matrices[k_chpart_wb] );
   m4x3_identity( ch->matrices[k_chpart_wf] );
}

static void character_draw( struct character *ch, float temp )
{
   shader_character_use();
   shader_character_uPv( vg_pv );

   vg_tex2d_bind( &tex_pallet, 0 );
   shader_character_uTexMain( 0 );
   shader_character_uOpacity( temp );
   
   glEnable( GL_CULL_FACE );
   glCullFace( GL_BACK );

   mesh_bind( &ch->mesh );

   for( int i=4; i<PART_COUNT; i++ )
   {
      shader_character_uMdl( ch->matrices[i] );
      submodel_draw( &ch->parts[i] );
   }

   for( int i=0; i<2; i++ )
   {
      if( ch->shoes[i] )
      {
         shader_character_uMdl( ch->matrices[i] );
         submodel_draw( &ch->parts[i] );
      }
      else
      {
         shader_character_uMdl( ch->matrices[i+2] );
         submodel_draw( &ch->parts[i] );
         shader_character_uMdl( ch->matrices[i] );
         submodel_draw( &ch->parts[i+2] );
      }
   }
}

/* 
 * Ragdoll Stuff
 */

static void character_rd_box( struct character *ch, enum character_part id,
                              v3f dims )
{
   v3_muls( dims, -0.5f, ch->ragdoll[id].bbx[0] );
   v3_muls( dims,  0.5f, ch->ragdoll[id].bbx[1] );
}

struct rd_joint
{
   enum character_part ia, ib;
   v3f lca, lcb;

   struct rd_joint_axis
   {
       v3f va, vb;
       float spring, ang;
   }
   min, maj;
};

static const float k_human_major = 0.5f,
                   k_human_minor = 0.5f,
                   k_human_major_max = 0.4f,
                   k_human_minor_max = 0.1f;

#define HUMAN_VERTICAL_DEFAULT                        \
.min = {                                              \
   .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f},    \
   .spring = k_human_minor, .ang = k_human_minor_max  \
},                                                    \
.maj = {                                              \
   .va = {0.0f,1.0f,0.0f}, .vb = {0.0f,1.0f,0.0f},    \
   .spring = k_human_major, .ang = k_human_major_max  \
}

#define HUMAN_ARM_LEFT                                \
.min = {                                              \
   .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f},    \
   .spring = k_human_minor, .ang = k_human_minor_max  \
},                                                    \
.maj = {                                              \
   .va = {0.0f,0.0f,1.0f}, .vb = {0.0f,0.0f,1.0f},    \
   .spring = k_human_major, .ang = k_human_major_max  \
}

#define HUMAN_ARM_RIGHT                               \
.min = {                                              \
   .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f},    \
   .spring = k_human_minor, .ang = k_human_minor_max  \
},                                                    \
.maj = {                                              \
   .va = {0.0f,0.0f,-1.0f}, .vb = {0.0f,0.0f,-1.0f},  \
   .spring = k_human_major, .ang = k_human_major_max  \
}

static struct rd_joint rd_joints[] =
{
   { .ia = k_chpart_leg_l1, .ib = k_chpart_foot_l, HUMAN_VERTICAL_DEFAULT },
   { .ia = k_chpart_leg_r1, .ib = k_chpart_foot_r, HUMAN_VERTICAL_DEFAULT },

   { .ia = k_chpart_body0, .ib = k_chpart_body1, HUMAN_VERTICAL_DEFAULT },
   { .ia = k_chpart_body1, .ib = k_chpart_neck, HUMAN_VERTICAL_DEFAULT },
   { .ia = k_chpart_neck, .ib = k_chpart_head, HUMAN_VERTICAL_DEFAULT },
   { .ia = k_chpart_body0, .ib = k_chpart_leg_l0, HUMAN_VERTICAL_DEFAULT },
   { .ia = k_chpart_leg_l0, .ib = k_chpart_leg_l1, HUMAN_VERTICAL_DEFAULT },
   { .ia = k_chpart_body0, .ib = k_chpart_leg_r0, HUMAN_VERTICAL_DEFAULT },
   { .ia = k_chpart_leg_r0, .ib = k_chpart_leg_r1, HUMAN_VERTICAL_DEFAULT },

   { .ia = k_chpart_body1, .ib = k_chpart_arm_l0, HUMAN_ARM_LEFT },
   { .ia = k_chpart_arm_l0, .ib = k_chpart_arm_l1, HUMAN_ARM_LEFT },
   { .ia = k_chpart_arm_l1, .ib = k_chpart_hand_l, HUMAN_ARM_LEFT },

   { .ia = k_chpart_body1, .ib = k_chpart_arm_r0, HUMAN_ARM_RIGHT },
   { .ia = k_chpart_arm_r0, .ib = k_chpart_arm_r1, HUMAN_ARM_RIGHT },
   { .ia = k_chpart_arm_r1, .ib = k_chpart_hand_r, HUMAN_ARM_RIGHT }
};

/* Ragdoll should be in rest pose when calling this function */
static void character_init_ragdoll_joints( struct character *ch )
{
   for( int i=0; i<vg_list_size(rd_joints); i++ )
   {
      struct rd_joint *joint = &rd_joints[i];
      
      float *hinge = ch->parts[joint->ib].pivot;
      v3_sub( hinge, ch->ragdoll[joint->ia].co, joint->lca );
      v3_sub( hinge, ch->ragdoll[joint->ib].co, joint->lcb );
   }

   for( int i=0; i<PART_COUNT; i++ )
   {
      float *pivot = ch->parts[i].pivot;
      v3_sub( ch->ragdoll[i].co, pivot, ch->ragdoll[i].delta );
   }
}

static void character_init_ragdoll( struct character *ch )
{
   v3f *offs = ch->offsets;
   rigidbody *rbs = ch->ragdoll;

   /* CHest */
   float chest_width = fabsf(offs[k_chpart_arm_r0][2])*2.0f,
         chest_depth = chest_width * 0.571f,
         chest_height = offs[k_chpart_neck][1];
   v3f chest_dims = { chest_depth, chest_height, chest_width };
   character_rd_box( ch, k_chpart_body1, chest_dims );

   v3_copy( ch->parts[k_chpart_body1].pivot, rbs[k_chpart_body1].co );
   rbs[k_chpart_body1].co[1] += chest_height*0.5f;

   /* Torso */
   v3f torso_dims = { chest_depth, 
                      offs[k_chpart_body1][1]-offs[k_chpart_leg_l0][1],
                      chest_width*0.85f };
   v3_copy( ch->parts[k_chpart_body0].pivot, rbs[k_chpart_body0].co );
   character_rd_box( ch, k_chpart_body0, torso_dims );

   /* Neck */
   v3f neck_dims = { chest_depth*0.5f, 
                     offs[k_chpart_head][1], 
                     chest_depth*0.5f };
   v3_copy( ch->parts[k_chpart_neck].pivot, rbs[k_chpart_neck].co );
   rbs[k_chpart_neck].co[1] += neck_dims[1]*0.5f;
   character_rd_box( ch, k_chpart_neck, neck_dims );

   /* Head */
   v3f head_dims = { chest_width*0.5f, chest_width*0.5f, chest_width*0.5f };
   v3_copy( ch->parts[k_chpart_head].pivot, rbs[k_chpart_head].co );
   rbs[k_chpart_head].co[1] += head_dims[1]*0.5f;
   character_rd_box( ch, k_chpart_head, head_dims );
   
   /* ARms */
   v3f ua_dims = { 0.0f, 0.0f, fabsf(offs[k_chpart_arm_l1][2]) };
   ua_dims[1] = 0.38f*ua_dims[2];
   ua_dims[0] = 0.38f*ua_dims[2];
   v3f la_dims = { ua_dims[0], ua_dims[1], fabsf(offs[k_chpart_hand_l][2]) };
   v3f hand_dims = { ua_dims[1], ua_dims[1]*0.5f, ua_dims[1] };

   character_rd_box( ch, k_chpart_arm_l0, ua_dims );
   character_rd_box( ch, k_chpart_arm_r0, ua_dims );
   character_rd_box( ch, k_chpart_arm_l1, la_dims );
   character_rd_box( ch, k_chpart_arm_r1, la_dims );
   character_rd_box( ch, k_chpart_hand_l, hand_dims );
   character_rd_box( ch, k_chpart_hand_r, hand_dims );

   v3_copy( ch->parts[k_chpart_arm_l0].pivot, rbs[k_chpart_arm_l0].co );
   rbs[k_chpart_arm_l0].co[2] += ua_dims[2] * 0.5f;
   v3_copy( ch->parts[k_chpart_arm_l1].pivot, rbs[k_chpart_arm_l1].co );
   rbs[k_chpart_arm_l1].co[2] += la_dims[2] * 0.5f;
   v3_copy( ch->parts[k_chpart_hand_l].pivot, rbs[k_chpart_hand_l].co );
   rbs[k_chpart_hand_l].co[2] += hand_dims[2] * 0.5f;

   v3_copy( ch->parts[k_chpart_arm_r0].pivot, rbs[k_chpart_arm_r0].co );
   rbs[k_chpart_arm_r0].co[2] -= ua_dims[2] * 0.5f;
   v3_copy( ch->parts[k_chpart_arm_r1].pivot, rbs[k_chpart_arm_r1].co );
   rbs[k_chpart_arm_r1].co[2] -= la_dims[2] * 0.5f;
   v3_copy( ch->parts[k_chpart_hand_r].pivot, rbs[k_chpart_hand_r].co );
   rbs[k_chpart_hand_r].co[2] -= hand_dims[2] * 0.5f;

   /* LEgs */
   v3f ul_dims = { 0.0f, fabsf(offs[k_chpart_leg_l1][1]), 0.0f };
   ul_dims[0] = 0.38f*ul_dims[1];
   ul_dims[2] = 0.38f*ul_dims[1];
   v3f ll_dims = { ul_dims[0], fabsf(offs[k_chpart_foot_l][1]), ul_dims[2] };
   v3f foot_dims = { 2.0f*ul_dims[0], ul_dims[0], ul_dims[0] };

   character_rd_box( ch, k_chpart_leg_l0, ul_dims );
   character_rd_box( ch, k_chpart_leg_r0, ul_dims );
   character_rd_box( ch, k_chpart_leg_l1, ll_dims );
   character_rd_box( ch, k_chpart_leg_r1, ll_dims );
   character_rd_box( ch, k_chpart_foot_l, foot_dims );
   character_rd_box( ch, k_chpart_foot_r, foot_dims );

   v3_copy( ch->parts[k_chpart_leg_l0].pivot, rbs[k_chpart_leg_l0].co );
   rbs[k_chpart_leg_l0].co[1] -= ul_dims[1] * 0.5f;
   v3_copy( ch->parts[k_chpart_leg_l1].pivot, rbs[k_chpart_leg_l1].co );
   rbs[k_chpart_leg_l1].co[1] -= ll_dims[1] * 0.5f;
   v3_copy( ch->parts[k_chpart_foot_l].pivot, rbs[k_chpart_foot_l].co );
   rbs[k_chpart_foot_l].co[1] -= foot_dims[1] * 0.5f;
   rbs[k_chpart_foot_l].co[0] -= foot_dims[0] * 0.5f;

   v3_copy( ch->parts[k_chpart_leg_r0].pivot, rbs[k_chpart_leg_r0].co );
   rbs[k_chpart_leg_r0].co[1] -= ul_dims[1] * 0.5f;
   v3_copy( ch->parts[k_chpart_leg_r1].pivot, rbs[k_chpart_leg_r1].co );
   rbs[k_chpart_leg_r1].co[1] -= ll_dims[1] * 0.5f;
   v3_copy( ch->parts[k_chpart_foot_r].pivot, rbs[k_chpart_foot_r].co );
   rbs[k_chpart_foot_r].co[1] -= foot_dims[1] * 0.5f;
   rbs[k_chpart_foot_r].co[0] -= foot_dims[0] * 0.5f;

   character_rd_box( ch, k_chpart_sock_l, foot_dims );
   character_rd_box( ch, k_chpart_sock_r, foot_dims );
   v3_copy( rbs[k_chpart_foot_l].co, rbs[k_chpart_sock_l].co );
   v3_copy( rbs[k_chpart_foot_r].co, rbs[k_chpart_sock_r].co );

   box_copy( (boxf){{-0.2f,-0.2f,-0.7f},{0.2f,0.2f,0.7f}}, 
         rbs[k_chpart_board].bbx );

   for( int i=0; i<PART_COUNT; i++ )
      rb_init( &ch->ragdoll[i] );

   character_init_ragdoll_joints( ch );
}

static void character_ragdoll_go( struct character *ch, v3f pos )
{
   character_init_ragdoll( ch );
   for( int i=0; i<PART_COUNT; i++ )
      v3_add( pos, ch->ragdoll[i].co, ch->ragdoll[i].co );
}

static void character_ragdoll_copypose( struct character *ch, v3f v )
{
   for( int i=0; i<PART_COUNT; i++ )
   {
      rigidbody *rb = &ch->ragdoll[i];

      m4x3_mulv( ch->matrices[i], rb->delta, rb->co );
      m3x3_q( ch->matrices[i], rb->q );
      v3_copy( v, rb->v );
      v3_zero( rb->I );
      rb->manifold_count = 0; /* ? */

      rb_update_transform( rb );
   }

   float vel = v3_length(v);

   ch->shoes[0] = 1;
   ch->shoes[1] = 1;
}

static void character_mimic_ragdoll( struct character *ch )
{
   for( int i=0; i<PART_COUNT; i++ )
   {
      rigidbody *rb = &ch->ragdoll[i];
      v3f *mat = ch->matrices[i];
      
      m3x3_copy( rb->to_world, mat );
      v3f inv_delta;
      v3_negate( rb->delta, inv_delta );
      m4x3_mulv( rb->to_world, inv_delta, mat[3] );
   }

   /* Attach wheels to board */
   m3x3_copy( ch->matrices[k_chpart_board], ch->matrices[k_chpart_wb] );
   m3x3_copy( ch->matrices[k_chpart_board], ch->matrices[k_chpart_wf] );
   m4x3_mulv( ch->matrices[k_chpart_board], ch->offsets[k_chpart_wb],
                                            ch->matrices[k_chpart_wb][3] );
   m4x3_mulv( ch->matrices[k_chpart_board], ch->offsets[k_chpart_wf],
                                            ch->matrices[k_chpart_wf][3] );
}

static void character_debug_ragdoll( struct character *ch )
{
   rb_debug( &ch->ragdoll[k_chpart_body0], 0xffffffff );
   rb_debug( &ch->ragdoll[k_chpart_body1], 0xffffffff );
   rb_debug( &ch->ragdoll[k_chpart_neck], 0xff00ff00 );
   rb_debug( &ch->ragdoll[k_chpart_head], 0xff00ff00 );

   rb_debug( &ch->ragdoll[k_chpart_arm_l0], 0xffffa500 );
   rb_debug( &ch->ragdoll[k_chpart_arm_l1], 0xffffa500 );
   rb_debug( &ch->ragdoll[k_chpart_hand_l], 0xffffa500 );

   rb_debug( &ch->ragdoll[k_chpart_arm_r0], 0xff00a5ff );
   rb_debug( &ch->ragdoll[k_chpart_arm_r1], 0xff00a5ff );
   rb_debug( &ch->ragdoll[k_chpart_hand_r], 0xff00a5ff );

   rb_debug( &ch->ragdoll[k_chpart_leg_l0], 0xffffa500 );
   rb_debug( &ch->ragdoll[k_chpart_leg_l1], 0xffffa500 );
   rb_debug( &ch->ragdoll[k_chpart_foot_l], 0xffffa500 );
   rb_debug( &ch->ragdoll[k_chpart_leg_r0], 0xff00a5ff );
   rb_debug( &ch->ragdoll[k_chpart_leg_r1], 0xff00a5ff );
   rb_debug( &ch->ragdoll[k_chpart_foot_r], 0xff00a5ff );
}

static void character_ragdoll_iter( struct character *ch )
{
   for( int i=0; i<PART_COUNT; i++ )
      rb_build_manifold( &ch->ragdoll[i] );

   v3f rv;

   float shoe_vel[2];
   for( int i=0; i<2; i++ )
      if( ch->shoes[i] )
         shoe_vel[i] = v3_length( ch->ragdoll[i].v );

   for( int i=0; i<20; i++ )
   {
      float const k_springfactor = 1.0f/20.0f;

      for( int j=0; j<PART_COUNT; j++ )
         rb_constraint_manifold( &ch->ragdoll[j] );

      for( int j=0; j<vg_list_size(rd_joints); j++ )
      {
         struct rd_joint *joint = &rd_joints[j];
         rigidbody *rba = &ch->ragdoll[joint->ia],
                   *rbb = &ch->ragdoll[joint->ib];

         rb_constraint_position( rba, joint->lca, rbb, joint->lcb );
         rb_constraint_angle( rba, joint->maj.va, rbb, joint->maj.vb,
                                   joint->maj.ang, 
                                   joint->maj.spring * k_springfactor );

         rb_constraint_angle( rba, joint->min.va, rbb, joint->min.vb,
                                   joint->min.ang, 
                                   joint->min.spring * k_springfactor );
      }
   }

   for( int j=0; j<vg_list_size(rd_joints); j++ )
   {
      struct rd_joint *joint = &rd_joints[j];
      rigidbody *rba = &ch->ragdoll[joint->ia],
                *rbb = &ch->ragdoll[joint->ib];
      rb_angle_limit_force( rba, joint->min.va, rbb, joint->min.vb,
                                 joint->min.ang );
      rb_angle_limit_force( rba, joint->maj.va, rbb, joint->maj.vb,
                                 joint->maj.ang );
   }

   for( int i=0; i<PART_COUNT; i++ )
      rb_iter( &ch->ragdoll[i] );
   
   for( int i=0; i<2; i++ )
   {
      if( ch->shoes[i] )
      {
         float a = v3_length( ch->ragdoll[i].v ) - shoe_vel[i];

         if( a > 2.0f )
         {
            ch->shoes[i] = 0;
            
            rigidbody *src = &ch->ragdoll[k_chpart_foot_l];
            rigidbody *dst = &ch->ragdoll[k_chpart_sock_l];

            v3_copy( src->co, dst->co );
            v3_copy( src->v, dst->v );
            v3_copy( src->q, dst->q );
            v3_copy( src->I, dst->I );
         }
      }
   }

   for( int i=0; i<PART_COUNT; i++ )
      rb_update_transform( &ch->ragdoll[i] );
}

#endif