static int run(int ms) {
static unsigned long long int clock = 0;
static unsigned long long int temp_clock = 0;
int i, j;
// Computing the distance travelled by the robots
for (i = 0; i < ROBOTS; i++) {
const double *pos = wb_supervisor_field_get_sf_vec3f(robTrans[i]);
distance_travelled[i] += sqrt(pow(previous_x[i]-pos[0],2) + pow(previous_y[i] - pos[2], 2));
previous_x[i] = pos[0];
previous_y[i] = pos[2];
}
// Check if iteration over
if (temp_clock > MAX_TIME) {
// long long int duration = (clock - temp_clock) / 1000; // Duration in seconds
long long int duration = MAX_TIME / 1000; // Duration in seconds
// Events per unit of time
double metric1 = ((double) events_handled)/((double)duration); // Compute performance
// Total distance travelled per unit of time
double total_distance = 0;
for (i = 0; i < ROBOTS; i++) {
total_distance += distance_travelled[i];
distance_travelled[i] = 0;
}
double metric2 = total_distance / events_handled;
//mean_perf1 += metric1/MAX_DURATION;
//mean_perf2 += metric2/MAX_DURATION;
fprintf(outfile, "metric1(%d) = %f;\n", iterations, metric1);
fprintf(outfile, "metric2(%d) = %f;\n", iterations, metric2);
printf("Robots completed %d tasks in %llis, performance = %f \n", events_handled, duration, metric1);
printf("Robots travelled %f meters in %llis, performance = %f \n", total_distance, duration, metric2);
iterations++;
sleep(1);
events_handled = 0;
//temp_clock = clock;
temp_clock = 0;
reset();
}
// Stop simulating after a certain number of iterations
if (iterations > MAX_DURATION) {
//printf("Simulation terminated in %llis. Mean speed = %f. Mean distance per event = %f\n", clock/1000, mean_perf1, mean_perf2);
sleep(5);
fclose(outfile);
while(true);
}
/* Get data */
for (i=0;i<ROBOTS;i++) {
/* Test if a robot has bounced in an obstacle */
for (j=0;j<MAX_EVENTS;j++){
bool collision = are_colliding(robTrans[i], events[j].eventTrans);
/* Update the number of events and remove/replace the handled event*/
if(collision){
events[j].state -= EVENT_PRODUCTIVITY_STEP;
if (events[j].state<=0) {
// reset event
events[j].state = 1.0;
randomize_event_position(j);
randomize_event_color(j);
events_handled++;
//printf("%d events handled \n", events_handled);
}
}
}
}
clock+=STEP_SIZE;
temp_clock+=STEP_SIZE;
return STEP_SIZE;
}
void physics_update(float dt)
{
int i, j;
float dst, momentumToward, resolutionDistance;
for(i = 0; i < PHYSICS_OBJECT_COUNT; i++)
{
PhysicsObject* o = &physicsObjects[i];
// Update velocity
o->Velocity.x += o->Force.x * dt;
o->Velocity.y += o->Force.y * dt;
// Apply friction
o->Velocity.x = o->Velocity.x * o->Friction;
o->Velocity.y = o->Velocity.y * o->Friction;
// Update location
o->Location.x += o->Velocity.x * dt;
o->Location.y += o->Velocity.y * dt;
// Clear forces
o->Force.x = 0;
o->Force.y = 0;
}
// Collision checks and resolution
for(i = 0; i < PHYSICS_OBJECT_COUNT; i++)
{
PhysicsObject* a = &physicsObjects[i];
if(a->IsUsed)
{
for(j = i + 1; j < PHYSICS_OBJECT_COUNT; j++)
{
// Check whether objects are colliding
PhysicsObject* b = &physicsObjects[j];
Vector2F displacement;
if(b->IsUsed && are_colliding(a, b, &displacement))
{
// Normalize (=hit normal)
dst = sqrt(displacement.x*displacement.x + displacement.y*displacement.y);
displacement.x /= dst;
displacement.y /= dst;
// Find the distance required to seperate the circles
resolutionDistance = a->Radius + b->Radius - dst + 2;
a->Location.x += resolutionDistance * displacement.x / 2;
a->Location.y += resolutionDistance * displacement.y / 2;
b->Location.x -= resolutionDistance * displacement.x / 2;
b->Location.y -= resolutionDistance * displacement.y / 2;
// Ellastic collision
momentumToward = a->Mass * vector_dot(a->Velocity, displacement) - b->Mass * vector_dot(b->Velocity, displacement);
if(momentumToward > 0)
{
Vector2F impulse;
impulse.x = PHYSICS_RESTITUTION * momentumToward * displacement.x * 0.5;
impulse.y = PHYSICS_RESTITUTION * momentumToward * displacement.y * 0.5;
a->Velocity.x += impulse.x / a->Mass;
a->Velocity.y += impulse.y / a->Mass;
b->Velocity.x -= impulse.x / b->Mass;
b->Velocity.y -= impulse.y / b->Mass;
}
}
}
}
}
// Check the bounds for all objects
for(i = 0; i < PHYSICS_OBJECT_COUNT; i++)
{
PhysicsObject* a = &physicsObjects[i];
if(a->IsUsed)
{
check_bounds(a);
}
}
}
bool chunk_in_view(ne::transform3f view, world_chunk* chunk) {
return are_colliding(view, chunk->transform.position.xy, { chunk_pixel_width(), chunk_pixel_height() });
}
