// given an unaligned pointer, round it up to align and
// store the offset just before the returned pointer.
static inline void* hkMemoryRoundUp(void* pvoid, int align=16)
{
char* p = reinterpret_cast<char*>(pvoid);
char* aligned = reinterpret_cast<char*>(
HK_NEXT_MULTIPLE_OF( align, reinterpret_cast<int>(p+1)) );
reinterpret_cast<int*>(aligned)[-1] = (int)(aligned - p);
return aligned;
}
hk_Hinge_Constraint::hk_Hinge_Constraint(
hk_Local_Constraint_System* constraint_system,
const hk_Hinge_BP* bp,
hk_Rigid_Body* a ,
hk_Rigid_Body* b )
: hk_Constraint( constraint_system, a, b, HK_PRIORITY_LOCAL_CONSTRAINT, HK_NEXT_MULTIPLE_OF(16,sizeof(hk_Hinge_Constraint_Work)))
{
init_constraint(bp);
}
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));
}
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_Hinge_Constraint::get_vmq_storage_size()
{
return HK_NEXT_MULTIPLE_OF(16, sizeof(hk_Hinge_Constraint_Work) );
}
hk_Dynamic_Dense_Vector::hk_Dynamic_Dense_Vector(int n)
: hk_Dense_Vector(0, n, HK_NEXT_MULTIPLE_OF(4,n))
{
m_elt = hk_allocate( hk_real, HK_NEXT_MULTIPLE_OF(4,n), HK_MEMORY_CLASS_DENSE_VECTOR );
}
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));
}
