+ /*
+ * motorized joints
+ */
+ if( run_sim &&
+ (v3_length2(player_dead.v_lpf)>(k_ragdoll_active_threshold*
+ k_ragdoll_active_threshold)) ){
+ assert( rd->cone_constraints_count == rd->position_constraints_count );
+
+ mdl_keyframe anim[32];
+ skeleton_sample_anim( &localplayer.skeleton, player_dead.anim_bail,
+ 0.0f, anim );
+
+ for( u32 i=0; i<rd->cone_constraints_count; i ++ ){
+ rb_constr_swingtwist *st = &rd->cone_constraints[i];
+ rb_constr_pos *pc = &rd->position_constraints[i];
+
+ v3f va, vap;
+
+ m3x3_mulv( st->rbb->to_world, st->coneva, va );
+
+ /* calculate va as seen in rest position, from the perspective of the
+ * parent object, mapped to pose world space using the parents
+ * transform. thats our target */
+
+ u32 id_p = rd->constraint_associations[i][0],
+ id_a = rd->constraint_associations[i][1];
+
+ struct ragdoll_part *pa = &rd->parts[ id_a ],
+ *pp = &rd->parts[ id_p ];
+
+ mdl_keyframe *kf = &anim[ pa->bone_id-1 ];
+ m3x3_mulv( pa->collider_mtx, st->coneva, vap );
+ q_mulv( kf->q, vap, vap );
+
+ /* This could be a transfer function */
+ m3x3_mulv( pp->inv_collider_mtx, vap, vap );
+ m3x3_mulv( st->rba->to_world, vap, vap );
+
+ f32 d = v3_dot( vap, va ),
+ a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
+
+ v3f axis;
+ v3_cross( vap, va, axis );
+
+ f32 Fs = -a * k_ragdoll_spring,
+ Fd = -v3_dot( st->rbb->w, axis ) * k_ragdoll_dampening,
+ F = Fs+Fd;
+
+ v3f torque;
+ v3_muls( axis, F, torque );
+ v3_muladds( st->rbb->w, torque, k_rb_delta, st->rbb->w );
+
+ /* apply a adjustment to keep velocity at joint 0 */
+#if 0
+ v3f wcb, vcb;
+ m3x3_mulv( st->rbb->to_world, pc->lcb, wcb );
+ v3_cross( torque, wcb, vcb );
+ v3_muladds( st->rbb->v, vcb, k_rb_delta, st->rbb->v );
+#endif
+ }
+ }
+