- /* angle correction */
- if( v3_length2( s->state.up_dir ) > 0.001f ){
-
- if( v4_length(s->state.smoothed_rotation) <= 0.1f ||
- v4_length(s->state.smoothed_rotation) >= 1.1f ){
- vg_warn( "FIX THIS! CARROT\n" );
- v4_copy( player->rb.q, s->state.smoothed_rotation );
- }
- v4_lerp( s->state.smoothed_rotation, player->rb.q,
- 2.0f*vg.time_frame_delta,
- s->state.smoothed_rotation );
- q_normalize( s->state.smoothed_rotation );
-
- v3f yaw_ref = {1.0f,0.0f,0.0f},
- yaw_smooth = {1.0f,0.0f,0.0f};
- q_mulv( player->rb.q, yaw_ref, yaw_ref );
- q_mulv( s->state.smoothed_rotation, yaw_smooth, yaw_smooth );
- m3x3_mulv( player->rb.to_local, yaw_smooth, yaw_smooth );
- m3x3_mulv( player->rb.to_local, yaw_ref, yaw_ref );
-
- float yaw_counter_rotate = v3_dot(yaw_ref,yaw_smooth);
- yaw_counter_rotate = vg_clampf(yaw_counter_rotate,-1.0f,1.0f);
- yaw_counter_rotate = acosf( yaw_counter_rotate );
- yaw_counter_rotate *= 1.0f-s->blend_fly;
-
- v3f ndir;
- m3x3_mulv( player->rb.to_local, s->state.up_dir, ndir );
- v3_normalize( ndir );
-
- v3f up = { 0.0f, 1.0f, 0.0f };
-
- float a = v3_dot( ndir, up );
- a = acosf( vg_clampf( a, -1.0f, 1.0f ) );
-
- v3f axis;
- v4f qfixup, qcounteryaw, qtotal;
-
- v3_cross( up, ndir, axis );
- q_axis_angle( qfixup, axis, a );
-
- q_axis_angle( qcounteryaw, (v3f){0.0f,1.0f,0.0f}, yaw_counter_rotate );
- q_mul( qcounteryaw, qfixup, qtotal );
- q_normalize( qtotal );
-
- mdl_keyframe *kf_hip = &dest->pose[av->id_hip-1];
- v3f origin;
- v3_add( av->sk.bones[av->id_hip].co, kf_hip->co, origin );
-
- for( int i=0; i<vg_list_size(apply_to); i ++ ){
- mdl_keyframe *kf = &dest->pose[apply_to[i]-1];
-
- keyframe_rotate_around( kf, origin, av->sk.bones[apply_to[i]].co,
- qtotal );
- }
-
- v3f p1, p2;
- m3x3_mulv( player->rb.to_world, up, p1 );
- m3x3_mulv( player->rb.to_world, ndir, p2 );
-
- vg_line_arrow( player->rb.co, p1, 0.25f, VG__PINK );
- vg_line_arrow( player->rb.co, p2, 0.25f, VG__PINK );