uint8_t
path_get_next (vect_t *p)
{
if (path.found)
{
assert (path.get != PATH_DST_NODE_INDEX);
uint8_t prev = path.get;
vect_t pp;
path_pos (prev, &pp);
uint8_t next = path.astar_nodes[path.get].prev;
path.get = next;
path_pos (next, p);
while (next != 0xff)
{
/* Try to remove useless points. */
uint8_t next = path.astar_nodes[path.get].prev;
if (next == 0xff || next == PATH_DST_NODE_INDEX)
break;
vect_t np;
path_pos (next, &np);
vect_t vnp = np; vect_sub (&vnp, &pp);
vect_t vp = *p; vect_sub (&vp, &pp);
if (vect_normal_dot_product (&vp, &vnp) == 0)
{
path.get = next;
*p = np;
}
else
break;
}
return 1;
}
else
return 0;
}
/** Update the cache of blocked nodes. */
static void
path_blocked_update (void)
{
uint8_t i, j;
for (i = 0; i < PATH_GRID_NODES_NB; i++)
{
uint8_t valid = 1;
/* First, gather information from tables. */
if (!path_nodes[i].usable
|| food_blocking (path_nodes[i].carry_corn))
valid = 0;
else
{
vect_t pos;
path_pos (i, &pos);
/* Then, test for obstacles. */
for (j = 0; j < PATH_OBSTACLES_NB; j++)
{
if (path.obstacles[j].valid)
{
vect_t v = pos; vect_sub (&v, &path.obstacles[j].c);
uint32_t dsq = vect_dot_product (&v, &v);
uint32_t r = path.obstacles[j].r;
if (dsq <= r * r)
{
valid = 0;
break;
}
}
}
}
/* Update cache. */
path.valid[i] = valid;
}
}
uint16_t
path_astar_heuristic_callback (uint8_t node)
{
/* TODO: a better and faster heuristic can be found, considering that
* movement is only allowed on the grid. */
vect_t pos;
path_pos (node, &pos);
return distance_point_point (&pos, &path.endpoints[0]);
}
void
path_update (void)
{
path_blocked_update ();
path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX,
PATH_SRC_NODE_INDEX);
path.get = PATH_SRC_NODE_INDEX;
#if AC_PATH_REPORT
if (path.found)
{
uint8_t n, len = 0;
vect_t points[PATH_NODES_NB];
for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev)
path_pos (n, &points[len++]);
path_pos (n, &points[len++]);
AC_PATH_REPORT_CALLBACK (points, len, path.obstacles,
PATH_OBSTACLES_NB);
}
#endif
}
static uint8_t
path_element_blocking (uint8_t node, uint8_t escape)
{
vect_t pos;
path_pos (node, &pos);
int16_t square_x = (pos.x - 450 - 1) / 350;
int16_t square_y = (2100 - pos.y - 1) / 350;
uint8_t element_id = ELEMENT_UNLOAD_START + square_x + 6 * square_y;
if (element_blocking (element_id, escape))
return 1;
uint8_t intersection = ((pos.x - 450) / 350) != square_x;
if (intersection)
{
if (element_blocking (element_id + 1, escape))
return 1;
if (element_blocking (element_id + 6, escape))
return 1;
if (element_blocking (element_id + 6 + 1, escape))
return 1;
}
return 0;
}
/** Return 1 if the direct path between a and b nodes is blocked, also compute
* distance. */
static uint8_t
path_blocking (uint8_t a, uint8_t b, int16_t *dp)
{
uint8_t i;
vect_t va;
vect_t vb;
uint8_t escape_factor = 0;
if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX)
escape_factor = path.escape_factor;
path_pos (a, &va);
path_pos (b, &vb);
/* Test for a blocking obstacle. */
for (i = 0; i < PATH_OBSTACLES_NB; i++)
{
if (path.obstacles[i].valid)
{
uint16_t d = distance_segment_point (&va, &vb,
&path.obstacles[i].c);
if (d < path.obstacles[i].r)
{
if (escape_factor)
{
int16_t d = distance_point_point (&va, &vb);
*dp = d * escape_factor;
return 0;
}
else
return 1;
}
}
}
/* Test for a blocking food. */
int16_t d = distance_point_point (&va, &vb);
if (d == 0)
{
*dp = 0;
return 0;
}
else if (food_blocking_path (va, vb, d))
{
if (escape_factor)
{
*dp = d * escape_factor;
return 0;
}
else
return 1;
}
/* Test for the wall. */
if (va.x < BOT_SIZE_RADIUS || va.x >= PG_WIDTH - BOT_SIZE_RADIUS
|| vb.x < BOT_SIZE_RADIUS || vb.x >= PG_WIDTH - BOT_SIZE_RADIUS)
{
int16_t dx = va.x - vb.x;
int16_t dy = va.y - vb.y;
/* Do not authorise path going parallel to the wall. */
if (UTILS_ABS (dx) < UTILS_ABS (dy))
{
if (escape_factor)
{
*dp = d * escape_factor;
return 0;
}
else
return 1;
}
}
/* No blocking. */
*dp = d;
return 0;
}
/**
* Build a path from #_path_builder xpos/ypos to the mouse cursor position.
* @param mousexy Mouse position.
*/
void PathBuildManager::ComputeNewLongPath(const Point32 &mousexy)
{
static const TrackSlope slope_prios_down[] = {TSL_DOWN, TSL_FLAT, TSL_UP, TSL_INVALID}; // Order of preference when going down.
static const TrackSlope slope_prios_flat[] = {TSL_FLAT, TSL_UP, TSL_DOWN, TSL_INVALID}; // Order of preference when at the right height.
static const TrackSlope slope_prios_up[] = {TSL_UP, TSL_FLAT, TSL_DOWN, TSL_INVALID}; // Order of preference when going up.
Viewport *vp = GetViewport();
if (vp == nullptr) return;
int c1, c2, c3;
switch (vp->orientation) {
case VOR_NORTH: c1 = 1; c2 = 2; c3 = 2; break;
case VOR_EAST: c1 = -1; c2 = -2; c3 = 2; break;
case VOR_SOUTH: c1 = 1; c2 = -2; c3 = -2; break;
case VOR_WEST: c1 = -1; c2 = 2; c3 = -2; break;
default: NOT_REACHED();
}
XYZPoint16 path_pos(0, 0, 0);
path_pos.y = this->pos.y * 256 + 128;
int32 lambda_y = path_pos.y - mousexy.y; // Distance to constant Y plane at current tile cursor.
path_pos.x = this->pos.x * 256 + 128;
int32 lambda_x = path_pos.x - mousexy.x; // Distance to constant X plane at current tile cursor.
if (abs(lambda_x) < abs(lambda_y)) {
/* X constant. */
path_pos.x /= 256;
path_pos.y = Clamp<int32>(mousexy.y + c1 * lambda_x, 0, _world.GetYSize() * 256 - 1) / 256;
path_pos.z = Clamp<int32>(vp->view_pos.z + c3 * lambda_x, 0, WORLD_Z_SIZE * 256 - 1) / 256;
} else {
/* Y constant. */
path_pos.x = Clamp<int32>(mousexy.x + c1 * lambda_y, 0, _world.GetXSize() * 256 - 1) / 256;
path_pos.y /= 256;
path_pos.z = Clamp<int32>(vp->view_pos.z + c2 * lambda_y, 0, WORLD_Z_SIZE * 256 - 1) / 256;
}
if (this->long_pos != path_pos) {
this->long_pos = path_pos;
_additions.Clear();
path_pos = this->pos;
/* Find the right direction from the selected tile to the current cursor location. */
TileEdge direction;
Point16 dxy;
for (direction = EDGE_BEGIN; direction < EDGE_COUNT; direction++) {
dxy = _tile_dxy[direction];
if (!GoodDirection(dxy.x, path_pos.x, this->long_pos.x) || !GoodDirection(dxy.y, path_pos.y, this->long_pos.y)) continue;
break;
}
if (direction == EDGE_COUNT) return;
/* 'Walk' to the cursor as long as possible. */
while (path_pos.x != this->long_pos.x || path_pos.y != this->long_pos.y) {
uint8 slopes = CanBuildPathFromEdge(path_pos, direction);
const TrackSlope *slope_prio;
/* Get order of slope preference. */
if (path_pos.z > this->long_pos.z) {
slope_prio = slope_prios_down;
} else if (path_pos.z == this->long_pos.z) {
slope_prio = slope_prios_flat;
} else {
slope_prio = slope_prios_up;
}
/* Find best slope, and take it. */
while (*slope_prio != TSL_INVALID && (slopes & (1 << *slope_prio)) == 0) slope_prio++;
if (*slope_prio == TSL_INVALID) break;
path_pos.x += dxy.x;
path_pos.y += dxy.y;
const Voxel *v = _world.GetVoxel(path_pos);
if (v != nullptr && HasValidPath(v)) {
if (!ChangePath(path_pos, this->path_type, false)) break;
if (*slope_prio == TSL_UP) path_pos.z++;
} else {
if (*slope_prio == TSL_UP) {
if (!BuildUpwardPath(path_pos, static_cast<TileEdge>((direction + 2) & 3), this->path_type, false)) break;
path_pos.z++;
} else if (*slope_prio == TSL_DOWN) {
v = _world.GetVoxel(path_pos + XYZPoint16(0, 0, -1));
if (v != nullptr && HasValidPath(v)) {
if (!ChangePath(path_pos + XYZPoint16(0, 0, -1), this->path_type, false)) break;
} else {
if (!BuildDownwardPath(path_pos, static_cast<TileEdge>((direction + 2) & 3), this->path_type, false)) break;
}
path_pos.z--;
} else {
if (!BuildFlatPath(path_pos, this->path_type, false)) break;
}
}
}
vp->EnableWorldAdditions();
vp->EnsureAdditionsAreVisible();
}
}
/** Return 1 if the direct path between a and b nodes is blocked, also compute
* distance. */
static uint8_t
path_blocking (uint8_t a, uint8_t b, int16_t *dp)
{
uint8_t i;
vect_t va;
vect_t vb;
uint8_t escape_factor = 0;
uint8_t factor = 1;
uint8_t blocking = 0;
if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX)
escape_factor = path.escape_factor;
path_pos (a, &va);
path_pos (b, &vb);
/* Test for green zone. */
uint8_t a_green, b_green;
a_green = va.x < PG_GREEN_WIDTH_MM || va.x > PG_WIDTH - PG_GREEN_WIDTH_MM;
b_green = vb.x < PG_GREEN_WIDTH_MM || vb.x > PG_WIDTH - PG_GREEN_WIDTH_MM;
if ((va.x < BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
&& vb.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)
|| (va.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
&& vb.x < BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)
|| (va.x > PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
&& vb.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)
|| (va.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
&& vb.x > PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM))
return 1;
if (a_green && b_green)
return 1;
if (a_green || b_green)
factor = 4;
/* Test for protected zone. */
if (va.y <= 350 && va.x > PG_WIDTH / 2 - 350 && va.y < PG_WIDTH / 2 + 350
&& (vb.x < PG_WIDTH / 2 - 350 || vb.x > PG_WIDTH / 2 + 350))
return 1;
if (vb.y <= 350 && vb.x > PG_WIDTH / 2 - 350 && vb.y < PG_WIDTH / 2 + 350
&& (va.x < PG_WIDTH / 2 - 350 || va.x > PG_WIDTH / 2 + 350))
return 1;
/* Test for a blocking obstacle. */
for (i = 0; i < PATH_OBSTACLES_NB && !blocking; i++)
{
if (path.obstacles[i].valid)
{
uint16_t d = distance_segment_point (&va, &vb,
&path.obstacles[i].c);
if (d < path.obstacles[i].r)
blocking = 1;
}
}
/* Compute distance. */
int16_t d = distance_point_point (&va, &vb);
if (d == 0)
{
*dp = 0;
return 0;
}
/* Test for a blocking element. */
if (element_blocking_path (va, vb, d, path.escape_factor))
blocking = 1;
/* Handle escaping. */
if (blocking)
{
if (escape_factor)
{
*dp = d * escape_factor;
return 0;
}
else
return 1;
}
/* No blocking. */
*dp = d * factor;
return 0;
}
