void hk_Hinge_Constraint::step_constraint(
hk_PSI_Info& pi,
void *mem,
hk_real tau_factor,
hk_real strength_factor )
{
hk_Hinge_Constraint_Work& work = *(hk_Hinge_Constraint_Work*)mem;
hk_VM_Query_Builder< hk_VMQ_Storage<5> >& query_engine = work.query_engine;
hk_Rigid_Body *b0 = get_rigid_body(0);
hk_Rigid_Body *b1 = get_rigid_body(1);
query_engine.get_vmq_storage().clear_velocities();
query_engine.update_velocities(HK_BODY_A, b0);
query_engine.update_velocities(HK_BODY_B, b1);
hk_real* vmq_velocity = query_engine.get_vmq_storage().get_velocities();
hk_Fixed_Dense_Vector<5> delta;
delta.set_mul_add_mul( m_tau * tau_factor, work.m_correction, -1.0f * m_strength * strength_factor, vmq_velocity );
hk_Dense_Matrix& mass_matrix = query_engine.get_vmq_storage().get_dense_matrix();
hk_Fixed_Dense_Vector<5> impulses;
hk_Dense_Matrix_Util::mult( mass_matrix, delta, impulses);
query_engine.apply_impulses( HK_BODY_A, b0, impulses.get_const_real_pointer() );
query_engine.apply_impulses( HK_BODY_B, b1, impulses.get_const_real_pointer() );
}
void hk_Pulley_Constraint::step_constraint( hk_PSI_Info& pi, void *mem, hk_real tau_factor, hk_real damp_factor )
{
hk_Pulley_Work &work = *(hk_Pulley_Work *)mem;
hk_VM_Query_Builder< hk_VMQ_Storage<1> > &query_engine = work.query_engine;
hk_real *approaching_velocity = query_engine.get_vmq_storage().get_velocities();
approaching_velocity[0] = 0.0f;
hk_Rigid_Body *b0 = get_rigid_body(0);
hk_Rigid_Body *b1 = get_rigid_body(1);
query_engine.update_velocities(HK_BODY_A, b0);
query_engine.update_velocities(HK_BODY_B, b1);
if ( m_is_rigid || work.current_length >= 0 )
{
hk_real delta_dist = tau_factor * m_tau * pi.get_inv_delta_time() * work.current_length - damp_factor * m_strength * approaching_velocity[0];
hk_Vector3 impulses;
hk_Dense_Matrix& mass_matrix = query_engine.get_vmq_storage().get_dense_matrix();
impulses(0) = delta_dist * mass_matrix(0,0);
query_engine.apply_impulses( HK_BODY_A, b0, (hk_real *)&impulses(0) );
query_engine.apply_impulses( HK_BODY_B, b1, (hk_real *)&impulses(0) );
}
}
int hk_Pulley_Constraint::setup_and_step_constraint( hk_PSI_Info& pi, void *mem, hk_real tau_factor, hk_real damp_factor )
{
hk_Pulley_Work &work = *new (mem) hk_Pulley_Work;
hk_VM_Query_Builder< hk_VMQ_Storage<1> > &query_engine = work.query_engine;
hk_Rigid_Body *b0 = get_rigid_body(0);
hk_Rigid_Body *b1 = get_rigid_body(1);
hk_Vector3 translation_ws_ks[2];
translation_ws_ks[0]._set_transformed_pos( b0->get_cached_transform(), m_translation_os_ks[0]);
translation_ws_ks[1]._set_transformed_pos( b1->get_cached_transform(), m_translation_os_ks[1]);
hk_Vector3 dir[2];
dir[0].set_sub( m_worldspace_point[0], translation_ws_ks[0] );
dir[1].set_sub( m_worldspace_point[1], translation_ws_ks[1] );
dir[1]*=m_gearing;
work.current_length = dir[0].normalize_with_length() + dir[1].normalize_with_length() - m_length;
query_engine.begin(1);
{
query_engine.begin_entries(1);
{
query_engine.add_linear( 0, HK_BODY_A, b0, translation_ws_ks[0], dir[0], 1.0f );
query_engine.add_linear( 0, HK_BODY_B, b1, translation_ws_ks[1], dir[1], 1.0f );
}
query_engine.commit_entries(1);
}
query_engine.commit(HK_BODY_A, b0);
query_engine.commit(HK_BODY_B, b1);
hk_Dense_Matrix& mass_matrix = query_engine.get_vmq_storage().get_dense_matrix();
mass_matrix(0,0) = 1.0f / mass_matrix(0,0); // invert in place
{ // step
if ( m_is_rigid || work.current_length >= 0 )
{
hk_real *approaching_velocity = query_engine.get_vmq_storage().get_velocities();
hk_real delta_dist = tau_factor * m_tau * pi.get_inv_delta_time() * work.current_length - damp_factor * m_strength * approaching_velocity[0];
hk_Vector3 impulses;
impulses(0) = delta_dist * mass_matrix(0,0);
query_engine.apply_impulses( HK_BODY_A, b0, (hk_real *)&impulses(0) );
query_engine.apply_impulses( HK_BODY_B, b1, (hk_real *)&impulses(0) );
}
}
return HK_NEXT_MULTIPLE_OF(16, sizeof(hk_Pulley_Work));
}
void hk_Hinge_Constraint::init_hinge_constraint(const hk_Hinge_BP* bp)
{
m_tau = bp->m_tau;
m_strength = bp->m_strength;
m_axis_os[0] = bp->m_axis_os[0];
m_axis_os[1] = bp->m_axis_os[1];
(hk_Constraint_Limit_BP&)m_limit = bp->m_limit;
/* m_axis_perp_os[0,1] is a vector perpendicular to the hinge axis
* of body[0,1] in object space - its stored to avoid a normalize
* and an 'if' which is needed to compute it on the fly */
for(int i=0; i<2; ++i)
{
if( bp->m_axis_perp_os[i].length() != 0.0f)
{
m_axis_perp_os[i] = bp->m_axis_perp_os[i];
m_axis_perp_os[i].normalize();
}
else
{
m_axis_perp_os[i] = hk_Vector3_Util::perp_vec( m_axis_os[i].get_direction() );
}
}
hk_Rigid_Body* b0 = get_rigid_body(0);
hk_Rigid_Body* b1 = get_rigid_body(1);
const hk_Transform &t0 = b0->get_cached_transform();
const hk_Transform &t1 = b1->get_cached_transform();
hk_Vector3 line_ws[2];
line_ws[0].set_rotated_dir( t0, m_axis_os[0].get_direction());
line_ws[1].set_rotated_dir( t1, m_axis_os[1].get_direction());
hk_Vector3 perp_x_ws;
hk_Vector3 perp_y_ws;
perp_x_ws.set_rotated_dir( t0, m_axis_perp_os[0]);
perp_y_ws.set_cross( perp_x_ws, line_ws[0] );
hk_Vector3 perp_x2_ws;
perp_x2_ws.set_rotated_dir( t1, m_axis_perp_os[1]);
hk_Constraint_Limit_Util::init_angular_limit( perp_x_ws, perp_y_ws, this->m_limit, perp_x2_ws );
}
int hk_Hinge_Constraint::setup_and_step_constraint(
hk_PSI_Info& pi,
void *mem,
hk_real tau_factor,
hk_real strength_factor )
{
hk_Rigid_Body* b0 = get_rigid_body(0);
hk_Rigid_Body* b1 = get_rigid_body(1);
const hk_Transform &t0 = b0->get_cached_transform();
const hk_Transform &t1 = b1->get_cached_transform();
hk_Vector3 orig_ws[2];
orig_ws[0].set_transformed_pos( t0, m_axis_os[0].get_origin());
orig_ws[1].set_transformed_pos( t1, m_axis_os[1].get_origin());
hk_Vector3 line_ws[2];
line_ws[0].set_rotated_dir( t0, m_axis_os[0].get_direction());
line_ws[1].set_rotated_dir( t1, m_axis_os[1].get_direction());
hk_Vector3 perp_x_ws;
hk_Vector3 perp_y_ws;
perp_x_ws.set_rotated_dir( t0, m_axis_perp_os[0]);
perp_y_ws.set_cross( perp_x_ws, line_ws[0] );
if ( this->m_limit.m_limit_is_enabled | m_limit.m_friction_is_enabled ) // joint friction
{
hk_Vector3 perp_x2_ws; perp_x2_ws.set_rotated_dir( t1, m_axis_perp_os[1]);
hk_real sin_alpha = perp_x2_ws.dot( perp_y_ws );
hk_real cos_alpha = perp_x2_ws.dot( perp_x_ws );
hk_real alpha = hk_Math::atan2( sin_alpha, cos_alpha );
hk_Constraint_Limit_Util::do_angular_limit( pi, b0, line_ws[0], alpha, b1, m_limit, tau_factor, strength_factor );
}
//HK_DISPLAY_POINT(orig_ws[0], 0xff00ff);
//HK_DISPLAY_POINT(orig_ws[1], 0x00ffff);
//HK_DISPLAY_RAY(orig_ws[0], line_ws[0], 0xff00ff);
//HK_DISPLAY_RAY(orig_ws[1], line_ws[1], 0x00ffff);
hk_Hinge_Constraint_Work &work = *new (mem) hk_Hinge_Constraint_Work;
hk_VM_Query_Builder< hk_VMQ_Storage<5> >& query_engine = work.query_engine;
query_engine.begin(5);
{
query_engine.begin_entries(5);
hk_Mass_Relative_Vector3 mcr_pt_0( b0, orig_ws[0]);
hk_Mass_Relative_Vector3 mcr_pt_1( b1, orig_ws[1]);
// point-to-point
query_engine.add_linear_xyz( 0, HK_X_DIRECTION, HK_BODY_A, b0, mcr_pt_0, 1.0f );
query_engine.add_linear_xyz( 0, HK_X_DIRECTION, HK_BODY_B, b1, mcr_pt_1, -1.0f );
query_engine.add_linear_xyz( 1, HK_Y_DIRECTION, HK_BODY_A, b0, mcr_pt_0, 1.0f );
query_engine.add_linear_xyz( 1, HK_Y_DIRECTION, HK_BODY_B, b1, mcr_pt_1, -1.0f );
query_engine.add_linear_xyz( 2, HK_Z_DIRECTION, HK_BODY_A, b0, mcr_pt_0, 1.0f );
query_engine.add_linear_xyz( 2, HK_Z_DIRECTION, HK_BODY_B, b1, mcr_pt_1, -1.0f );
// angular
query_engine.add_angular( 3, HK_BODY_A, b0, perp_x_ws, 1.0f );
query_engine.add_angular( 3, HK_BODY_B, b1, perp_x_ws, -1.0f );
query_engine.add_angular( 4, HK_BODY_A, b0, perp_y_ws, 1.0f );
query_engine.add_angular( 4, HK_BODY_B, b1, perp_y_ws, -1.0f );
query_engine.commit_entries(5);
}
query_engine.commit(HK_BODY_A, b0);
query_engine.commit(HK_BODY_B, b1);
const hk_real* vmq_velocity = query_engine.get_vmq_storage().get_velocities();
hk_Fixed_Dense_Vector<5> delta;
{
hk_Vector3 &delta_dist = *(hk_Vector3 *)&work.m_correction(0);
delta_dist.set_sub( orig_ws[1], orig_ws[0] );
delta_dist.set_mul( pi.get_inv_delta_time(), delta_dist);
work.m_correction(3) = pi.get_inv_delta_time() * perp_y_ws.dot(line_ws[1]);
work.m_correction(4) = pi.get_inv_delta_time() * -perp_x_ws.dot(line_ws[1]);
delta.set_mul_add_mul( m_tau * tau_factor, work.m_correction, -1.0f * m_strength * strength_factor, vmq_velocity );
}
hk_Fixed_Dense_Matrix<5>& mass_matrix = query_engine.get_vmq_storage().get_fixed_dense_matrix();
hk_Dense_Matrix_Util::invert_5x5( mass_matrix, 0.0f);
hk_Fixed_Dense_Vector<5> impulses;
hk_Dense_Matrix_Util::mult( mass_matrix, delta, impulses);
query_engine.apply_impulses( HK_BODY_A, b0, impulses.get_const_real_pointer() );
query_engine.apply_impulses( HK_BODY_B, b1, impulses.get_const_real_pointer() );
return HK_NEXT_MULTIPLE_OF(16, sizeof(hk_Hinge_Constraint_Work));
}
void hk_Car_Wheel_Constraint::init_car_wheel_constraint(const hk_Car_Wheel_BP* bp)
{
m_tau = bp->m_tau;
m_damp = bp->m_damp;
m_translation_os_ks[0] = bp->m_translation_os_ks[0];
m_translation_os_ks[1] = bp->m_translation_os_ks[1];
m_hinge_axis_os[0] = bp->m_hinge_axis_os[0];
m_hinge_axis_os[1] = bp->m_hinge_axis_os[1];
m_hinge_axis_os[0].normalize();
m_hinge_axis_os[1].normalize();
m_initial_hinge_axis_Ros = m_hinge_axis_os[0];
m_steering_axis_Ros = bp->m_steering_axis_Ros;
m_steering_axis_Ros.normalize();
m_perp_hinge_axis_Aos = hk_Vector3_Util::perp_vec(m_hinge_axis_os[1]);
m_suspension_limit.init_limit( bp->m_suspension_limit, 1.0f);
m_wheel_limit.init_limit( bp->m_wheel_limit, 1.0f );
{
hk_Rigid_Body* b0 = get_rigid_body(0);
hk_Rigid_Body* b1 = get_rigid_body(1);
const hk_Transform &t0 = b0->get_cached_transform();
const hk_Transform &t1 = b1->get_cached_transform();
hk_Vector3 orig_ws[2];
orig_ws[0].set_transformed_pos( t0, m_translation_os_ks[0] );
orig_ws[1].set_transformed_pos( t1, m_translation_os_ks[1] );
hk_Vector3 delta_pos_ws;
delta_pos_ws.set_sub ( orig_ws[1], orig_ws[0] );
hk_Vector3 steering_axis_ws;
steering_axis_ws.set_rotated_dir( t0, m_steering_axis_Ros );
hk_Constraint_Limit_Util::init_linear_limit( steering_axis_ws, delta_pos_ws, m_suspension_limit);
{
hk_Vector3 hinge_axis_ws;
hinge_axis_ws.set_rotated_dir( t0, m_hinge_axis_os[0] );
hk_Vector3 perp_steering_axis_ws;
perp_steering_axis_ws.set_cross( steering_axis_ws, hinge_axis_ws);
hk_Vector3 perp_x2_ws;
perp_x2_ws.set_rotated_dir( t1, m_perp_hinge_axis_Aos);
hk_real sin_alpha = perp_x2_ws.dot( steering_axis_ws );
hk_real cos_alpha = perp_x2_ws.dot( perp_steering_axis_ws );
hk_real alpha = -hk_Math::atan2( sin_alpha, cos_alpha );
hk_Constraint_Limit_Util::init_angular_limit( m_wheel_limit, alpha);
}
}
}
int hk_Car_Wheel_Constraint::setup_and_step_constraint(
hk_PSI_Info& pi,
void *mem,
hk_real tau_factor,
hk_real damp_factor )
{
hk_Rigid_Body* b0 = get_rigid_body(0);
hk_Rigid_Body* b1 = get_rigid_body(1);
const hk_Transform &t0 = b0->get_cached_transform();
const hk_Transform &t1 = b1->get_cached_transform();
hk_Vector3 orig_ws[2];
orig_ws[0].set_transformed_pos( t0, m_translation_os_ks[0] );
orig_ws[1].set_transformed_pos( t1, m_translation_os_ks[1] );
hk_Vector3 hinge_axis_ws[2];
hinge_axis_ws[0].set_rotated_dir( t0, m_hinge_axis_os[0] );
hinge_axis_ws[1].set_rotated_dir( t1, m_hinge_axis_os[1] );
hk_Vector3 steering_axis_ws;
steering_axis_ws.set_rotated_dir( t0, m_steering_axis_Ros );
hk_Vector3 perp_steering_axis_ws;
perp_steering_axis_ws.set_cross( steering_axis_ws, hinge_axis_ws[0] );
hk_Vector3 delta_pos_ws;
delta_pos_ws.set_sub ( orig_ws[1], orig_ws[0] );
if ( this->m_suspension_limit.m_limit_is_enabled | m_suspension_limit.m_friction_is_enabled ) { // joint friction
hk_Constraint_Limit_Util::do_linear_limit( pi, b0, b1, orig_ws[0], steering_axis_ws, delta_pos_ws, m_suspension_limit, tau_factor, damp_factor );
}
if ( this->m_wheel_limit.m_limit_is_enabled | m_wheel_limit.m_friction_is_enabled ) { // joint friction
hk_Vector3 perp_x2_ws;
perp_x2_ws.set_rotated_dir( t1, m_perp_hinge_axis_Aos);
hk_real sin_alpha = perp_x2_ws.dot( steering_axis_ws );
hk_real cos_alpha = perp_x2_ws.dot( perp_steering_axis_ws );
hk_real alpha = -hk_Math::atan2( sin_alpha, cos_alpha );
hk_Constraint_Limit_Util::do_angular_limit( pi, b0, hinge_axis_ws[0], alpha, b1, m_wheel_limit, tau_factor, damp_factor );
}
hk_Car_Wheel_Constraint_Work &work = *new (mem) hk_Car_Wheel_Constraint_Work;
hk_VM_Query_Builder< hk_VMQ_Storage<4> >& query_engine = work.query_engine;
query_engine.begin(4);
{
query_engine.begin_entries(4);
hk_Mass_Relative_Vector3 mcr_pt_0( b0, orig_ws[0]);
hk_Mass_Relative_Vector3 mcr_pt_1( b1, orig_ws[1]);
// point-to-point
query_engine.add_linear( 0, HK_BODY_A, b0, mcr_pt_0, hinge_axis_ws[0], 1.0f );
query_engine.add_linear( 0, HK_BODY_B, b1, mcr_pt_1, hinge_axis_ws[0], -1.0f );
query_engine.add_linear( 1, HK_BODY_A, b0, mcr_pt_0, perp_steering_axis_ws, 1.0f );
query_engine.add_linear( 1, HK_BODY_B, b1, mcr_pt_1, perp_steering_axis_ws, -1.0f );
// angular
query_engine.add_angular( 2, HK_BODY_A, b0, steering_axis_ws, 1.0f );
query_engine.add_angular( 2, HK_BODY_B, b1, steering_axis_ws, -1.0f );
query_engine.add_angular( 3, HK_BODY_A, b0, perp_steering_axis_ws, 1.0f );
query_engine.add_angular( 3, HK_BODY_B, b1, perp_steering_axis_ws, -1.0f );
query_engine.commit_entries(4);
}
query_engine.commit(HK_BODY_A, b0);
query_engine.commit(HK_BODY_B, b1);
const hk_real* vmq_velocity = query_engine.get_vmq_storage().get_velocities();
hk_Fixed_Dense_Vector<4> delta;
{
hk_real tau = pi.get_inv_delta_time();
work.m_correction(0) = delta_pos_ws.dot( hinge_axis_ws[0] ) * tau;
work.m_correction(1) = delta_pos_ws.dot( perp_steering_axis_ws ) * tau;
work.m_correction(2) = hinge_axis_ws[1].dot(perp_steering_axis_ws) * tau;
work.m_correction(3) = -hinge_axis_ws[1].dot(steering_axis_ws) * tau;
delta.set_mul_add_mul( m_tau * tau_factor, work.m_correction, -1.0f * m_damp * damp_factor, vmq_velocity );
}
hk_Fixed_Dense_Matrix<4>& mass_matrix = query_engine.get_vmq_storage().get_fixed_dense_matrix();
hk_Dense_Matrix_Util::invert_4x4( mass_matrix, 0.0f);
hk_Fixed_Dense_Vector<4> impulses;
hk_Dense_Matrix_Util::mult( mass_matrix, delta, impulses);
query_engine.apply_impulses( HK_BODY_A, b0, impulses.get_const_real_pointer() );
query_engine.apply_impulses( HK_BODY_B, b1, impulses.get_const_real_pointer() );
return HK_NEXT_MULTIPLE_OF(16, sizeof(hk_Car_Wheel_Constraint_Work));
}
