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 Shape2D::add_rect(const Rectf &rect, const Angle &angle, bool reverse)
{
Path2D path;
Pointf point_1(rect.left, rect.top);
Pointf point_2(rect.right, rect.top);
Pointf point_3(rect.right, rect.bottom);
Pointf point_4(rect.left, rect.bottom);
if (angle.to_radians() != 0.0f)
{
Pointf center = rect.get_center();
point_1.rotate(center, angle);
point_2.rotate(center, angle);
point_3.rotate(center, angle);
point_4.rotate(center, angle);
}
path.add_line_to(point_1);
path.add_line_to(point_2);
path.add_line_to(point_3);
path.add_line_to(point_4);
if (reverse)
path.reverse();
add_path(path);
}
ctrl::ctrl(kernel::object_create_t const& oc, dict_copt dict)
: view(oc,dict)
, anim_started(false)
{
//nodes_management::manager_ptr manager = find_first_child<nodes_management::manager_ptr>(this);
if (nodes_manager_)
{
if (nodes_manager_->get_model() != settings_.model)
nodes_manager_->set_model(settings_.model);
root_->set_position(geo_position(state_.pos, point_3(), state_.orien, point_3()));
}
}
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 );
}
void visual::update(double time)
{
view::update(time);
if (nodes_management::vis_node_info_ptr(root_)->is_visible() && aerotow_)
{
if (!tow_visual_object_)
{
std::string tow_type = "towbar"; // "towbar" "tube"
tow_visual_object_ = dynamic_cast<visual_system*>(sys_)->create_visual_object(tow_type) ;
ts_ = boost::make_shared<tow_support>(*tow_visual_object_, tow_type);
}
aircraft::info_ptr towair;
if (tow_visual_object_ && *tow_visual_object_)
{
geo_base_3 base = dynamic_cast<visual_system_props*>(sys_)->vis_props().base_point;
nodes_management::node_info_ptr aerotow_root = aerotow_->root();
quaternion atr_quat = aerotow_root->get_global_orien();
point_3f offset = base(tow_point_node_->get_global_pos());
geo_point_3 air_tow_pos = geo_base_3(aerotow_root->get_global_pos())(aerotow_root->get_global_orien().rotate_vector(point_3(aerotow_->tow_point_transform().translation())));
point_3f offset2 = base(air_tow_pos);
cg::polar_point_3f dir(offset2 - offset);
cpr orien(dir.course, dir.pitch, 0);
// cg::transform_4f tr(cg::as_translation(offset), rotation_3f(orien), cg::as_scale(point_3f(1., dir.range, 1.)));
// (*tow_visual_object_)->node()->as_transform()->set_transform(tr);
ts_->update(dir, offset);
(*tow_visual_object_)->set_visible(true);
}
}
else
{
if (tow_visual_object_ && *tow_visual_object_)
(*tow_visual_object_)->set_visible(false);
}
}
void ctrl::attach_tow()
{
aircraft::info_ptr towair;
visit_objects<aircraft::info_ptr>(collection_, [this, &towair](aircraft::info_ptr air)->bool
{
geo_point_3 tow_pos = geo_base_3(air->pos())(cg::rotation_3(cpr(air->orien().course, 0, 0)) * (point_3(0, 5., 0) + point_3(air->tow_point_transform().translation())));
if (cg::distance2d(tow_pos, this->pos()) < 25)
{
towair = air;
return false;
}
return true;
});
if (towair)
send_cmd(msg::attach_tow_msg_t(object_info_ptr(towair)->object_id()));
}
void ctrl::attach_tow()
{
aircraft::info_ptr towair;
bool reverse = false;
visit_objects<aircraft::info_ptr>(collection_, [this, &towair,&reverse](aircraft::info_ptr air)->bool
{
geo_point_3 tow_pos = geo_base_3(air->pos())(cg::rotation_3(cpr(air->orien().course, 0, 0)) * (point_3(0, 5., 0) + point_3(air->tow_point_transform().translation())));
if (cg::distance2d(tow_pos, this->pos()) < 25)
{
double dist_tp = tow_point_node_ ?cg::distance2d(tow_pos,tow_point_node_->get_global_pos()) :10000.0;
double dist_rtp = rtow_point_node_?cg::distance2d(tow_pos,rtow_point_node_->get_global_pos()):10000.0;
if ( dist_rtp < dist_tp)
{
reverse = true;
}
towair = air;
return false;
}
return true;
});
if (towair)
send_cmd(msg::attach_tow_msg_t(object_info_ptr(towair)->object_id(), reverse));
}
void ctrl::fire_fight()
{
aircraft::info_ptr burning_plane;
bool reverse = false;
visit_objects<aircraft::info_ptr>(collection_, [this, &burning_plane](aircraft::info_ptr air)->bool
{
geo_point_3 tow_pos = geo_base_3(air->pos())(cg::rotation_3(cpr(air->orien().course, 0, 0)) * (point_3(0, 5., 0) ));
if (cg::distance2d(tow_pos, this->pos()) < 25)
{
burning_plane = air;
return false;
}
return true;
});
if (burning_plane)
send_cmd(msg::fight_fire_msg_t(object_info_ptr(burning_plane)->object_id(), 1));
}
