void Magnet::pull(Entity* b) { double distance = distBetween(center, ((Ball*)b)->getCenter()); if(distance > maxDistance) return; double force = (magneticForce - (magneticForce * (distance - getRadius() - b->getRadius()) / maxDistance)); double angle = calcAngle(b->getCenter(),getCenter()); double xForce = ( force * cos(angle)); double yForce = ( force * sin(angle)); if(!linearForce) { int sign = 1; if(xForce < 0) sign = -1; xForce = xForce * xForce * sign; sign = 1; if(yForce < 0) sign = -1; yForce = yForce * yForce * sign; } ((Ball*)b)->setXVel( ((Ball*)b)->getXVel() + xForce * window->timedMovement()); ((Ball*)b)->setYVel( ((Ball*)b)->getYVel() + yForce * window->timedMovement()); ((Ball*)b)->movedByMagnet(); }
// Body of action ArActionDesired * wander::fire(ArActionDesired d) { frontSonar = myRobot->getClosestSonarRange(-20, 20); speed = FULL_SPEED; currentDistance = distBetween(startWanderingX, startWanderingY, myRobot->getX(), myRobot->getY()); if ((frontSonar > 1500) && (currentDistance > WANDERINGDistance)) { printf("\nTurning RANDOMLY \n"); //Decreasing speed before making the turn speed = 0; deltaHeading = randomHeading; if (randomNoBetween(0, 1) == 0) { deltaHeading = -deltaHeading; } if (startedRandom.mSecSince() > 2000) { startWanderingX = myRobot->getX(); startWanderingY = myRobot->getY(); WANDERINGDistance = rand() % (1500 - 500 + 1) + 500; } } desiredState.reset(); // reset the desired state (must be done) desiredState.setVel(speed); // set the speed of the robot in the desired state desiredState.setDeltaHeading(deltaHeading); // set the new angle //printf("\n DeltaHeading ===== %d \n", deltaHeading); //printf("%f %f %f\n", myRobot->getX(), myRobot->getY(), myRobot->getTh()); //printf("\n my speed = %i", speed); //printf("\n current Distance %d \n wandering Distance %d", currentDistance, WANDERINGDistance); return &desiredState; // give the desired state to the robot for actionin }
Coor aStar(int xDep, int yDep , int xArr, int yArr, Point list) { int startId,goalId; calculDepartArrivee(list,xDep,yDep,xArr,yArr,&startId,&goalId);/// int danger; int x,y,yr;//varialble d'id pas de coordonées int tentativeGscore,tentativeIsBetter; Point p =NULL; ListeFils f = NULL; Set presqueResultat = NULL; Coor resultat = NULL; Set closedSet = NULL; Set openSet = NULL; addSet(&openSet , startId); Came cameFrom = NULL; Data data = NULL; data = setGscore(startId, 0 ,data); data = setHscore(startId, heuristicCostEstimate(startId,goalId,list),data); data = setFscore(startId, data->g + data->h, data); while (openSet != NULL) { x = lowestFscore(openSet,data); if ( x == goalId) { presqueResultat = reconstruireChemin(cameFrom,startId,goalId); resultat = setToCoor(presqueResultat,list); resultat = simplifierResultat(resultat); addCoor(&resultat,0,xArr,yArr); resultat = inverser(resultat); return resultat; } openSet = removeIdSet(openSet,x); addSet(&closedSet, x); p = sePlacerEn(x,list); for ( f = p->lPt ; f != NULL ; f = f->queue )//pour tout les y voisins de x { y = f->id; yr = y; if ( y > 1000) { y = y-1000; } if ( isInSet(closedSet,y) || p->enable == 0) { continue; } tentativeGscore = getGscore(x,data) + distBetween(x,yr); if ( isInSet(openSet,y) == 0) { addSet(&openSet,y); data = setHscore(y,heuristicCostEstimate(y,goalId,list),data); tentativeIsBetter = 1; } else if (tentativeGscore <= getGscore(y,data)) { tentativeIsBetter = 1; } else { tentativeIsBetter = 0; } if ( tentativeIsBetter == 1) { cameFrom = setCameFrom( y , x , cameFrom); data = setGscore(y,tentativeGscore,data); danger = p->enable; if (danger == 3) danger = 0; else if (danger == 2) danger = 100; else if (danger == 1) danger = 500; data = setFscore(y,getGscore(y,data) + getHscore(y,data) + danger, data); } } } return NULL;// = erreur }