/* Решение дифференциального уравнения первого порядка методом Рунге-Кутта второго порядка точности */
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;
}
