+ 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] );