void Robot::drive() { Logger::log( __PRETTY_FUNCTION__); try { for (std::shared_ptr< AbstractSensor > sensor : sensors) { //sensor->setOn(); } if (speed == 0.0) { speed = 10.0; } GoalPtr goal = RobotWorld::getRobotWorld().getGoalByName( "Leon"); calculateRoute(goal); unsigned pathPoint = 0; while (position.x > 0 && position.x < 500 && position.y > 0 && position.y < 500 && pathPoint < path.size()) { const Vertex& vertex = path[pathPoint+=speed]; front = Vector( vertex.asPoint(), position); position.x = vertex.x; position.y = vertex.y; if (arrived(goal) || collision()) { notifyObservers(); break; } notifyObservers(); std::this_thread::sleep_for( std::chrono::milliseconds( 100)); // this should be either the last call in the loop or // part of the while. if(stop == true) { return; } } // while for (std::shared_ptr< AbstractSensor > sensor : sensors) { //sensor->setOff(); } } catch (std::exception& e) { std::cerr << __PRETTY_FUNCTION__ << ": " << e.what() << std::endl; } catch (...) { std::cerr << __PRETTY_FUNCTION__ << ": unknown exception" << std::endl; } }
static void do_something_server(char *wherenewline) { int n; *wherenewline = '\0'; if (match_arg(buf, "loc", &n)) { loc(n); } else if (match_arg(buf, "here", &n)) { here(n); } else if (match_arg(buf, "arr", &n)) { arrived(n); } else if (match_arg(buf, "dep", &n)) { departed(n); } else if (match_arg(buf, "poked", &n)) { pokedby(n); } else if (match_arg(buf, "said", &n)){ said(n); }else if (strcmp(buf, "ib") == 0) { saw_inventory_something = 0; printf("%s\n", lang_inv_heading); } else if (match_arg(buf, "i", &n)) { saw_inventory_something = 1; printf(" %s (#%d)\n", lang_thing[n], n); } else if (strcmp(buf, "ie") == 0) { if (!saw_inventory_something) printf("%s\n", lang_inv_nothing); } else if (strcmp(buf, "ok") == 0) { printf("%s\n", lang_ok); } else if (strcmp(buf, "ng") == 0) { printf("%s\n", lang_get_nosuch); } else if (strcmp(buf, "nd") == 0) { printf("%s\n", lang_drop_nosuch); } else if (strcmp(buf, "np") == 0) { printf("%s\n", lang_get_nosuch); } else if (match_arg(buf, "name", &n)) { char *p; if ((p = strchr(buf, ' ')) == NULL || (p = strchr(p + 1, ' ')) == NULL) fprintf(stderr, "error: malformed 'name' from server\n"); else storename(n, p + 1); } else if (match_arg(buf, "quit", &n)) { removename(n); } else if (strncmp(buf, "error ", 6) == 0) { printf("error from server: %s\n", buf + 6); } else { fprintf(stderr, "unexpected data from server: %s\n", buf); } n = wherenewline - buf; n++; if (bytes_in_buf > n && (buf[n] == '\r' || buf[n] == '\n')) n++; bytes_in_buf -= n; memmove(buf, buf + n, bytes_in_buf); }
Node* nextNode() { if (arrived()) return owner->dst.get(); assert(hops < owner->path.size()); Link* currLink = owner->path[hops].get(); // printf("nextNode src %p dst %p curr (%p %p)\n", // owner->src.get(), owner->dst.get(), currLink->a.get(), currLink->b.get()); if (currLink->a.get() == lastNode) return currLink->b.get(); else if (currLink->b.get() == lastNode) return currLink->a.get(); else assert(false && "Invalid lastNode, currLink association"); }
void Transport::step() { /* DT days elapse each step which makes DT*24 hours. m_speed is speed in km/h, so time traveled during each step is m_speed * DT*24 */ m_travelTimeLeft.addHours(-DT*24); if(!m_travelTimeLeft) { // zero travel time left CDPR(tr("Arrived at %1").arg(m_destination->getName())); emit arrived(m_destination); } else { qreal travelDist = m_speed*DT*24; // travel travelDist km towards destination City's Position m_position = m_position.moveTowards(m_destination->getPosition(), travelDist); } }
void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i,n; dBodyID b1,b2; // only collide things with the ground int g1 = (o1 == ground ); int g2 = (o2 == ground ); if (!(g1 ^ g2)) { //printf ("Ground colliding..\n"); //return; } b1 = dGeomGetBody(o1); b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnected (b1,b2)) return; if (b1 && isAction(b1) && b2 && (isType(b2,WALRUS) || (isType(b2,MANTA))) && isMineFire(gVehicle(b2),(Gunshot*)gVehicle(b1)) ) return; if (b2 && isAction(b2) && b1 && (isType(b1,WALRUS) || (isType(b1,MANTA))) && isMineFire(gVehicle(b1),(Gunshot*)gVehicle(b2)) ) return; int val[]={CARRIER,WALRUS,MANTA}; if (o1 && isRay(o1) && b2 && isType(b2,val,3) && rayHit(gVehicle(b2),(LaserRay*)gVehicle(o1))) {return;} if (o2 && isRay(o2) && b1 && isType(b1,val,3) && rayHit(gVehicle(b1),(LaserRay*)gVehicle(o2))) {return;} const int N = 10; dContact contact[N]; n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); if (n > 0) { for (i=0; i<n; i++) { Vehicle *v1=NULL,*v2=NULL; Structure *s1=NULL, *s2=NULL; gVehicle(v1,v2,dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2),s1,s2,contact[i].geom.g1,contact[i].geom.g2); // Bullets if ((isAction(v1) && isWalrus(v2) && isMineFire(v2,(Gunshot*)v1)) || (isAction(v2) && isWalrus(v1) && isMineFire(v1,(Gunshot*)v2))) { // Water buyoncy reaction contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; contact[i].surface.mu = 0.0f; contact[i].surface.slip1 = 1.0f; contact[i].surface.slip2 = 1.0f; contact[i].surface.soft_erp = 1.0f; // 0 in both will force the surface to be tight. contact[i].surface.soft_cfm = 1.0f; } else if ( isAction(v1) || isAction(v2)) { contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; contact[i].surface.mu = 0; contact[i].surface.slip1 = 0.1f; contact[i].surface.slip2 = 0.1f; if (isAction(v1) && isCarrier(v2) && hit(v2,(Gunshot*)v1)) {} if (isAction(v2) && isCarrier(v1) && hit(v1,(Gunshot*)v2)) {} if (isAction(v1) && isManta(v2) && hit(v2,(Gunshot*)v1)) {} if (isAction(v2) && isManta(v1) && hit(v1,(Gunshot*)v2)) {} if (isAction(v1) && isWalrus(v2) && hit(v2,(Gunshot*)v1)) {} if (isAction(v2) && isWalrus(v1) && hit(v1,(Gunshot*)v2)) {} if (isAction(v1) && s2 && hit(s2)) {} if (isAction(v2) && s1 && hit(s1)) {} } else if ( ( isManta(v1) && isCarrier(v2) && releasecontrol(v1) ) || ( isManta(v2) && isCarrier(v1) && releasecontrol(v2) ) ) { // Manta landing on Carrier contact[i].surface.mode = dContactBounce | dContactApprox1; contact[i].surface.mu = dInfinity; contact[i].surface.slip1 = 0.0f; contact[i].surface.slip2 = 0.0f; contact[i].surface.bounce = 0.2f; } else if (isRunway(s1) || isRunway(s2)) { // Manta landing on Runways. contact[i].surface.mode = dContactBounce | dContactApprox1; contact[i].surface.mu = 0.99f; contact[i].surface.slip1 = 0.9f; contact[i].surface.slip2 = 0.9f; contact[i].surface.bounce = 0.2f; if ( isRunway(s1) && isManta(v2) && landed(v2, s1->island)) {} if ( isRunway(s2) && isManta(v1) && landed(v1, s2->island)) {} } else if (isIsland(contact[i].geom.g1) || isIsland(contact[i].geom.g2)) { // Island reaction contact[i].surface.mode = dContactBounce | dContactApprox1; contact[i].surface.mu = 0; contact[i].surface.bounce = 0.2f; contact[i].surface.slip1 = 0.1f; contact[i].surface.slip2 = 0.1f; contact[i].surface.soft_erp = 0; // 0 in both will force the surface to be tight. contact[i].surface.soft_cfm = 0; // Carrier stranded and Walrus arrived on island. if (isIsland(contact[i].geom.g1) && isCarrier(v2) && stranded(v2,getIsland(contact[i].geom.g1))) {} if (isIsland(contact[i].geom.g2) && isCarrier(v1) && stranded(v1,getIsland(contact[i].geom.g2))) {} if (isIsland(contact[i].geom.g1) && isWalrus(v2) && arrived(v2,getIsland(contact[i].geom.g1))) {} if (isIsland(contact[i].geom.g2) && isWalrus(v1) && arrived(v1,getIsland(contact[i].geom.g2))) {} } else if (ground == contact[i].geom.g1 || ground == contact[i].geom.g2 ) { // Water buyoncy reaction contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; contact[i].surface.mu = 0.0f; contact[i].surface.slip1 = 0.1f; contact[i].surface.slip2 = 0.1f; contact[i].surface.soft_erp = .5f; // 0 in both will force the surface to be tight. contact[i].surface.soft_cfm = .3f; // Walrus reaching shore. if (ground == contact[i].geom.g1 && isWalrus(v2) && departed(v2)) {} if (ground == contact[i].geom.g2 && isWalrus(v1) && departed(v1)) {} if (ground == contact[i].geom.g1 && v2 && isManta(v2) && groundcollisions(v2)) {} if (ground == contact[i].geom.g2 && v1 && isManta(v1) && groundcollisions(v1)) {} } else { /** // Water buyoncy reaction contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; contact[i].surface.mu = 0.0f; contact[i].surface.slip1 = 0.1f; contact[i].surface.slip2 = 0.1f; contact[i].surface.soft_erp = .5f; // 0 in both will force the surface to be tight. contact[i].surface.soft_cfm = .3f; **/ if (v1 && isManta(v1) && groundcollisions(v1)) {} if (v2 && isManta(v2) && groundcollisions(v2)) {} } dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } } }
static void save_arrival(struct tfrc *state, struct timeval curr_time, uint16_t seq) { int kk, inc, last_jj; uint16_t udelta; uint32_t now; uint32_t ext_seq; static uint16_t last_seq; static uint32_t ext_last_ack; static int last_ack_jj = 0; gettimeofday(&curr_time, NULL); now = tv_diff_usec(curr_time, state->start_time); if (state->jj == -1) { /* first packet arrival */ state->jj = 0; last_seq = seq; ext_last_ack = seq; last_ack_jj = 0; arrival[state->jj].seq = seq; arrival[state->jj].ts = now; return; } udelta = seq - last_seq; state->total_pckts++; if (udelta < MAX_DROPOUT) { /* in order, with permissible gap */ if (seq < last_seq) { state->cycles++; } /* record arrival */ last_jj = state->jj; state->jj = (state->jj + udelta) % MAX_HISTORY; set_zero(last_jj, state->jj, udelta); ext_seq = seq + state->cycles * RTP_SEQ_MOD; last_seq = seq; arrival[state->jj].seq = ext_seq; arrival[state->jj].ts = now; if (udelta < 10) state->gap[udelta - 1]++; if ((ext_seq - ext_last_ack) == 1) { /* We got two consecutive packets, no loss */ ext_last_ack = ext_seq; last_ack_jj = state->jj; } else { /* Sequence number jumped, we've missed a packet for some reason */ if (arrived(last_ack_jj, state->jj) >= 4) { record_loss(state, ext_last_ack, ext_seq, arrival[last_ack_jj].ts, now); ext_last_ack = ext_seq; last_ack_jj = state->jj; } } } else if (udelta <= RTP_SEQ_MOD - MAX_MISORDER) { printf(" -- seq:%u last seq:%u ", seq, arrival[state->jj].seq); abort(); /* FIXME */ } else { /* duplicate or reordered packet */ ext_seq = seq + state->cycles * RTP_SEQ_MOD; state->ooo++; if (ext_seq > ext_last_ack) { inc = ext_seq - arrival[state->jj].seq; kk = (state->jj + inc) % MAX_HISTORY; if (arrival[kk].seq == 0) { arrival[kk].seq = ext_seq; arrival[kk].ts = (arrival[last_ack_jj].ts + now) / 2; /* NOT the best interpolation */ } while (arrival[last_ack_jj + 1].seq != 0 && last_ack_jj < state->jj) { last_ack_jj = (last_ack_jj + 1) % MAX_HISTORY; } ext_last_ack = arrival[last_ack_jj].seq; } } }