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