inline void ExpandNodeFromNode(struct Map * themap,struct Path * route,unsigned int from_node,unsigned int to_node,unsigned char current_heading,unsigned int turning_penalty)
{
unsigned int x=from_node % themap->world_size_x; // Ypologizoume tin syntetagmeni x , y
unsigned int y=from_node / themap->world_size_x; // Ypologizoume tin syntetagmeni x , y
unsigned int new_heuristic=ManhattanDistance(x,y,route->target_x,route->target_y);
unsigned int new_movement_cost=ManhattanDistance(x,y,route->target_x,route->target_y);
unsigned int new_score=themap->world[to_node].movement_cost+themap->world[to_node].heuristic;//=themap->world[from_node].score;
unsigned int turning_movement_overhead=GetHeadingChangeOverhead(current_heading,themap->world[from_node].arrived_direction);
new_movement_cost=themap->world[from_node].movement_cost + turning_movement_overhead + GetHeadingMovementOverhead(current_heading);
new_score = new_movement_cost + new_heuristic;
if ((new_score<themap->world[to_node].score) || (themap->world[to_node].score==0) )
{
//Antikathistoume ton parent tou komvou to_node me ton from_node afou yparxei kalytero monopati!..!
themap->world[to_node].parent_node = from_node;
themap->world[to_node].score = new_score;
themap->world[to_node].movement_cost=new_movement_cost;
themap->world[to_node].heuristic=new_heuristic;
themap->world[to_node].arrived_direction = current_heading;
}
}
/** Search a sub-tree for the element nearest to a given point */
node_distance FindNearestRecursive(CoordT xy[2], size_t node_idx, int level) const
{
/* Dimension index of current level */
int dim = level % 2;
/* Node reference */
const node &n = this->nodes[node_idx];
/* Coordinate of element splitting at this node */
CoordT c = this->xyfunc(n.element, dim);
/* This node's distance to target */
DistT thisdist = ManhattanDistance(n.element, xy[0], xy[1]);
/* Assume this node is the best choice for now */
node_distance best = std::make_pair(n.element, thisdist);
/* Next node to visit */
size_t next = (xy[dim] < c) ? n.left : n.right;
if (next != INVALID_NODE) {
/* Check if there is a better node down the tree */
best = SelectNearestNodeDistance(best, this->FindNearestRecursive(xy, next, level + 1));
}
/* Check if the distance from current best is worse than distance from target to splitting line,
* if it is we also need to check the other side of the split. */
size_t opposite = (xy[dim] >= c) ? n.left : n.right; // reverse of above
if (opposite != INVALID_NODE && best.second >= abs((int)xy[dim] - (int)c)) {
node_distance other_candidate = this->FindNearestRecursive(xy, opposite, level + 1);
best = SelectNearestNodeDistance(best, other_candidate);
}
return best;
}
void COORD::UnitTest()
{
assert(COORD(3, 3) + COORD(2, 2) == COORD(5, 5));
COORD coord(5, 2);
coord += COORD(2, 5);
assert(coord == COORD(7, 7));
assert(COORD(2, 2) + North == COORD(2, 3));
assert(COORD(2, 2) + East == COORD(3, 2));
assert(COORD(2, 2) + South == COORD(2, 1));
assert(COORD(2, 2) + West == COORD(1, 2));
assert(Compass[E_NORTH] == North);
assert(Compass[E_EAST] == East);
assert(Compass[E_WEST] == West);
assert(Compass[E_SOUTH] == South);
assert(Clockwise(E_NORTH) == E_EAST);
assert(Clockwise(E_EAST) == E_SOUTH);
assert(Clockwise(E_SOUTH) == E_WEST);
assert(Clockwise(E_WEST) == E_NORTH);
assert(Opposite(E_NORTH) == E_SOUTH);
assert(Opposite(E_EAST) == E_WEST);
assert(Opposite(E_SOUTH) == E_NORTH);
assert(Opposite(E_WEST) == E_EAST);
assert(Anticlockwise(E_NORTH) == E_WEST);
assert(Anticlockwise(E_EAST) == E_NORTH);
assert(Anticlockwise(E_SOUTH) == E_EAST);
assert(Anticlockwise(E_WEST) == E_SOUTH);
assert(ManhattanDistance(COORD(3, 2), COORD(-4, -7)) == 16);
assert(DirectionalDistance(COORD(3, 2), COORD(-4, -7), E_NORTH) == -9);
assert(DirectionalDistance(COORD(3, 2), COORD(-4, -7), E_EAST) == -7);
assert(DirectionalDistance(COORD(3, 2), COORD(-4, -7), E_SOUTH) == 9);
assert(DirectionalDistance(COORD(3, 2), COORD(-4, -7), E_WEST) == 7);
}
int DistanceTo(const State& goal) const {
int total_distance = 0;
for (int goal_cell = 0; goal_cell < N; ++goal_cell) {
const int v = goal.a_[goal_cell];
int our_cell = 0;
for (; our_cell < N && a_[our_cell] != v; ++our_cell) {
}
total_distance += ManhattanDistance(goal_cell, our_cell);
}
return total_distance;
}
AStarNode::AStarNode(sf::Vector2f position, sf::Vector2f goal, int moveCost, AStarNode *parent)
:m_position(position), m_goal(goal), m_parent(parent), m_moveCost(moveCost)
{
// Set G value
if(m_parent)
{
m_G = m_parent->GetG()+moveCost;
}
else
{
m_G = moveCost;
}
// Set H value
m_H = ManhattanDistance(m_position, m_goal);
}
float AStar::HeuristicDistance(Vector StartPos, Vector EndPos)
{
// Seems like HEURISTIC_EUCLIDEAN == HEURISTIC_DISTANCE
if(Heuristic == HEURISTIC_EUCLIDEAN)
{
return EuclideanDistance(StartPos, EndPos);
}
else if(Heuristic == HEURISTIC_DISTANCE)
{
return (StartPos - EndPos).Length();
}
else if(Heuristic == HEURISTIC_CUSTOM)
{
//gLua->PushReference(HeuristicRef);
//GMOD_PushVector(StartPos);
//GMOD_PushVector(EndPos);
//gLua->Call(2, 1);
//return gLua->GetNumber(1);
}
return ManhattanDistance(StartPos, EndPos);
}
bool
RasterMap::FirstIntersection(const GeoPoint &origin, const int h_origin,
const GeoPoint &destination, const int h_destination,
const int h_virt, const int h_ceiling,
const int h_safety,
GeoPoint &intx, int &h) const
{
const auto c_origin = projection.ProjectCoarse(origin);
const auto c_destination = projection.ProjectCoarse(destination);
const int c_diff = c_origin.ManhattanDistance(c_destination);
const bool can_climb = (h_destination< h_virt);
intx = destination; h = h_destination; // fallback, pass
if (c_diff==0) {
return false; // no distance
}
const int slope_fact = (((int)h_virt) << RASTER_SLOPE_FACT) / c_diff;
const int vh_origin = std::max(h_origin,
h_destination
- ((c_diff * slope_fact) >> RASTER_SLOPE_FACT));
RasterLocation c_int;
if (raster_tile_cache.FirstIntersection(c_origin.x, c_origin.y,
c_destination.x, c_destination.y,
vh_origin, h_destination,
slope_fact, h_ceiling, h_safety,
c_int, h,
can_climb)) {
bool changed = c_int != c_destination ||
(h > h_destination && c_int == c_destination);
if (changed) {
intx = projection.UnprojectCoarse(c_int);
assert(h>= h_origin);
}
return changed;
} else {
return false;
}
}
GeoPoint
RasterMap::Intersection(const GeoPoint& origin,
const int h_origin, const int h_glide,
const GeoPoint& destination,
const int height_floor) const
{
const auto c_origin = projection.ProjectCoarseRound(origin);
const auto c_destination = projection.ProjectCoarseRound(destination);
const int c_diff = ManhattanDistance(c_origin, c_destination);
if (c_diff == 0)
return GeoPoint::Invalid();
const int slope_fact = (((int)h_glide) << RASTER_SLOPE_FACT) / c_diff;
auto c_int =
raster_tile_cache.Intersection(c_origin, c_destination,
h_origin, slope_fact, height_floor);
if (c_int.x < 0)
return GeoPoint::Invalid();
return projection.UnprojectCoarse(c_int);
}
GeoPoint
RasterMap::Intersection(const GeoPoint& origin,
const int h_origin, const int h_glide,
const GeoPoint& destination) const
{
const auto c_origin = projection.ProjectCoarse(origin);
const auto c_destination = projection.ProjectCoarse(destination);
const int c_diff = c_origin.ManhattanDistance(c_destination);
if (c_diff==0) {
return destination; // no distance
}
const int slope_fact = (((int)h_glide) << RASTER_SLOPE_FACT) / c_diff;
auto c_int =
raster_tile_cache.Intersection(c_origin.x, c_origin.y,
c_destination.x, c_destination.y,
h_origin, slope_fact);
if (c_int == c_destination) // made it to grid location, return exact location
// of destination
return destination;
return projection.UnprojectCoarse(c_int);
}
static unsigned
_PolygonToTriangles(const PT *points, unsigned num_points,
GLushort *triangles, typename PT::scalar_type min_distance)
{
// no redundant start/end please
if (num_points >= 1 && points[0] == points[num_points - 1])
num_points--;
if (num_points < 3)
return 0;
assert(num_points < 65536);
// next vertex pointer
auto next = new GLushort[num_points];
// index of the first vertex
GLushort start = 0;
// initialize next pointer counterclockwise
if (PolygonRotatesLeft(points, num_points)) {
for (unsigned i = 0; i < num_points-1; i++)
next[i] = i + 1;
next[num_points - 1] = 0;
} else {
next[0] = num_points - 1;
for (unsigned i = 1; i < num_points; i++)
next[i] = i - 1;
}
// thinning
if (min_distance > 0) {
for (unsigned a = start, b = next[a], c = next[b], heat = 0;
num_points > 3 && heat < num_points;
a = b, b = c, c = next[c], heat++) {
bool point_removeable = TriangleEmpty(points[a], points[b], points[c]);
if (!point_removeable) {
typename PT::scalar_type distance = ManhattanDistance(points[a],
points[b]);
if (distance < min_distance) {
point_removeable = true;
if (distance > 0) {
for (unsigned p = next[c]; p != a; p = next[p]) {
if (InsideTriangle(points[p], points[a], points[b], points[c])) {
point_removeable = false;
break;
}
}
}
}
}
if (point_removeable) {
// remove node b from polygon
if (b == start)
// keep track of the smallest index
start = std::min(a, c);
next[a] = c;
num_points--;
// 'a' should stay the same in the next loop
b = a;
// reset heat
heat = 0;
}
}
//LogDebug(_T("polygon thinning (%u) removed %u of %u vertices"),
// min_distance, orig_num_points-num_points, orig_num_points);
}
// triangulation
auto t = triangles;
for (unsigned a = start, b = next[a], c = next[b], heat = 0;
num_points > 2; a = b, b = c, c = next[c]) {
typename PT::product_type bendiness =
LeftBend(points[a], points[b], points[c]);
// left bend, spike or line with a redundant point in the middle
bool ear_cuttable = (bendiness >= 0);
if (bendiness > 0) {
// left bend
for (unsigned prev_p = c, p = next[c]; p != a;
prev_p = p, p = next[p]) {
typename PT::product_type ab = PointLeftOfLine(points[p], points[a],
points[b]);
typename PT::product_type bc = PointLeftOfLine(points[p], points[b],
points[c]);
typename PT::product_type ca = PointLeftOfLine(points[p], points[c],
points[a]);
if (ab > 0 && bc > 0 && ca > 0) {
// p is inside a,b,c
ear_cuttable = false;
break;
} else if (ab >= 0 && bc >= 0 && ca >= 0) {
// p is on one or two edges of a,b,c
bool outside_ab = (ab == 0) &&
PointLeftOfLine(points[prev_p], points[a], points[b]) <= 0;
bool outside_bc = (bc == 0) &&
PointLeftOfLine(points[prev_p], points[b], points[c]) <= 0;
bool outside_ca = (ca == 0) &&
PointLeftOfLine(points[prev_p], points[c], points[a]) <= 0;
if (!(outside_ab || outside_bc || outside_ca)) {
// line p,prev_p intersects with triangle a,b,c
ear_cuttable = false;
break;
}
outside_ab = (ab == 0) &&
PointLeftOfLine(points[next[p]], points[a], points[b]) <= 0;
outside_bc = (bc == 0) &&
PointLeftOfLine(points[next[p]], points[b], points[c]) <= 0;
outside_ca = (ca == 0) &&
PointLeftOfLine(points[next[p]], points[c], points[a]) <= 0;
if (!(outside_ab || outside_bc || outside_ca)) {
// line p,next[p] intersects with triangle a,b,c
ear_cuttable = false;
break;
}
}
}
if (ear_cuttable) {
// save triangle indices
*t++ = a;
*t++ = b;
*t++ = c;
}
}
if (ear_cuttable) {
// remove node b from polygon
next[a] = c;
num_points--;
// 'a' should stay the same in the next loop
b = a;
// reset heat
heat = 0;
}
if (heat++ > num_points) {
// if polygon edges overlap we may loop endlessly
//LogDebug(_T("polygon_to_triangle: bad polygon"));
delete[] next;
return 0;
}
}
delete[] next;
return t - triangles;
}
void inline OpenNode(struct Map * themap,struct Path * route,unsigned int parent_node,unsigned int the_node)
{
if ( ( !themap->world[the_node].opened ) && (themap->world[the_node].unpassable==0) && (themap->world[the_node].in_unpassable_radious==0) )
{
if (route->openlist_top+1<route->openlist_size)
{
themap->world[the_node].opened=1;
// ADDING NEW NODE TO PENDING LIST!
route->openlist[route->openlist_top].node = the_node;
unsigned int x=parent_node % themap->world_size_x; // Ypologizoume tin syntetagmeni x , y
unsigned int y=parent_node / themap->world_size_x; // Ypologizoume tin syntetagmeni x , y
route->openlist[route->openlist_top].score = themap->world[the_node].movement_cost+ManhattanDistance(x,y,route->target_x,route->target_y);
++route->openlist_top;
// WE HEAVE SUCCESSFULLY ADDED THE NEW NODE TO THE TAIL OF THE OPENLIST
}
else
{
fprintf(stderr,"Out of space on open list , lets hope open nodes have a solution ( it might not be optimal )!!!\n");
/* Mas teleiwse o xoros.. krima.. as elpisoume oti ta open nodes exoun lysi!! */
}
}
}
bool
XShape::BuildIndices(unsigned thinning_level, ShapeScalar min_distance)
{
assert(indices[thinning_level] == nullptr);
unsigned short *idx, *idx_count;
unsigned num_points = 0;
for (unsigned i=0; i < num_lines; i++)
num_points += lines[i];
if (type == MS_SHAPE_LINE) {
if (num_points <= 2)
return false; // line cannot be simplified, so don't create indices
index_count[thinning_level] = idx_count =
new GLushort[num_lines + num_points];
indices[thinning_level] = idx = idx_count + num_lines;
const unsigned short *end_l = lines + num_lines;
const ShapePoint *p = points;
unsigned i = 0;
for (const unsigned short *l = lines; l < end_l; l++) {
assert(*l >= 2);
const ShapePoint *end_p = p + *l - 1;
// always add first point
*idx++ = i;
p++; i++;
const unsigned short *after_first_idx = idx;
// add points if they are not too close to the previous point
for (; p < end_p; p++, i++)
if (ManhattanDistance(points[idx[-1]], *p) >= min_distance)
*idx++ = i;
// remove points from behind if they are too close to the end point
while (idx > after_first_idx &&
ManhattanDistance(points[idx[-1]], *p) < min_distance)
idx--;
// always add last point
*idx++ = i;
p++; i++;
*idx_count++ = idx - after_first_idx + 1;
}
// TODO: free memory saved by thinning (use malloc/realloc or some class?)
return true;
} else if (type == MS_SHAPE_POLYGON) {
index_count[thinning_level] = idx_count =
new GLushort[1 + 3*(num_points-2) + 2*(num_lines-1)];
indices[thinning_level] = idx = idx_count + 1;
*idx_count = 0;
const ShapePoint *pt = points;
for (unsigned i=0; i < num_lines; i++) {
unsigned count = PolygonToTriangles(pt, lines[i], idx + *idx_count,
min_distance);
if (i > 0) {
const GLushort offset = pt - points;
const unsigned max_idx_count = *idx_count + count;
for (unsigned j=*idx_count; j < max_idx_count; j++)
idx[j] += offset;
}
*idx_count += count;
pt += lines[i];
}
*idx_count = TriangleToStrip(idx, *idx_count, num_points, num_lines);
// TODO: free memory saved by thinning (use malloc/realloc or some class?)
return true;
} else {
gcc_unreachable();
}
}