void model::sync_phys(double dt)
{
if (!phys_flock_ || !phys_)
return;
point_3 wind(0.0,0.0,0.0);
double const max_accel = 15;
cg::geo_base_3 base = phys_->get_base(*phys_zone_);
decart_position cur_pos = phys_flock_->get_position();
geo_position cur_glb_pos(cur_pos, base);
double cur_speed = cg::norm(cur_pos.dpos);
double cur_course = cur_pos.orien.cpr().course;
double cur_roll = cur_pos.orien.cpr().roll;
// ?? cg::geo_direction
cg::polar_point_3 cp (cg::geo_base_3(desired_position_)(cur_glb_pos.pos));
cpr cpr_des = cg::cpr(cp.course,cp.pitch);
quaternion desired_orien_(cpr_des);
point_3 forward_dir = -cg::normalized_safe(cur_pos.orien.rotate_vector(point_3(0, 1, 0))) ;
point_3 right_dir = cg::normalized_safe(cur_pos.orien.rotate_vector(point_3(1, 0, 0))) ;
point_3 up_dir = cg::normalized_safe(cur_pos.orien.rotate_vector(point_3(0, 0, 1))) ;
#if 0
point_3 vk = cur_pos.dpos - wind;
point_3 Y = !cg::eq_zero(cg::norm(vk)) ? cg::normalized(vk) : forward_dir;
point_3 Z = cg::normalized_safe(right_dir ^ Y);
point_3 X = cg::normalized_safe(Y ^ Z);
cg::rotation_3 vel_rotation(X, Y, Z);
cg::rotation_3 rot(desired_orien_.cpr());
cg::geo_base_3 predict_pos = /*cur_glb_pos.pos*/desired_position_;//; FIXME desired_position_;
cg::geo_base_3 predict_tgt_pos = predict_pos(rot * cg::transform_4().inverted().translation()/* * body_transform_inv_.inverted().translation()*/); // FIXME *body_transform_inv_.inverted().translation()
double dist2target = cg::distance2d(cur_glb_pos.pos, predict_tgt_pos);
point_2 offset = cg::point_2(cur_glb_pos.pos(predict_tgt_pos));
point_3 Y_right_dir_proj = Y - Y * right_dir * right_dir;
double attack_angle = cg::rad2grad(cg::angle(Y_right_dir_proj, forward_dir)) * (-cg::sign(Y * up_dir));
double slide_angle = cg::rad2grad(cg::angle(Y, Y_right_dir_proj)) * (-cg::sign(Y * right_dir));
#endif
point_3 omega_rel = cg::get_rotate_quaternion(cur_glb_pos.orien, desired_orien_).rot_axis().omega() * _damping * (dt);
if(_targetSpeed > -1){
phys::flock::control_ptr(phys_flock_)->set_angular_velocity(omega_rel);
}
phys::flock::control_ptr(phys_flock_)->set_linear_velocity(forward_dir * _speed );
}
void model::sync_phys(double dt)
{
if ( !phys_ || !phys_model_)
return;
point_3 wind(0.0,0.0,0.0);
double const max_accel = 15;
cg::geo_base_3 base = phys_->get_base(*phys_zone_);
decart_position cur_pos = phys_model_->get_position();
geo_position cur_glb_pos(cur_pos, base);
double cur_speed = cg::norm(cur_pos.dpos);
double cur_course = cur_pos.orien.cpr().course;
double cur_roll = cur_pos.orien.cpr().roll;
// ?? cg::geo_direction
cg::polar_point_3 cp (cg::geo_base_3(desired_position_)(cur_glb_pos.pos));
cpr cpr_des = cg::cpr(cp.course,cp.pitch);
quaternion desired_orien_(cpr_des);
point_3 forward_dir = cg::normalized_safe(cur_pos.orien.rotate_vector(point_3(0, 1, 0))) ;
point_3 right_dir = cg::normalized_safe(cur_pos.orien.rotate_vector(point_3(1, 0, 0))) ;
point_3 up_dir = cg::normalized_safe(cur_pos.orien.rotate_vector(point_3(0, 0, 1))) ;
point_3 omega_rel = cg::get_rotate_quaternion(cur_glb_pos.orien, desired_orien_).rot_axis().omega() * _damping * (dt);
if(_targetSpeed > -1 && !_down && !_off){
phys::rocket_flare::control_ptr(phys_model_)->set_angular_velocity(omega_rel);
}
if(cur_pos.pos.z > 300)
_down = true;
if(!_down)
phys::rocket_flare::control_ptr(phys_model_)->set_linear_velocity(forward_dir * _speed );
}
