// convenience function to accept Vector3f. Only x and y are adjusted
void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector3f &desired_vel)
{
Vector2f des_vel_xy(desired_vel.x, desired_vel.y);
adjust_velocity(kP, accel_cmss, des_vel_xy);
desired_vel.x = des_vel_xy.x;
desired_vel.y = des_vel_xy.y;
}
///////////////////////////////////////////////////////////////////////////////
/// Enforce the mass conservation by adjusting the outlet flow rate
///
/// The details ware published in the paper
/// "W. Zuo, J. Hu, Q. Chen 2010.
/// Improvements on FFD modeling by using different numerical schemes,
/// Numerical Heat Transfer, Part B Fundamentals, 58(1), 1-16."
///
///\param para Pointer to FFD parameters
///\param var Pointer to FFD simulation variables
///\param BINDEX Pointer to boundary index
///
///\return 0 if no error occurred
///////////////////////////////////////////////////////////////////////////////
int mass_conservation(PARA_DATA *para, REAL **var, int **BINDEX) {
int i, j, k;
int it;
int imax = para->geom->imax, jmax = para->geom->jmax;
int kmax = para->geom->kmax;
int index= para->geom->index;
int IMAX = imax+2, IJMAX = (imax+2)*(jmax+2);
REAL *u = var[VX], *v = var[VY], *w = var[VZ];
REAL dvel;
REAL *flagp = var[FLAGP];
dvel = adjust_velocity(para, var, BINDEX); //(mass_in-mass_out)/area_out
/*---------------------------------------------------------------------------
| Adjust the outflow
---------------------------------------------------------------------------*/
for(it=0;it<index;it++) {
i = BINDEX[0][it];
j = BINDEX[1][it];
k = BINDEX[2][it];
if(flagp[IX(i,j,k)]==2) {
if(i==0) u[IX(i,j,k)] -= dvel;
if(i==imax+1) u[IX(i-1,j,k)]+= dvel;
if(j==0) v[IX(i,j,k)] -= dvel;
if(j==jmax+1) v[IX(i,j-1,k)] += dvel;
if(k==0) w[IX(i,j,k)] -= dvel;
if(k==kmax+1) w[IX(i,j,k-1)] += dvel;
}
}
return 0;
} // End of mass_conservation()
void interact(particle& p0, particle& p1, time_t new_time) {
{
interaction_info info(p0, p1, new_time);
assert (info.discriminant_quarter >= 0);
p0.last_time = new_time;
p0.last_position = info.new_pos0;
p0.last_kinetic_energy = divide(info.total_momentum_sq*p0.mass - info.total_momentum*isqrt(info.discriminant_quarter*4) + p1.mass*(info.new_kinetic_energy*(p0.mass+p1.mass) - info.total_momentum_sq),
(p0.mass+p1.mass)*(p0.mass+p1.mass),
rounding_strategy<round_down, negative_continuous_with_positive>());
p0.last_energy_of_proximity_to_next = info.new_eop;
p1.last_time = new_time;
p1.last_position = info.new_pos1;
p1.last_kinetic_energy = info.new_kinetic_energy - p0.last_kinetic_energy;
adjust_velocity(p0);
adjust_velocity(p1);
}
class particles_interact : public event {
public:
particles_interact(entity_id id0, entity_id id0) : id0(id0),id1(id1) {}
entity_id id0;
entity_id id1;
void operator()(time_steward::accessor* accessor)const override {
particle& p0 = accessor->get_mut<particle>(accessor->get(id0));
particle& p1 = accessor->get_mut<particle>(accessor->get(id1));
interact(p0, p1);
}
};
class particle_repulsion_preparer : public trigger {
public:
player_moves_around(entity_id id0, entity_id id0) : id0(id0),id1(id1) {}
entity_id id0;
entity_id id1;
void operator()(time_steward::accessor* accessor)const override {
particle const& p0 = accessor->get<particle>(accessor->get(id0));
particle const& p1 = accessor->get<particle>(accessor->get(id1));
const time_t t = max(p0.last_time, p1.last_time);
const position_t pos0 = p0.last_position + p0.approx_velocity*(t-p0.last_time);
const position_t dp = pos1 - pos0;
const velocity_t dv = p1.approx_velocity - p0.approx_velocity;
time_t t2 = t-1;
if (dv > 0) {
t2 = t + divide(dp, dv*10, rounding_strategy<round_down, negative_is_forbidden>());
}
if (dv < 0) {
t2 = t + divide(dp, -dv*12, rounding_strategy<round_down, negative_is_forbidden>());
while (!can_interact(p0, p1, t2)) {
t2 = divide(t2+t, 2, rounding_strategy<round_down, negative_continuous_with_positive>());
}
}
if (t2 >= t) {
accessor->anticipate_event(t2, std::shared_ptr<event>(new particles_interact(id0, id1)));
}
}
};
class initialize_world : public event {
public:
void operator()(time_steward::accessor* accessor)const override {
std::vector<particle> particles;
const uint32_t num_particles = 4;
for (uint32_t i = 0; i < num_particles; ++i) {
particle p;
p.last_time = 0;
particles.push_back(p);
}
particles[0].mass = 1200 * grams * identity(mass_units/grams);
particles[0].last_position = 0 * meters * identity(position_units/meters);
supernaturally_set_velocity(particles[0], 10 * milli*meters/seconds * identity(velocity_units/(milli*meters/seconds)));
particles[1].mass = 1200 * grams * identity(mass_units/grams);
particles[1].last_position = 1 * meters * identity(position_units/meters);
supernaturally_set_velocity(particles[1], -10 * milli*meters/seconds * identity(velocity_units/(milli*meters/seconds)));
particles[2].mass = 1 * grams * identity(mass_units/grams);
particles[2].last_position = 2 * meters * identity(position_units/meters);
supernaturally_set_velocity(particles[2], 0 * milli*meters/seconds * identity(velocity_units/(milli*meters/seconds)));
particles[3].mass = 8000 * grams * identity(mass_units/grams);
particles[3].last_position = 20 * meters * identity(position_units/meters);
supernaturally_set_velocity(particles[3], -10000 * milli*meters/seconds * identity(velocity_units/(milli*meters/seconds)));
for (uint32_t i = 0; i < num_particles; ++i) {
entity_id id = siphash_id::combining(i);
entity_ref e = accessor->get(id);
particle& p = particles[i];
if (i+1 < num_particles) {
p.last_energy_of_proximity_to_next = energy_of_proximity(p.last_position, particles[i+1].last_kinetic_energy);
accessor->set_trigger(id, std::shared_ptr<trigger>(new player_could_hit_walls(id, siphash_id::combining(i+1))));
}
accessor->set<particle>(e, p);
}7
}
};
/*
* The task 'VehicleTask' updates the current velocity of the vehicle
*/
void VehicleTask(void* pdata)
{
INT8U err, err_tmr_1, err_tmr_start, err_sem;
void* msg;
INT8U* throttle;
INT8S acceleration; /* Value between 40 and -20 (4.0 m/s^2 and -2.0 m/s^2) */
INT8S retardation; /* Value between 20 and -10 (2.0 m/s^2 and -1.0 m/s^2) */
INT16U position = 0; /* Value between 0 and 20000 (0.0 m and 2000.0 m) */
INT16S wind_factor; /* Value between -10 and 20 (2.0 m/s^2 and -1.0 m/s^2) */
Sem_VehicleTask = OSSemCreate(0);
//OSSemPend(Sem_VehicleTask,0,&err_sem);
printf("Vehicle task created!\n");
SWTimer_VehicleTask = OSTmrCreate(0, VEHICLE_PERIOD, OS_TMR_OPT_PERIODIC, Tmr_Callback_Vehicle_Task, (void *)0,"S/W Timer 2",&err_tmr_1);
if(err_tmr_1 == OS_ERR_NONE){
if(DEBUG)
printf("Timer for VECHICLE TASK is created\n");
OSTmrStart(SWTimer_VehicleTask, &err_tmr_start);
if(err_tmr_start == OS_ERR_NONE){
if(DEBUG)
printf("Timer started in VEHICLE TASK \n");
}
else {
printf("Problem starting timer in VEHICLE TASK \n");
}
}
else {
printf("Error while creating Vehicle task timer \n");
if(err_tmr_1 == OS_ERR_TMR_INVALID_DLY)
printf(" Delay INVALID : VEHICLE TASK\n");
}
while(1)
{
// Wait for user inputs
OSSemPend(Sem_SwitchIO_IP , 0, &err_sem);
OSSemPend(Sem_ButtonIO_IP, 0, &err_sem);
if((brake_pedal == on) && (CUR_VELOCITY > 0)){
CUR_VELOCITY = CUR_VELOCITY - 10;
// if(CUR_VELOCITY < -200) // Max value for velocity
//CUR_VELOCITY = -200;
if(DEBUG_IO)
printf("********** Brake brake_pedal : %d ********** \n", CUR_VELOCITY);
}
err = OSMboxPost(Mbox_Velocity, (void *) &CUR_VELOCITY);
if(DEBUG)
printf("Vehicle task Posted MBoxPost_Velocity \n");
if(DEBUG)
printf("SEM ACCESS VEHICLE TASK\n\n");
OSSemPend(Sem_VehicleTask,0,&err_sem);
/* Non-blocking read of mailbox:
- message in mailbox: update throttle
- no message: use old throttle
*/
if(DEBUG)
printf("Vehicle task waiting for MBoxPost_Throttle for 1 unit time \n");
msg = OSMboxPend(Mbox_Throttle, 1, &err);
if (err == OS_NO_ERR)
throttle = (INT8U*) msg;
if(DEBUG)
printf("Vehicle task GOT MBoxPost_Throttle \n");
/* Retardation : Factor of Terrain and Wind Resistance */
if (CUR_VELOCITY > 0)
wind_factor = CUR_VELOCITY * CUR_VELOCITY / 10000 + 1;
else
wind_factor = (-1) * CUR_VELOCITY * CUR_VELOCITY / 10000 + 1;
if (position < 4000)
retardation = wind_factor; // even ground
else if (position < 8000)
retardation = wind_factor + 15; // traveling uphill
else if (position < 12000)
retardation = wind_factor + 25; // traveling steep uphill
else if (position < 16000)
retardation = wind_factor; // even ground
else if (position < 20000)
retardation = wind_factor - 10; //traveling downhill
else
retardation = wind_factor - 5 ; // traveling steep downhill
acceleration = *throttle / 2 - retardation;
position = adjust_position(position, CUR_VELOCITY, acceleration, 300);
CUR_VELOCITY = adjust_velocity(CUR_VELOCITY, acceleration, brake_pedal, 300);
printf("Position: %dm\n", position / 10);
printf("Velocity: %4.1fm/s\n", CUR_VELOCITY /10.0);
printf("Throttle: %dV Time : %d \n\n", *throttle / 10, (int) (alt_timestamp() / (alt_timestamp_freq()/1000)));
alt_timestamp_start();
/*
INT32U stk_size;
err = OSTaskStkChk(16, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used OverLoadDetection : %d \n", stk_size);
err = OSTaskStkChk(14, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used DYNAMIV_UTI : %d \n", stk_size);
err = OSTaskStkChk(12, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used CONTROLTASK: %d \n", stk_size);
err = OSTaskStkChk(10, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used VehicleTask: %d \n", stk_size);
err = OSTaskStkChk(8, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used SwitchIO: %d \n", stk_size);
err = OSTaskStkChk(7, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used ButtonIO: %d \n", stk_size);
err = OSTaskStkChk(6, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used WatchDog: %d \n", stk_size);
err = OSTaskStkChk(5, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used Start Task: %d \n", stk_size);
*/
show_velocity_on_sevenseg((INT8S) (CUR_VELOCITY / 10));
show_active_signals();
show_position(position);
// OSSemPend(Sem_VehicleTask,0,&err_sem);
}
}
/**
* Updates Tux's position taking into account gravity, friction, tree
* collisions, etc. This is the main physics function.
*/
void
update_player_pos(float timestep)
{
ppogl::Vec3d surf_nml; // normal vector of terrain
ppogl::Vec3d tmp_vel;
float speed;
float paddling_factor;
ppogl::Vec3d local_force;
float flap_factor;
pp::Plane surf_plane;
float dist_from_surface;
if(timestep > 2.0*EPS){
for(int i=0; i<GameMgr::getInstance().numPlayers; i++){
solve_ode_system(players[i], timestep);
}
}
for(int i=0; i<GameMgr::getInstance().numPlayers; i++){
Player& plyr=players[i];
tmp_vel = plyr.vel;
//Set position, orientation, generate particles
surf_plane = get_local_course_plane( plyr.pos );
surf_nml = surf_plane.nml;
dist_from_surface = surf_plane.distance( plyr.pos );
adjust_velocity( &plyr.vel, surf_plane );
adjust_position( &plyr.pos, surf_plane, dist_from_surface );
speed = tmp_vel.normalize();
set_tux_pos( plyr, plyr.pos );
adjust_orientation( plyr, timestep, plyr.vel,
dist_from_surface, surf_nml );
flap_factor = 0;
if(plyr.control.is_paddling){
double factor;
factor = (GameMgr::getInstance().time - plyr.control.paddle_time) / PADDLING_DURATION;
if(plyr.airborne){
paddling_factor = 0;
flap_factor = factor;
}else{
paddling_factor = factor;
flap_factor = 0;
}
}else{
paddling_factor = 0.0;
}
// calculate force in Tux's local coordinate system
local_force = plyr.orientation.conjugate().rotate(plyr.net_force);
if(plyr.control.jumping){
flap_factor = (GameMgr::getInstance().time - plyr.control.jump_start_time) / JUMP_FORCE_DURATION;
}
tux[i].adjustJoints( plyr.control.turn_animation, plyr.control.is_braking,
paddling_factor, speed, local_force, flap_factor );
}
}
