int KinematicAltBands::first_band_alt_generic(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
const TrafficState& ownship, const std::vector<TrafficState>& traffic, bool dir, bool green) {
int upper = (int)(dir ? std::floor((max_val(ownship)-min_val(ownship))/get_step())+1 :
std::floor((ownship.altitude()-min_val(ownship))/get_step()));
int lower = dir ? (int)(std::ceil(ownship.altitude()-min_val(ownship))/get_step()) : 0;
if (ownship.altitude() < min_val(ownship) || ownship.altitude() > max_val(ownship)) {
return -1;
} else {
return first_nat(lower,upper,dir,conflict_det,recovery_det,B,T,B2,T2,ownship,traffic,green);
}
}
void check_program(pState state){
pProgram thisStep = get_step(state, state->currentStep);
if (thisStep == NULL){
state->finished = TRUE;
return;
}
if (thisStep->step.type == 0){
pSensor thisSensor = find_sensor(state, thisStep->step.sensorID);
//if no sensor corresponds to sensorID, go to next step
if (thisSensor == NULL){
state->currentStep = state->currentStep + 1;
return;
}
//check temp, if sensortemp+5 > current temp, set net temp and go next step
if ( ( (unsigned int) thisSensor->sensorTemp + 5 ) > (unsigned int) state->setTemp){
//might need to break this.
state->setTemp = thisStep->step.temp;
state->currentStep = state->currentStep + 1;
return;
}
} else {
//if time passes trigger, set new temp and go to next step
if ((unsigned int)state->currentTime > (unsigned int)thisStep->step.timeVal){
state->setTemp = thisStep->step.temp;
state->currentStep = state->currentStep + 1;
return;
}
}
return;
}
double advance_system(double ** U, double dx , double t, int NT, double * P){
double ** alpha; initnxN(&alpha, NT-2, 2);
make_P(U, NT, P);
double ** F; initnxN(&F, NT, 3); make_F(U, F, alpha, NT, P);
double maxalpha = get_maxalpha(alpha, NT);
double dt = get_step(maxalpha, dx);
double Un[3];
//printf(" column %f %f %f F[1][1] = %f\n", U[0][0],U[1][0],U[2][0], F[1][1]);
double L;
int i,j; for(i=0;i<NT;++i){
for(j=0;j<3;++j){
L = - dt*(F[j][i+1]-F[j][i])/dx;
Un[j] = U[j][i] + L;
//printf("j=%d L = %f\n", j, L);
U[j][i] = Un[j];
}
//printf("i %d column %f %f %f\n", i,U[0][i],U[1][i],U[2][i]);
}
//printf("%f\n", dt);
Reset_BC(U, NT);
for(i=0;i<NT;++i){
//printf("i %d column %f %f %f\n", i,U[0][i],U[1][i],U[2][i]);
}
t += dt;
return(t);
}
void Physics2DDirectBodyState::integrate_forces() {
real_t step = get_step();
Vector2 lv = get_linear_velocity();
lv+=get_total_gravity() * step;
real_t av = get_angular_velocity();
float damp = 1.0 - step * get_total_linear_damp();
if (damp<0) // reached zero in the given time
damp=0;
lv*=damp;
damp = 1.0 - step * get_total_angular_damp();
if (damp<0) // reached zero in the given time
damp=0;
av*=damp;
set_linear_velocity(lv);
set_angular_velocity(av);
}
void Term__tweak(caStack* stack)
{
Term* t = as_term_ref(circa_input(stack, 0));
if (t == NULL)
return circa_output_error(stack, "NULL reference");
int steps = tweak_round(to_float(circa_input(stack, 1)));
caValue* val = term_value(t);
if (steps == 0)
return;
if (is_float(val)) {
float step = get_step(t);
// Do the math like this so that rounding errors are not accumulated
float new_value = (tweak_round(as_float(val) / step) + steps) * step;
set_float(val, new_value);
} else if (is_int(val))
set_int(val, as_int(val) + steps);
else
circa_output_error(stack, "Ref is not an int or number");
}
virtual void assign(CarData *car_data, bool full)
{
car_data->set_node_from(m_from);
car_data->set_node_to(to_node());
car_data->set_max_step(get_max_steps());
car_data->set_step(get_step());
car_data->set_type(static_cast<CarData::ProtoCarType>(get_type())); // safe
}
void SpinBox::_range_click_timeout() {
if (!drag.enabled && Input::get_singleton()->is_mouse_button_pressed(BUTTON_LEFT)) {
bool up = get_local_mouse_position().y < (get_size().height / 2);
set_value(get_value() + (up ? get_step() : -get_step()));
if (range_click_timer->is_one_shot()) {
range_click_timer->set_wait_time(0.075);
range_click_timer->set_one_shot(false);
range_click_timer->start();
}
} else {
range_click_timer->stop();
}
}
void SpinBox::_value_changed(double) {
String value = String::num(get_value(), Math::step_decimals(get_step()));
if (prefix != "")
value = prefix + " " + value;
if (suffix != "")
value += " " + suffix;
line_edit->set_text(value);
}
// dir=false is down, dir=true is up. Return NaN if there is not a resolution
double KinematicAltBands::resolution(Detection3D* conflict_det, Detection3D* recovery_det, const TrafficState& repac,
int epsh, int epsv, double B, double T, const TrafficState& ownship, const std::vector<TrafficState>& traffic,
bool dir) {
int ires = first_band_alt_generic(conflict_det,recovery_det,B,T,0,B,ownship,traffic,dir,true);
if (ires < 0) {
return (dir ? 1 : -1)*PINFINITY;
} else {
return min_val(ownship)+ires*get_step();
}
}
virtual void print ( std::ostream & os ) const
{
os << m_from
<< " "
<< to_node()
<< " "
<< get_max_steps()
<< " "
<< get_step()
<< " "
<< static_cast<unsigned int> ( get_type() );
}
void Range::set_as_ratio(double p_value) {
double v;
if (shared->exp_ratio && get_min() > 0) {
double exp_min = Math::log(get_min()) / Math::log((double)2);
double exp_max = Math::log(get_max()) / Math::log((double)2);
v = Math::pow(2, exp_min + (exp_max - exp_min) * p_value);
} else {
double percent = (get_max() - get_min()) * p_value;
if (get_step() > 0) {
double steps = round(percent / get_step());
v = steps * get_step() + get_min();
} else {
v = percent + get_min();
}
}
set_value(v);
}
virtual void assign(CarData *car_data, bool full)
{
// meghívjuk a protobuf által generált setter függvényeket
// és belehelyezzük a CarData objektumba az autó adatait
car_data->set_node_from(m_from);
car_data->set_node_to(to_node());
car_data->set_max_step(get_max_steps());
car_data->set_step(get_step());
// a cast biztosan lehetséges, így a static_cast is megfelelő lesz
// (nincs runtime ellenőrzés benne)
car_data->set_type(static_cast<CarData::ProtoCarType>(get_type())); // safe
}
int main()
{
int n, i, x, y;
scanf("%d", &n);
while(n-- > 0)
{
scanf("%d %d", &x, &y);
printf("%d\n", get_step(y - x));
}
return 0;
}
std::pair<Vect3, Velocity> KinematicTrkBands::trajectory(const TrafficState& ownship, double time, bool dir) const {
std::pair<Position,Velocity> posvel;
if (instantaneous_bands()) {
double trk = ownship.getVelocity().trk()+(dir?1:-1)*j_step_*get_step();
posvel = std::pair<Position,Velocity>(ownship.getPosition(),ownship.getVelocity().mkTrk(trk));
} else {
double gso = ownship.groundSpeed();
double bank = turn_rate_ == 0 ? bank_angle_ : std::abs(Kinematics::bankAngle(gso,turn_rate_));
double R = Kinematics::turnRadius(ownship.get_v().gs(), bank);
posvel = ProjectedKinematics::turn(ownship.getPosition(),ownship.getVelocity(),time,R,dir);
}
return std::pair<Vect3, Velocity>(ownship.pos_to_s(posvel.first),ownship.vel_to_v(posvel.first,posvel.second));
}
osmium::unsigned_object_id_type justine::robocar::Traffic::naive_nearest_gangster (
osmium::unsigned_object_id_type from,
osmium::unsigned_object_id_type to,
osmium::unsigned_object_id_type step )
{
osmium::unsigned_object_id_type ret = from;
double lon1 {0.0}, lat1 {0.0};
toGPS ( from, to, step, &lon1, &lat1 );
double maxd = std::numeric_limits<double>::max();
double lon2 {0.0}, lat2 {0.0};
/*for ( auto car:m_smart_cars )
{
if ( car->get_type() == CarType::GANGSTER )
{
toGPS ( car->from(), car->to() , car->get_step(), &lon2, &lat2 );
double d = dst ( lon1, lat1, lon2, lat2 );
if ( d < maxd )
{
maxd = d;
ret = car->to_node();
}
}
}*/
//atirva
for ( std::vector<std::shared_ptr<SmartCar>>::iterator it=m_smart_cars.begin() ; it != m_smart_cars.end() ; ++it )
{
auto car = *it;
if ( car->get_type() == CarType::GANGSTER )
{
toGPS ( car->from(), car->to() , car->get_step(), &lon2, &lat2 );
double d = dst ( lon1, lat1, lon2, lat2 );
if ( d < maxd )
{
maxd = d;
ret = car->to_node();
}
}
}
return ret;
}
void prepare_data( RTC_Clock clock)
{
power_screen(true);
display_clear();
show_picture(15, 30, IMAGE_LOGO);
// get_up_config();
get_down_config();
// get_gen_config();
get_relation();
get_base();
get_act();
get_step();
get_position();
get_event();
}
virtual void print(std::ostream &os) const {
os << m_from
<< " "
<< to_node()
<< " "
<< get_max_steps()
<< " "
<< get_step()
<< " "
<< static_cast<unsigned int>(get_type())
<< " "
<< get_num_captured_gangsters()
<< " "
<< m_name;
}
std::pair<Vect3, Velocity> KinematicAltBands::trajectory(const TrafficState& ownship, double time, bool dir) const {
double target_alt = min_val(ownship)+j_step_*get_step();
std::pair<Position,Velocity> posvel;
if (instantaneous_bands()) {
posvel = std::pair<Position,Velocity>(ownship.getPosition().mkZ(target_alt),ownship.getVelocity().mkVs(0));
} else {
double tsqj = ProjectedKinematics::vsLevelOutTime(ownship.getPosition(),ownship.getVelocity(),vertical_rate_,
target_alt,vertical_accel_)+time_step(ownship);
if (time <= tsqj) {
posvel = ProjectedKinematics::vsLevelOut(ownship.getPosition(), ownship.getVelocity(), time, vertical_rate_, target_alt, vertical_accel_);
} else {
Position npo = ownship.getPosition().linear(ownship.getVelocity(),time);
posvel = std::pair<Position,Velocity>(npo.mkZ(target_alt),ownship.getVelocity().mkVs(0));
}
}
return std::pair<Vect3,Velocity>(ownship.pos_to_s(posvel.first),ownship.vel_to_v(posvel.first,posvel.second));
}
virtual void print (std::ostream & os) const
{
os << m_from
<< " "
<< to_node()
<< " "
<< get_max_steps()
<< " "
<< get_step()
<< " "
<< static_cast<unsigned int> (get_type())
<< " "
<< num_gangsters_caught_
<< " "
<< team_name_
<< " "
<< id_;
}
static void handle_sort(t_heap **a_heap, t_heap **b_heap,
t_opt *opt, int *sorted_tab)
{
int nbr_elem_lst;
int i;
i = 1;
nbr_elem_lst = get_nbr_elem(a_heap);
opt->step = get_step(nbr_elem_lst);
while (is_sort(a_heap) != 1)
{
opt->split = get_pivot(sorted_tab, nbr_elem_lst, opt->step, i);
push_step_to_b(opt, nbr_elem_lst, i);
sort_step(a_heap, b_heap, opt);
put_max_in_last_pos(opt);
++i;
}
}
int main(int number_arguments, char** arguments)
{
int integral_parameter_length, integral_results_length;
int likelihood_parameter_length, likelihood_results_length;
double* min_parameters;
double* max_parameters;
double* point;
double* step;
mpi_evaluator__init(&number_arguments, &arguments);
mpi_evaluator__read_data(read_data);
integral_parameter_length = ap->number_parameters;
integral_results_length = 1 + ap->number_streams;
likelihood_parameter_length = 1 + ap->number_streams;
likelihood_results_length = 2;
evaluator__init_integral(integral_f, integral_parameter_length, integral_compose, integral_results_length);
evaluator__init_likelihood(likelihood_f, likelihood_parameter_length, likelihood_compose, likelihood_results_length);
printf("starting...\n");
mpi_evaluator__start();
get_min_parameters(ap, &min_parameters);
get_max_parameters(ap, &max_parameters);
get_search_parameters(ap, &point);
get_step(ap, &step);
printf("searching...\n");
if (arguments[2][0] == 'g' && arguments[2][1] == 'd')
{
synchronous_gradient_descent(arguments[1], arguments[2], point, step, ap->number_parameters);
}
else if (arguments[2][0] == 'c')
{
synchronous_conjugate_gradient_descent(arguments[1], arguments[2], point, step, ap->number_parameters);
// } else if (arguments[2][0] == 'n') {
// synchronous_newton_method(arguments[1], arguments[2], point, step, ap->number_parameters);
// } else if (arguments[2][0] == 'r') {
// randomized_newton_method(arguments[1], arguments[2], point, step, ap->number_parameters);
}
return 0;
}
void KinematicAltBands::alt_bands_generic(std::vector<Integerval>& l,
Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
const TrafficState& ownship, const std::vector<TrafficState>& traffic) {
int max_step = (int)std::floor((max_val(ownship)-min_val(ownship))/get_step())+1;
int d = -1; // Set to the first index with no conflict
for (int k = 0; k <= max_step; ++k) {
j_step_ = k;
if (d >=0 && conflict_free_traj_step(conflict_det,recovery_det,B,T,B2,T2,ownship,traffic)) {
continue;
} else if (d >=0) {
l.push_back(Integerval(d,k-1));
d = -1;
} else if (conflict_free_traj_step(conflict_det,recovery_det,B,T,B2,T2,ownship,traffic)) {
d = k;
}
}
if (d >= 0) {
l.push_back(Integerval(d,max_step));
}
}
virtual void assign(CarData *car_data, bool full)
{
// annyiban külonbozik a tobbi assign() függvénytől, hogy
// a rendőrautókra jellemző adatokat is átadjuk
// ezek a .proto fájlban "optional" értékek
car_data->set_node_from(m_from);
car_data->set_node_to(to_node());
car_data->set_max_step(get_max_steps());
car_data->set_step(get_step());
car_data->set_type(static_cast<CarData::ProtoCarType>(get_type())); // safe
car_data->set_caught(num_gangsters_caught_);
car_data->set_team(team_name_);
car_data->set_id(id_);
if(full){
car_data->set_size(route.size());
for(auto it = route.begin();it!=route.end();it++)
car_data->add_path(*it);
}else{
car_data->set_size(0);
}
}
bool KinematicAltBands::conflict_free_traj_step(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
bool trajdir = true;
if (instantaneous_bands()) {
return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic);
} else {
double tstep = time_step(ownship);
double target_alt = min_val(ownship)+j_step_*get_step();
Tuple5<double,double,double,double,double> tsqj = Kinematics::vsLevelOutTimes(ownship.altitude(),ownship.verticalSpeed(),
vertical_rate_,target_alt,vertical_accel_,-vertical_accel_,true);
double tsqj1 = tsqj.first+0;
double tsqj2 = tsqj.second+0;
double tsqj3 = tsqj.third+tstep;
for (int i=0; i<=std::floor(tsqj1/tstep);++i) {
double tsi = i*tstep;
if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
(recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
return false;
}
}
if ((tsqj2>=B &&
any_conflict_aircraft(conflict_det,B,std::min(T,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic)) ||
(recovery_det != NULL && tsqj2>=B2 &&
any_conflict_aircraft(recovery_det,B2,std::min(T2,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic))) {
return false;
}
for (int i=(int)std::ceil(tsqj2/tstep); i<=std::floor(tsqj3/tstep);++i) {
double tsi = i*tstep;
if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
(recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
return false;
}
}
return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,std::max(tsqj3,0.0),ownship,traffic);
}
}
void PhysicsDirectBodyState::integrate_forces() {
real_t step = get_step();
Vector3 lv = get_linear_velocity();
lv += get_total_gravity() * step;
Vector3 av = get_angular_velocity();
float linear_damp = 1.0 - step * get_total_linear_damp();
if (linear_damp < 0) // reached zero in the given time
linear_damp = 0;
float angular_damp = 1.0 - step * get_total_angular_damp();
if (angular_damp < 0) // reached zero in the given time
angular_damp = 0;
lv *= linear_damp;
av *= angular_damp;
set_linear_velocity(lv);
set_angular_velocity(av);
}
void Slider::_gui_input(Ref<InputEvent> p_event) {
if (!editable) {
return;
}
Ref<InputEventMouseButton> mb = p_event;
if (mb.is_valid()) {
if (mb->get_button_index() == BUTTON_LEFT) {
if (mb->is_pressed()) {
Ref<Texture> grabber = get_icon(mouse_inside || has_focus() ? "grabber_highlight" : "grabber");
grab.pos = orientation == VERTICAL ? mb->get_position().y : mb->get_position().x;
double grab_width = (double)grabber->get_size().width;
double grab_height = (double)grabber->get_size().height;
double max = orientation == VERTICAL ? get_size().height - grab_height : get_size().width - grab_width;
if (orientation == VERTICAL)
set_as_ratio(1 - (((double)grab.pos - (grab_height / 2.0)) / max));
else
set_as_ratio(((double)grab.pos - (grab_width / 2.0)) / max);
grab.active = true;
grab.uvalue = get_as_ratio();
} else {
grab.active = false;
}
} else if (mb->is_pressed() && mb->get_button_index() == BUTTON_WHEEL_UP) {
set_value(get_value() + get_step());
} else if (mb->is_pressed() && mb->get_button_index() == BUTTON_WHEEL_DOWN) {
set_value(get_value() - get_step());
}
}
Ref<InputEventMouseMotion> mm = p_event;
if (mm.is_valid()) {
if (grab.active) {
Size2i size = get_size();
Ref<Texture> grabber = get_icon("grabber");
float motion = (orientation == VERTICAL ? mm->get_position().y : mm->get_position().x) - grab.pos;
if (orientation == VERTICAL)
motion = -motion;
float areasize = orientation == VERTICAL ? size.height - grabber->get_size().height : size.width - grabber->get_size().width;
if (areasize <= 0)
return;
float umotion = motion / float(areasize);
set_as_ratio(grab.uvalue + umotion);
}
}
if (!mm.is_valid() && !mb.is_valid()) {
if (p_event->is_action("ui_left") && p_event->is_pressed()) {
if (orientation != HORIZONTAL)
return;
set_value(get_value() - (custom_step >= 0 ? custom_step : get_step()));
accept_event();
} else if (p_event->is_action("ui_right") && p_event->is_pressed()) {
if (orientation != HORIZONTAL)
return;
set_value(get_value() + (custom_step >= 0 ? custom_step : get_step()));
accept_event();
} else if (p_event->is_action("ui_up") && p_event->is_pressed()) {
if (orientation != VERTICAL)
return;
set_value(get_value() + (custom_step >= 0 ? custom_step : get_step()));
accept_event();
} else if (p_event->is_action("ui_down") && p_event->is_pressed()) {
if (orientation != VERTICAL)
return;
set_value(get_value() - (custom_step >= 0 ? custom_step : get_step()));
accept_event();
} else {
Ref<InputEventKey> k = p_event;
if (!k.is_valid() || !k->is_pressed())
return;
switch (k->get_scancode()) {
case KEY_HOME: {
set_value(get_min());
accept_event();
} break;
case KEY_END: {
set_value(get_max());
accept_event();
} break;
}
}
}
}
Tripoint Pathfinder::get_step(Path_type type, int x0, int y0, int z0,
int x1, int y1, int z1)
{
return get_step(type, Tripoint(x0, y0, z0), Tripoint(x1, y1, z1));
}
Tripoint Pathfinder::get_step(Path_type type, Point start, Point end)
{
return get_step(type, Tripoint(start.x, start.y, 0),
Tripoint(end.x, end.y, 0));
}
void justine::robocar::Traffic::cmd_session ( boost::asio::ip::tcp::socket client_socket )
{
const int network_buffer_size = 524288;
char data[network_buffer_size]; // TODO buffered write...
try
{
for ( ;; )
{
CarLexer cl;
boost::system::error_code err;
size_t length = client_socket.read_some ( boost::asio::buffer ( data ), err );
if ( err == boost::asio::error::eof )
{
break;
}
else if ( err )
{
throw boost::system::system_error ( err );
}
std::istringstream pdata ( data );
cl.switch_streams ( &pdata );
cl.yylex();
length = 0;
int resp_code = cl.get_errnumber();
int num = cl.get_num();
int id {0};
if ( cl.get_cmd() == 0 )
{
for ( ;; )
{
std::vector<std::shared_ptr<Car>> cars_copy;
{
std::lock_guard<std::mutex> lock ( cars_mutex );
cars_copy = cars;
}
std::stringstream ss;
ss <<
m_time <<
" " <<
m_minutes <<
" " <<
cars_copy.size()
<< std::endl;
/*for ( auto car:cars_copy )
{
car->step();
ss << *car
<< " " << std::endl;
}*/
//atirva
for ( std::vector<std::shared_ptr<Car>>::iterator it=cars_copy.begin(); it != cars_copy.end(); ++it )
{
auto car = *it;
car->step();
ss << *car
<< " " << std::endl;
}
boost::asio::write ( client_socket, boost::asio::buffer ( data, length ) );
length = std::sprintf ( data,
"%s", ss.str().c_str() );
boost::asio::write ( client_socket, boost::asio::buffer ( data, length ) );
std::this_thread::sleep_for ( std::chrono::milliseconds ( 200 ) );
}
}
else if ( cl.get_cmd() <100 )
{
std::lock_guard<std::mutex> lock ( cars_mutex );
for ( int i {0}; i<cl.get_num(); ++i )
{
if ( cl.get_role() =='c' )
id = addCop ( cl );
else
id = addGangster ( cl );
if ( !resp_code )
length += std::sprintf ( data+length,
"<OK %d %d/%d %c>", id, num,
i+1, cl.get_role() );
else
length += std::sprintf ( data+length,
"<WARN %d %d/%d %c>", id, num,
i+1, cl.get_role() );
}
} // cmd 100
else if ( cl.get_cmd() == 101 )
{
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
std::shared_ptr<SmartCar> c = m_smart_cars_map[cl.get_id()];
if ( c->set_route ( cl.get_route() ) )
length += std::sprintf ( data+length, "<OK %d>", cl.get_id() );
else
length += std::sprintf ( data+length, "<ERR bad routing vector>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 1001 )
{
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
std::shared_ptr<SmartCar> c = m_smart_cars_map[cl.get_id()];
length += std::sprintf ( data+length,
"<OK %d %u %u %u>", cl.get_id(), c->from(),
c->to_node(), c->get_step() );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 1002 )
{
std::lock_guard<std::mutex> lock ( cars_mutex );
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
bool hasGangsters {false};
/*for ( auto c:m_smart_cars )
{
if ( c->get_type() == CarType::GANGSTER )
{
length += std::sprintf ( data+length,
"<OK %d %u %u %u>", cl.get_id(), c->from(),
c->to_node(), c->get_step() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many gangsters to send through this implementation...>" );
break;
}
hasGangsters = true;
}
} */
//atirva
for ( std::vector<std::shared_ptr<SmartCar>>::iterator it = m_smart_cars.begin(); it != m_smart_cars.end(); ++it )
{
auto c = *it;
if ( c->get_type() == CarType::GANGSTER )
{
length += std::sprintf ( data+length,
"<OK %d %u %u %u>", cl.get_id(), c->from(),
c->to_node(), c->get_step() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many gangsters to send through this implementation...>" );
break;
}
hasGangsters = true;
}
}
if ( !hasGangsters )
length += std::sprintf ( data+length, "<WARN there is no gangsters>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 1003 )
{
std::lock_guard<std::mutex> lock ( cars_mutex );
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
bool hasCops {false};
/* for ( auto c:m_cop_cars )
{
length += std::sprintf ( data+length,
"<OK %d %u %u %u %d>", cl.get_id(), c->from(),
c->to_node(), c->get_step(), c->get_num_captured_gangsters() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many cops to send through this implementation...>" );
break;
}
hasCops = true;
}*/
//atirva
for ( std::vector<std::shared_ptr<CopCar>>::iterator it = m_cop_cars.begin(); it != m_cop_cars.end(); ++it )
{
auto c = *it;
length += std::sprintf ( data+length,
"<OK %d %u %u %u %d>", cl.get_id(), c->from(),
c->to_node(), c->get_step(), c->get_num_captured_gangsters() );
if ( length > network_buffer_size - 512 )
{
length += std::sprintf ( data+length,
"<WARN too many cops to send through this implementation...>" );
break;
}
hasCops = true;
}
if ( !hasCops )
length += std::sprintf ( data+length, "<WARN there is no cops>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else if ( cl.get_cmd() == 10001 )
{
if ( m_smart_cars_map.find ( cl.get_id() ) != m_smart_cars_map.end() )
{
std::shared_ptr<SmartCar> c = m_smart_cars_map[cl.get_id()];
if ( c->set_fromto ( cl.get_from(), cl.get_to() ) )
length += std::sprintf ( data+length, "<OK %d>", cl.get_id() );
else
length += std::sprintf ( data+length, "<ERR cannot set>" );
}
else
length += std::sprintf ( data+length, "<ERR unknown car id>" );
}
else
{
length += std::sprintf ( data+length, "<ERR unknown proto command>" );
}
boost::asio::write ( client_socket, boost::asio::buffer ( data, length ) );
}
}
catch ( std::exception& e )
{
std::cerr << "Ooops: " << e.what() << std::endl;
}
}
/*!Performs Conjugate gradient minimization on the PatchData, pd.*/
void ConjugateGradient::optimize_vertex_positions(PatchData &pd,
MsqError &err){
// pd.reorder();
MSQ_FUNCTION_TIMER( "ConjugateGradient::optimize_vertex_positions" );
Timer c_timer;
size_t num_vert=pd.num_free_vertices();
if(num_vert<1){
MSQ_DBGOUT(1) << "\nEmpty free vertex list in ConjugateGradient\n";
return;
}
/*
//zero out arrays
int zero_loop=0;
while(zero_loop<arraySize){
fGrad[zero_loop].set(0,0,0);
pGrad[zero_loop].set(0,0,0);
fNewGrad[zero_loop].set(0,0,0);
++zero_loop;
}
*/
// get OF evaluator
OFEvaluator& objFunc = get_objective_function_evaluator();
size_t ind;
//Michael cull list: possibly set soft_fixed flags here
//MsqFreeVertexIndexIterator free_iter(pd, err); MSQ_ERRRTN(err);
double f=0;
//Michael, this isn't equivalent to CUBIT because we only want to check
//the objective function value of the 'bad' elements
//if invalid initial patch set an error.
bool temp_bool = objFunc.update(pd, f, fGrad, err);
assert(fGrad.size() == num_vert);
if(MSQ_CHKERR(err))
return;
if( ! temp_bool){
MSQ_SETERR(err)("Conjugate Gradient not able to get valid gradient "
"and function values on intial patch.",
MsqError::INVALID_MESH);
return;
}
double grad_norm=MSQ_MAX_CAP;
if(conjGradDebug>0){
MSQ_PRINT(2)("\nCG's DEGUB LEVEL = %i \n",conjGradDebug);
grad_norm=Linf(arrptr(fGrad),fGrad.size());
MSQ_PRINT(2)("\nCG's FIRST VALUE = %f,grad_norm = %f",f,grad_norm);
MSQ_PRINT(2)("\n TIME %f",c_timer.since_birth());
grad_norm=MSQ_MAX_CAP;
}
//Initializing pGrad (search direction).
pGrad.resize(fGrad.size());
for (ind = 0; ind < num_vert; ++ind)
pGrad[ind]=(-fGrad[ind]);
int j=0; // total nb of step size changes ... not used much
int i=0; // iteration counter
unsigned m=0; //
double alp=MSQ_MAX_CAP; // alp: scale factor of search direction
//we know inner_criterion is false because it was checked in
//loop_over_mesh before being sent here.
TerminationCriterion* term_crit=get_inner_termination_criterion();
//while ((i<maxIteration && alp>stepBound && grad_norm>normGradientBound)
// && !inner_criterion){
while(!term_crit->terminate()){
++i;
//std::cout<<"\Michael delete i = "<<i;
int k=0;
alp=get_step(pd,f,k,err);
j+=k;
if(conjGradDebug>2){
MSQ_PRINT(2)("\n Alp initial, alp = %20.18f",alp);
}
// if alp == 0, revert to steepest descent search direction
if(alp==0){
for (m = 0; m < num_vert; ++m) {
pGrad[m]=(-fGrad[m]);
}
alp=get_step(pd,f,k,err);
j+=k;
if(conjGradDebug>1){
MSQ_PRINT(2)("\n CG's search direction reset.");
if(conjGradDebug>2)
MSQ_PRINT(2)("\n Alp was zero, alp = %20.18f",alp);
}
}
if(alp!=0){
pd.move_free_vertices_constrained( arrptr(pGrad), num_vert, alp, err );
MSQ_ERRRTN(err);
if (! objFunc.update(pd, f, fNewGrad, err)){
MSQ_SETERR(err)("Error inside Conjugate Gradient, vertices moved "
"making function value invalid.",
MsqError::INVALID_MESH);
return;
}
assert(fNewGrad.size() == (unsigned)num_vert);
if(conjGradDebug>0){
grad_norm=Linf(arrptr(fNewGrad),num_vert);
MSQ_PRINT(2)("\nCG's VALUE = %f, iter. = %i, grad_norm = %f, alp = %f",f,i,grad_norm,alp);
MSQ_PRINT(2)("\n TIME %f",c_timer.since_birth());
}
double s11=0;
double s12=0;
double s22=0;
//free_iter.reset();
//while (free_iter.next()) {
// m=free_iter.value();
for (m = 0; m < num_vert; ++m) {
s11+=fGrad[m]%fGrad[m];
s12+=fGrad[m]%fNewGrad[m];
s22+=fNewGrad[m]%fNewGrad[m];
}
// Steepest Descent (takes 2-3 times as long as P-R)
//double bet=0;
// Fletcher-Reeves (takes twice as long as P-R)
//double bet = s22/s11;
// Polack-Ribiere
double bet;
if (!divide( s22-s12, s11, bet ))
return; // gradient is zero
//free_iter.reset();
//while (free_iter.next()) {
// m=free_iter.value();
for (m = 0; m < num_vert; ++m) {
pGrad[m]=(-fNewGrad[m]+(bet*pGrad[m]));
fGrad[m]=fNewGrad[m];
}
if(conjGradDebug>2){
MSQ_PRINT(2)(" \nSEARCH DIRECTION INFINITY NORM = %e",
Linf(arrptr(fNewGrad),num_vert));
}
}//end if on alp == 0
term_crit->accumulate_patch( pd, err ); MSQ_ERRRTN(err);
term_crit->accumulate_inner( pd, f, arrptr(fGrad), err ); MSQ_ERRRTN(err);
}//end while
if(conjGradDebug>0){
MSQ_PRINT(2)("\nConjugate Gradient complete i=%i ",i);
MSQ_PRINT(2)("\n- FINAL value = %f, alp=%4.2e grad_norm=%4.2e",f,alp,grad_norm);
MSQ_PRINT(2)("\n FINAL TIME %f",c_timer.since_birth());
}
}
