void main(int argc, char* argv[]) { Node * head = linkedListCreate(testHashMapOneValue); linkedListAdd(head, testHashMapSixValues); linkedListAdd(head, testHashMapTwelveValues); linkedListAdd(head, testHashMapGetMissingKey); linkedListAdd(head, testHashMapRemoveSixValues); Node * testNode = head; while (testNode != NULL) { ((void (*)(void))testNode->data)(); printf("\n"); testNode = testNode->next; } testNode = head; while(testNode != NULL) { testNode = linkedListRemove(head, testNode); } printf("%d asserts successful.\n", assertsGetTotal()); if (assertsGetFailures() > 0) { printf("%d asserts failed.\n", assertsGetFailures()); } else { printf("All tests successful.\n"); } return; }
static void aStar( const BNavmesh *navmesh, const BTriangle *startTriangle, const BTriangle *endTriangle, const BVector *destination, BAStarOutput *output ) { assert( navmesh != NULL ); assert( startTriangle != NULL ); assert( endTriangle != NULL ); assert( destination != NULL ); assert( output != NULL ); assert( isPointInsideNavmeshTriangle( navmesh, destination, endTriangle) ); assert( startTriangle->connectedComponent == endTriangle->connectedComponent ); BLinkedList openList, closedList; linkedListInit( &openList, sizeof( BAStarNode ), NULL ); linkedListInit( &closedList, sizeof( BAStarNode ), NULL ); BAStarNode startNode; startNode.cost = 0; startNode.previousTriangle = NULL; startNode.triangle = startTriangle; startNode.heuristic = heuristic( startNode.triangle, destination ); linkedListPrepend( &openList, &startNode ); BAStarNode arrivalNode; while ( 1 ) { BAStarNode current; assert( !linkedListIsEmpty( &openList ) ); // TODO Handle case where no path exists linkedListGetHead( &openList, ¤t ); linkedListRemoveHead( &openList ); linkedListPrepend( &closedList, ¤t ); if ( current.triangle == endTriangle ) { arrivalNode = current; break; } for ( int neighborIndex = 0; neighborIndex < 3; neighborIndex++ ) { const int neighborTriangleIndex = current.triangle->neighbours[neighborIndex]; assert( neighborTriangleIndex < navmesh->numTriangles ); if ( neighborTriangleIndex < 0 ) { continue; } const BTriangle *neighborTriangle = &navmesh->triangles[neighborTriangleIndex]; const float newCost = current.cost + movementCost( current.triangle, neighborTriangle ); BAStarNode occurenceInOpenList; int inOpenList = linkedListFind( &openList, isTriangle, ( void * ) neighborTriangle, &occurenceInOpenList ); if ( inOpenList ) { if ( newCost < occurenceInOpenList.cost ) { linkedListRemove( &openList, isTriangle, ( void * ) ¤t.triangle ); inOpenList = 0; } } const int inClosedList = linkedListFind( &closedList, isTriangle, ( void * ) neighborTriangle, NULL ); if ( !inOpenList && !inClosedList ) { BAStarNode newNode; newNode.cost = newCost; newNode.triangle = neighborTriangle; newNode.heuristic = heuristic( newNode.triangle, destination ); newNode.previousTriangle = current.triangle; linkedListInsertBefore( &openList, &newNode, hasBetterRank ); } } } output->numTriangles = 0; { const BTriangle *currentTriangle = endTriangle; while ( currentTriangle != NULL ) { assert( currentTriangle >= 0 ); BAStarNode node; verify( linkedListFind( &closedList, isTriangle, ( void * ) currentTriangle, &node ) ); currentTriangle = node.previousTriangle; output->numTriangles++; } } output->triangles = malloc( sizeof( BTriangle * ) * output->numTriangles ); { int nextPathTriangleIndex = output->numTriangles - 1; const BTriangle *currentTriangle = endTriangle; while ( currentTriangle != NULL ) { assert( currentTriangle >= 0 ); assert( nextPathTriangleIndex >= 0 ); assert( nextPathTriangleIndex < output->numTriangles ); output->triangles[nextPathTriangleIndex] = currentTriangle; BAStarNode node; verify( linkedListFind( &closedList, isTriangle, ( void * ) currentTriangle, &node ) ); currentTriangle = node.previousTriangle; nextPathTriangleIndex--; } } linkedListFree( &openList ); linkedListFree( &closedList ); assert( output->triangles[0] == startTriangle ); assert( output->triangles[output->numTriangles - 1] == endTriangle ); }