void
Alarm_init(void) {
assert(alarmHeap == NULL);
alarmHeap = Heap_new((HeapValue_Comparator) Alarm_compare,
4, 4, 0.8);
}
List doPathfinding(Map map, Car cars[3])
{
Heap openSet = Heap_new(State_compare);
List closedSet = List_new();
List finalPath = List_new();
List neighbors;
Position neighbor;
State state, newState;
Vector newSpeed, acceleration;
int end = 0, i, j, useBoost, positionTaken, distance;
float cost;
LOGINFO("A* doin' da werk!");
state = State_new(Car_getPosition(cars[0]), Car_getSpeed(cars[0]), Car_getBoosts(cars[0]), map);
Heap_insert(openSet, state);
while(!Heap_isEmpty(openSet) && !end)
{
state = Heap_extractMin(openSet);
if(Map_getTile(map, State_getPosition(state)->x, State_getPosition(state)->y) == ARRIVAL)
{
end = 1;
break;
}
distance = Map_getDistance(map, State_getPosition(state)->x, State_getPosition(state)->y);
neighbors = Map_getReachablePositions(map, State_getPosition(state), State_getSpeed(state), State_getBoosts(state));
List_foreach(neighbors, neighbor, i)
{
if(Map_getDistance(map, neighbor->x, neighbor->y) > distance)
{
Position_delete(neighbor);
continue;
}
cost = State_getRealCost(state) + 1;
newSpeed = Position_findOffset(State_getPosition(state), neighbor);
acceleration = Vector_copy(newSpeed);
Vector_substract(acceleration, State_getSpeed(state));
useBoost = 0;
positionTaken = 0;
if(Vector_squaredLength(acceleration) > 2)
{
useBoost = 1;
}
for(j = 1; j < 3; j++)
{
if(Position_equal(neighbor, Car_getPosition(cars[j])))
{
positionTaken = 1;
}
}
if(!positionTaken)
{
newState = State_new(neighbor, newSpeed, State_getBoosts(state) - useBoost, map);
State_setRealCost(newState, cost);
State_setParent(newState, state);
Heap_insert(openSet, newState);
}
Vector_delete(newSpeed);
Vector_delete(acceleration);
Position_delete(neighbor);
}
List_insert(closedSet, state);
List_empty(neighbors);
List_delete(neighbors);
}
while(state != NULL)
{
List_insert(finalPath, Position_copy(State_getPosition(state)));
state = State_getParent(state);
}
List_head(closedSet);
while(!List_isEmpty(closedSet))
{
state = List_getCurrent(closedSet);
List_remove(closedSet);
State_delete(state);
}
List_delete(closedSet);
while((state = Heap_extractMin(openSet)) != NULL)
{
State_delete(state);
}
Heap_delete(openSet);
LOGINFO("A* is done mate");
return finalPath;
}
