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
}
