/* Решение дифференциального уравнения первого порядка методом Рунге-Кутта второго порядка точности */ void runge_kutta_2(struct grid_function *grid, double (*f)(struct point), double start) { double h = (grid->end - grid->start) / grid->num_pieces; for (int i = 0; i <= grid->num_pieces; i++) { if (i == 0) { /* Задание начального значения сеточной функции */ grid->values[0] = start; } else { /* Подсчёт точек предиктора и корректора */ struct point p_predict = grid_getPoint(grid, i - 1); struct point p_correct = p_predict; double correct = grid->values[i - 1] + f(p_predict) * h; p_correct.y = correct; p_correct.x += h; /* Вычисление текущего значения сеточной функции */ /* Формула для вычисления следующего значения: * y[i+1] = y[i] + ( f (x[i], y[i]) + f (x[i], p[i]) ) * h / 2, * где p[i] = y[i] + f (x[i], y[i]) * h */ grid->values[i] = grid->values[i - 1] + (f(p_predict) + f(p_correct)) * h / 2; } } }
/* ============================================================================= * traceToNeighbor * ============================================================================= */ static void traceToNeighbor (grid_t* myGridPtr, point_t* currPtr, point_t* movePtr, bool_t useMomentum, long bendCost, point_t* nextPtr) { long x = currPtr->x + movePtr->x; long y = currPtr->y + movePtr->y; long z = currPtr->z + movePtr->z; if (grid_isPointValid(myGridPtr, x, y, z) && !grid_isPointEmpty(myGridPtr, x, y, z) && !grid_isPointFull(myGridPtr, x, y, z)) { long value = grid_getPoint(myGridPtr, x, y, z); long b = 0; if (useMomentum && (currPtr->momentum != movePtr->momentum)) { b = bendCost; } if ((value + b) <= nextPtr->value) { /* '=' favors neighbors over current */ nextPtr->x = x; nextPtr->y = y; nextPtr->z = z; nextPtr->value = value; nextPtr->momentum = movePtr->momentum; } } }
/* Решение дифференциального уравнения первого порядка методом Рунге-Кутта четвёртого порядка точности */ void runge_kutta_4(struct grid_function *grid, double (*f)(struct point), double start) { double h = (grid->end - grid->start) / grid->num_pieces; for (int i = 0; i <= grid->num_pieces; i++) { if (i == 0) { /* Задание начального значения сеточной функции */ grid->values[0] = start; } else { struct point p_predict = grid_getPoint(grid, i - 1); double k1 = f(p_predict); double k2 = f((struct point) { .x = p_predict.x + h / 2, .y = p_predict.y + k1 * h / 2 }); double k3 = f((struct point) { .x = p_predict.x + h / 2, .y = p_predict.y + k2 * h / 2 }); double k4 = f((struct point) { .x = p_predict.x + h, .y = p_predict.y + k3 * h});
/* ============================================================================= * PdoTraceback * ============================================================================= */ static vector_t* PdoTraceback (grid_t* gridPtr, grid_t* myGridPtr, coordinate_t* dstPtr, long bendCost) { vector_t* pointVectorPtr = PVECTOR_ALLOC(1); assert(pointVectorPtr); point_t next; next.x = dstPtr->x; next.y = dstPtr->y; next.z = dstPtr->z; next.value = grid_getPoint(myGridPtr, next.x, next.y, next.z); next.momentum = MOMENTUM_ZERO; while (1) { long* gridPointPtr = grid_getPointRef(gridPtr, next.x, next.y, next.z); PVECTOR_PUSHBACK(pointVectorPtr, (void*)gridPointPtr); grid_setPoint(myGridPtr, next.x, next.y, next.z, GRID_POINT_FULL); /* Check if we are done */ if (next.value == 0) { break; } point_t curr = next; /* * Check 6 neighbors * * Potential Optimization: Only need to check 5 of these */ traceToNeighbor(myGridPtr, &curr, &MOVE_POSX, TRUE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_POSY, TRUE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_POSZ, TRUE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_NEGX, TRUE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_NEGY, TRUE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_NEGZ, TRUE, bendCost, &next); #if DEBUG printf("(%li, %li, %li)\n", next.x, next.y, next.z); #endif /* DEBUG */ /* * Because of bend costs, none of the neighbors may appear to be closer. * In this case, pick a neighbor while ignoring momentum. */ if ((curr.x == next.x) && (curr.y == next.y) && (curr.z == next.z)) { next.value = curr.value; traceToNeighbor(myGridPtr, &curr, &MOVE_POSX, FALSE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_POSY, FALSE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_POSZ, FALSE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_NEGX, FALSE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_NEGY, FALSE, bendCost, &next); traceToNeighbor(myGridPtr, &curr, &MOVE_NEGZ, FALSE, bendCost, &next); if ((curr.x == next.x) && (curr.y == next.y) && (curr.z == next.z)) { PVECTOR_FREE(pointVectorPtr); #if DEBUG puts("[dead]"); #endif return NULL; /* cannot find path */ } } } #if DEBUG puts(""); #endif /* DEBUG */ return pointVectorPtr; }
/* ============================================================================= * grid_isPointFull * ============================================================================= */ bool_t grid_isPointFull (grid_t* gridPtr, long x, long y, long z) { long value = grid_getPoint(gridPtr, x, y, z); return ((value == GRID_POINT_FULL) ? TRUE : FALSE); }
/* ============================================================================= * grid_isPointEmpty * ============================================================================= */ bool_t grid_isPointEmpty (grid_t* gridPtr, long x, long y, long z) { long value = grid_getPoint(gridPtr, x, y, z); return ((value == GRID_POINT_EMPTY) ? TRUE : FALSE); }
/* ============================================================================= * grid_isPointFull * ============================================================================= */ __attribute__((transaction_safe)) bool_t grid_isPointFull (grid_t* gridPtr, long x, long y, long z) { long value = grid_getPoint(gridPtr, x, y, z); return ((value == GRID_POINT_FULL) ? TRUE : FALSE); }
/* ============================================================================= * maze_checkPaths * ============================================================================= */ bool_t maze_checkPaths (maze_t* mazePtr, list_t* pathVectorListPtr, bool_t doPrintPaths) { grid_t* gridPtr = mazePtr->gridPtr; long width = gridPtr->width; long height = gridPtr->height; long depth = gridPtr->depth; long i; /* Mark walls */ grid_t* testGridPtr = grid_alloc(width, height, depth); grid_addPath(testGridPtr, mazePtr->wallVectorPtr); /* Mark sources */ vector_t* srcVectorPtr = mazePtr->srcVectorPtr; long numSrc = vector_getSize(srcVectorPtr); for (i = 0; i < numSrc; i++) { coordinate_t* srcPtr = (coordinate_t*)vector_at(srcVectorPtr, i); grid_setPoint(testGridPtr, srcPtr->x, srcPtr->y, srcPtr->z, 0); } /* Mark destinations */ vector_t* dstVectorPtr = mazePtr->dstVectorPtr; long numDst = vector_getSize(dstVectorPtr); for (i = 0; i < numDst; i++) { coordinate_t* dstPtr = (coordinate_t*)vector_at(dstVectorPtr, i); grid_setPoint(testGridPtr, dstPtr->x, dstPtr->y, dstPtr->z, 0); } /* Make sure path is contiguous and does not overlap */ long id = 0; list_iter_t it; list_iter_reset(&it, pathVectorListPtr); while (list_iter_hasNext(&it)) { vector_t* pathVectorPtr = (vector_t*)list_iter_next(&it); long numPath = vector_getSize(pathVectorPtr); long i; for (i = 0; i < numPath; i++) { id++; vector_t* pointVectorPtr = (vector_t*)vector_at(pathVectorPtr, i); /* Check start */ long* prevGridPointPtr = (long*)vector_at(pointVectorPtr, 0); long x; long y; long z; grid_getPointIndices(gridPtr, prevGridPointPtr, &x, &y, &z); if (grid_getPoint(testGridPtr, x, y, z) != 0) { grid_free(testGridPtr); return FALSE; } coordinate_t prevCoordinate; grid_getPointIndices(gridPtr, prevGridPointPtr, &prevCoordinate.x, &prevCoordinate.y, &prevCoordinate.z); long numPoint = vector_getSize(pointVectorPtr); long j; for (j = 1; j < (numPoint-1); j++) { /* no need to check endpoints */ long* currGridPointPtr = (long*)vector_at(pointVectorPtr, j); coordinate_t currCoordinate; grid_getPointIndices(gridPtr, currGridPointPtr, &currCoordinate.x, &currCoordinate.y, &currCoordinate.z); if (!coordinate_areAdjacent(&currCoordinate, &prevCoordinate)) { grid_free(testGridPtr); return FALSE; } prevCoordinate = currCoordinate; long x = currCoordinate.x; long y = currCoordinate.y; long z = currCoordinate.z; if (grid_getPoint(testGridPtr, x, y, z) != GRID_POINT_EMPTY) { grid_free(testGridPtr); return FALSE; } else { grid_setPoint(testGridPtr, x, y, z, id); } } /* Check end */ long* lastGridPointPtr = (long*)vector_at(pointVectorPtr, j); grid_getPointIndices(gridPtr, lastGridPointPtr, &x, &y, &z); if (grid_getPoint(testGridPtr, x, y, z) != 0) { grid_free(testGridPtr); return FALSE; } } /* iteratate over pathVector */ } /* iterate over pathVectorList */ if (doPrintPaths) { puts("\nRouted Maze:"); grid_print(testGridPtr); } grid_free(testGridPtr); return TRUE; }