/**
*
* rct2: 0x006D4204
*/
void vehicle_update_all()
{
uint16 sprite_index;
rct_vehicle *vehicle;
if (RCT2_GLOBAL(RCT2_ADDRESS_SCREEN_FLAGS, uint8) & 2)
return;
if ((RCT2_GLOBAL(RCT2_ADDRESS_SCREEN_FLAGS, uint8) & 4) && RCT2_GLOBAL(0x0141F570, uint8) != 6)
return;
sprite_index = RCT2_GLOBAL(RCT2_ADDRESS_SPRITES_START_VEHICLE, uint16);
while (sprite_index != SPRITE_INDEX_NULL) {
vehicle = &(RCT2_ADDRESS(RCT2_ADDRESS_SPRITE_LIST, rct_sprite)[sprite_index].vehicle);
sprite_index = vehicle->next;
vehicle_update(vehicle);
}
}
// Veh_Integral
void Vehicle::Update()
{
double wind_dir, hdng, rand_wind=0.0;
double headrot[4];
double senspostang[2], vehsenspos[2];
double chemDensity;
Point3D vpos;
double u_wind[3];
int i_v,iii,jjj, j;
static int detect_yet;
double Sim_Time;
if(ed == NULL) return; // if no enviromental data then return
Sim_Time = ed->GetSimTime();
// save last pos
GetPos(last_p);
// Simulate the batch of vehicles
for(i_v = 0; i_v < NUM_VEH; i_v++)
{
if(!(veh[i_v].src_fnd_flg)) // if not done
{
// following are the lines where the vehicle simulation interfaces
// with the wind and concentration sensor models. The wind speed
// and concentration can be evaluated at any position of interest
hdng = veh[i_v].omega;
headrot[0] = cos(hdng);
headrot[1] = -sin(hdng);
headrot[2] = sin(hdng);
headrot[3] = cos(hdng);
double sensposbody[2];
for(j = 0; j < NUM_SENS; j++)
{
sensposbody[0] = 0;
sensposbody[1] = 0; // (-ns+j)*lngth/ns;
m_mul(senspostang, headrot, sensposbody, 2, 2, 1);
m_add(vehsenspos, &(veh[i_v].x), senspostang, 2, 1);
//veh[i_v].snsd[j] = chem->ReadConc(vehsenspos, u_wind);
{
vpos.x = vehsenspos[0];
vpos.y = vehsenspos[1];
vpos.z = 0.0; // vehsenspos[2]
ed->ApplyWind(vpos, u_wind);
chemDensity = ed->GetChemDensity(vpos);
veh[i_v].snsd[j] = chemDensity;
vehsenspos[0] = vpos.x;
vehsenspos[1] = vpos.y;
//vehsenspos[2] = vpos.z;
}
}
if (i_v==0) rand_wind = 5.0*nrand();
wind_dir = atan2(u_wind[1],u_wind[0])*180/PI + rand_wind;
// following is the call to the strategy
strat->in[0] = veh[i_v].x;
strat->in[1] = (-1)*veh[i_v].y; // invert because of axis flip
strat->in[2] = sqrt(u_wind[0]*u_wind[0] + u_wind[1]*u_wind[1]);
strat->in[3] = wind_dir;
strat->in[4] = Sim_Time;
strat->in[5] = veh[i_v].u;
strat->in[6] = veh[i_v].omega;
// added by JES. copy global to fit older code
double src_loc[2];
src_loc[0] = srcloc.x;
src_loc[1] = srcloc.y;
//
for(j = 7; j < 7 + NUM_SENS; j++)
{
strat->in[j] = veh[i_v].snsd[j-7];
}
strat->out[0]= veh[i_v].vel_c; // Initialize output values
strat->out[1]= veh[i_v].omega_c; // Initialize output values
strat->out[2]= veh[i_v].src_fnd_flg; // Initialize output values
//-------------------------Call Strategy----------------------------------------------
MyStratFunc(strat->in,
strat->out,
&(strat->mem[N_STRAT_MEM*i_v]),
dt,
radius,
threshold,
Sim_Time,
currentBehaviorName
);
//-------------------------Return from strategy----------------------------------------------
veh[i_v].vel_c = strat->out[0];
veh[i_v].omega_c = strat->out[1];
// The source found flag once set is set for the run
veh[i_v].src_fnd_flg = ((strat->out[2])||(veh[i_v].src_fnd_flg));
x_left=0.0;
y_top=-50;
d_x=1;
d_y=1;
if( veh[i_v].snsd[0] > threshold)
{
veh[i_v].det_tm = Sim_Time; // store most recent detection time
if(veh[i_v].plm_find_tm > Sim_Time)
veh[i_v].plm_find_tm = Sim_Time; // record time of first detection
}
if( ( hdist(&(veh[i_v].x), src_loc) < radius) && // near the source
( veh[i_v].src_find_tm > Sim_Time) ) // for the first time
{
veh[i_v].src_find_tm = Sim_Time; // store the frst tm near src
fnd_cnt++;
} // if near source
if( (veh[i_v].src_fnd_flg) &&
(veh[i_v].src_dclr_tm > Sim_Time) )
{
// Graphics --------------------------------------------
if( (batch_not == 1) &&
(i_v ==0 ) )
{
for(iii = 0; iii < GRID_X; iii++)
{
for(jjj = 0; jjj < GRID_Y; jjj++)
{
bck_grnd[iii][jjj] = (float)(strat->mem[1000+iii*100+jjj]);
}
}
// RESET!
veh[i_v].x = vehicle_pos_x; // ((double)random(100))*(x_max-x_min)/100.0 + x_min;
veh[i_v].y = vehicle_pos_y; // ((double)random(100))*(y_max-y_min)/100.0 + y_min;
veh[i_v].z = vehicle_pos_z; // ((double)random(100))*(z_max-z_min)/100.0 + z_min;
veh[i_v].omega_c = nrand()*180.0;
veh[i_v].dsd = 0.0;
veh[i_v].dsf = 0.0;
veh[i_v].dpf = 0.0;
veh[i_v].det_tm = -100;
veh[i_v].plm_find_tm = 1e8;
veh[i_v].src_find_tm = 1e8;
veh[i_v].src_dclr_tm = 1e8;
veh[i_v].src_dclr_d = 0.0;
veh[i_v].snsd[NS + 1] = 0;
veh[i_v].src_fnd_flg = 0;
}
// --------------------------------------------
else
{
veh[i_v].src_dclr_tm = Sim_Time; // store tm of first src found flag
veh[i_v].src_dclr_d = hdist(&(veh[i_v].x), src_loc);
veh[i_v].x_real=veh[i_v].x;
veh[i_v].y_real=veh[i_v].y;
veh[i_v].vel_c = 0; // stop this vehicle
done_cnt++; // veh is done
}
} // if src fnd flag
vehicle_model(&(veh[i_v]), Sim_Time); // advance vehicle to next time
//Graphics --------------------------------------------
if((batch_not==1)&&(i_v==0))
{ // Have we detected anything this interval
detect_yet = ( (veh[i_v].snsd[0] > threshold) || (detect_yet) );
veh_plot_cnt++; // track interval
if(veh_plot_cnt >= VEH_MAX_PLT_CNT)
{
veh_plot_cnt = 0;
if(vh_count < VH_M)
{
det_flag[vh_count] = detect_yet;
detect_yet = 0;
veh_omega_flag[vh_count] = veh[0].omega*PI/180.0;
vh_x[vh_count] = veh[0].x;
vh_y[vh_count] = veh[0].y;
vh_count++;
}
else
{
vehicle_update();
det_flag[VH_M-1] = detect_yet;
detect_yet = 0;
veh_omega_flag[VH_M-1] = veh[0].omega*PI/180.0;
vh_x[VH_M-1] = veh[0].x;
vh_y[VH_M-1] = veh[0].y;
}
} // veh_plot_cnt
} // not batch
// --------------------------------------------
} // if not vehicle done
} // for each vehicle
init_flag = 0;
}