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