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));
}
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));
}
