Justin Marshall
348 lines
14 KiB
C++
348 lines
14 KiB
C++
#include "gamelib/aas2/obstacleavoidancecore.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
#include <limits>
|
|
#include <vector>
|
|
|
|
namespace {
|
|
|
|
float Cross2(const idVec2& first, const idVec2& second) {
|
|
return first.x * second.y - first.y * second.x;
|
|
}
|
|
|
|
idVec2 Sub2(const idVec2& first, const idVec2& second) {
|
|
return idVec2(first.x - second.x, first.y - second.y);
|
|
}
|
|
|
|
float Dot2(const idVec2& first, const idVec2& second) {
|
|
return first.x * second.x + first.y * second.y;
|
|
}
|
|
|
|
float Length2(const idVec2& value) {
|
|
return std::sqrt(Dot2(value, value));
|
|
}
|
|
|
|
idVec2 Normalize2(const idVec2& value) {
|
|
const float length = Length2(value);
|
|
return length > 1.0e-6f
|
|
? idVec2(value.x / length, value.y / length) : idVec2(0.0f, 0.0f);
|
|
}
|
|
|
|
float SegmentPointDistanceSqr(const idVec2& start, const idVec2& end,
|
|
const idVec2& point) {
|
|
const idVec2 delta = Sub2(end, start);
|
|
const float lengthSqr = Dot2(delta, delta);
|
|
float fraction = lengthSqr > 0.0f
|
|
? Dot2(Sub2(point, start), delta) / lengthSqr : 0.0f;
|
|
fraction = (std::max)(0.0f, (std::min)(1.0f, fraction));
|
|
const idVec2 closest(start.x + delta.x * fraction,
|
|
start.y + delta.y * fraction);
|
|
return Dot2(Sub2(point, closest), Sub2(point, closest));
|
|
}
|
|
|
|
bool SegmentIntersection(const idVec2& a, const idVec2& b,
|
|
const idVec2& c, const idVec2& d) {
|
|
const idVec2 r = Sub2(b, a);
|
|
const idVec2 s = Sub2(d, c);
|
|
const float denominator = Cross2(r, s);
|
|
if (std::fabs(denominator) < 1.0e-6f) return false;
|
|
const idVec2 delta = Sub2(c, a);
|
|
const float t = Cross2(delta, s) / denominator;
|
|
const float u = Cross2(delta, r) / denominator;
|
|
return t >= 0.0f && t <= 1.0f && u >= 0.0f && u <= 1.0f;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
idObstacleAvoidanceCore::idObstacleAvoidanceCore()
|
|
: bspNodes(nullptr), numBspNodes(0), obstacleIndices(nullptr),
|
|
expandedObstacles(nullptr), maxExpandedObstacles(0),
|
|
numExpandedObstacles(0), windingPoints(nullptr), maxWindingPoints(0),
|
|
numWindingPoints(0), tempMemory(nullptr), tempMemorySize(0),
|
|
tempMemoryStart(nullptr), tempMemoryEnd(nullptr), pathNodes(nullptr),
|
|
maxPathNodes(0), numPathNodes(0), debugLines(nullptr),
|
|
maxDebugLines(0), numDebugLines(0), debugText(nullptr),
|
|
maxDebugText(0), numDebugText(0), inputTransformValid(false),
|
|
inputTransform(), flags(0) {
|
|
}
|
|
|
|
idObstacleAvoidanceCore::~idObstacleAvoidanceCore() = default;
|
|
|
|
void idObstacleAvoidanceCore::AddDebugLine(const avoidColor_t color,
|
|
const idVec2& start, const idVec2& end) {
|
|
if (debugLines == nullptr || numDebugLines >= maxDebugLines - 1) return;
|
|
avoidDebugLine_t& line = debugLines[numDebugLines++];
|
|
line.color = color;
|
|
line.start.Set(start.x, start.y, 0.0f);
|
|
line.end.Set(end.x, end.y, 0.0f);
|
|
}
|
|
|
|
void idObstacleAvoidanceCore::AddDebugInt(const avoidColor_t color,
|
|
const idVec2& position, const int value) {
|
|
if (debugText == nullptr || numDebugText >= maxDebugText - 1) return;
|
|
avoidDebugText_t& text = debugText[numDebugText++];
|
|
std::snprintf(text.text, sizeof(text.text), "%d", value);
|
|
text.pos.Set(position.x, position.y, 0.0f);
|
|
text.color = color;
|
|
}
|
|
|
|
int idObstacleAvoidanceCore::BoxParallelProjectionSilhouetteVerts(
|
|
const idMat3& axis, const idVec3& center, const idVec3& extents,
|
|
const idVec3& projectionDir, idVec3* const silhouette) {
|
|
if (silhouette == nullptr) return 0;
|
|
idVec3 corners[8];
|
|
int count = 0;
|
|
for (int x = -1; x <= 1; x += 2) {
|
|
for (int y = -1; y <= 1; y += 2) {
|
|
for (int z = -1; z <= 1; z += 2) {
|
|
corners[count++] = center + axis[0] * (extents.x * x)
|
|
+ axis[1] * (extents.y * y)
|
|
+ axis[2] * (extents.z * z);
|
|
}
|
|
}
|
|
}
|
|
int output = 0;
|
|
for (int index = 0; index < 8 && output < 6; ++index) {
|
|
const float facing = (corners[index] - center).Dot(projectionDir);
|
|
if (facing <= 0.0f) silhouette[output++] = corners[index];
|
|
}
|
|
return output;
|
|
}
|
|
|
|
void idObstacleAvoidanceCore::GetCirclePoints(const idVec2& center,
|
|
const float radius, idVec2* const points) {
|
|
if (points == nullptr) return;
|
|
constexpr float diagonal = 0.70710678118f;
|
|
const idVec2 directions[8] = {
|
|
idVec2(1, 0), idVec2(diagonal, diagonal), idVec2(0, 1),
|
|
idVec2(-diagonal, diagonal), idVec2(-1, 0),
|
|
idVec2(-diagonal, -diagonal), idVec2(0, -1),
|
|
idVec2(diagonal, -diagonal)
|
|
};
|
|
for (int index = 0; index < 8; ++index) {
|
|
points[index].Set(center.x + directions[index].x * radius,
|
|
center.y + directions[index].y * radius);
|
|
}
|
|
}
|
|
|
|
idVec3 idObstacleAvoidanceCore::Plane2DFromPoints(const idVec2& start,
|
|
const idVec2& end, const bool normalize) {
|
|
return Plane2DFromVecs(start, Sub2(end, start), normalize);
|
|
}
|
|
|
|
idVec3 idObstacleAvoidanceCore::Plane2DFromVecs(const idVec2& start,
|
|
const idVec2& direction, const bool normalize) {
|
|
idVec2 normal(-direction.y, direction.x);
|
|
if (normalize) normal = Normalize2(normal);
|
|
return idVec3(normal.x, normal.y,
|
|
-(normal.x * start.x + normal.y * start.y));
|
|
}
|
|
|
|
bool idObstacleAvoidanceCore::PointInsideWinding(const idVec2* const winding,
|
|
const int numPoints, const idVec2& point, const float epsilon) {
|
|
if (winding == nullptr || numPoints <= 0) return true;
|
|
float sign = 0.0f;
|
|
for (int index = 0; index < numPoints; ++index) {
|
|
const float side = Cross2(Sub2(winding[(index + 1) % numPoints],
|
|
winding[index]), Sub2(point, winding[index]));
|
|
if (std::fabs(side) <= epsilon) continue;
|
|
if (sign == 0.0f) sign = side;
|
|
else if ((sign < 0.0f) != (side < 0.0f)) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void idObstacleAvoidanceCore::ExpandWinding(const idVec2* const winding,
|
|
const int numPoints, const float distance, idVec2* const expanded) {
|
|
if (winding == nullptr || expanded == nullptr || numPoints <= 0) return;
|
|
for (int index = 0; index < numPoints; ++index) {
|
|
const idVec2 previous = winding[(index + numPoints - 1) % numPoints];
|
|
const idVec2 current = winding[index];
|
|
const idVec2 next = winding[(index + 1) % numPoints];
|
|
const idVec2 first = Normalize2(idVec2(
|
|
-(current.y - previous.y), current.x - previous.x));
|
|
const idVec2 second = Normalize2(idVec2(
|
|
-(next.y - current.y), next.x - current.x));
|
|
idVec2 direction = Normalize2(idVec2(first.x + second.x,
|
|
first.y + second.y));
|
|
const float denominator = (std::max)(0.1f,
|
|
std::fabs(Dot2(direction, first)));
|
|
expanded[index].Set(current.x + direction.x * distance / denominator,
|
|
current.y + direction.y * distance / denominator);
|
|
}
|
|
}
|
|
|
|
void idObstacleAvoidanceCore::GetWindingBounds(
|
|
const idVec2* const winding, const int numPoints,
|
|
idVec2 bounds[2]) {
|
|
const float maximum = (std::numeric_limits<float>::max)();
|
|
bounds[0].Set(maximum, maximum);
|
|
bounds[1].Set(-maximum, -maximum);
|
|
for (int index = 0; index < numPoints; ++index) {
|
|
bounds[0].x = (std::min)(bounds[0].x, winding[index].x);
|
|
bounds[0].y = (std::min)(bounds[0].y, winding[index].y);
|
|
bounds[1].x = (std::max)(bounds[1].x, winding[index].x);
|
|
bounds[1].y = (std::max)(bounds[1].y, winding[index].y);
|
|
}
|
|
}
|
|
|
|
bool idObstacleAvoidanceCore::WindingRayIntersection(
|
|
const idVec2* const winding, const int numPoints,
|
|
const idVec2& start, const idVec2& direction, float& firstScale,
|
|
float& secondScale, int edgeNums[2]) {
|
|
firstScale = (std::numeric_limits<float>::max)();
|
|
secondScale = -(std::numeric_limits<float>::max)();
|
|
int intersections = 0;
|
|
for (int index = 0; index < numPoints; ++index) {
|
|
const idVec2 edgeStart = winding[index];
|
|
const idVec2 edge = Sub2(winding[(index + 1) % numPoints], edgeStart);
|
|
const float denominator = Cross2(direction, edge);
|
|
if (std::fabs(denominator) < 1.0e-6f) continue;
|
|
const idVec2 delta = Sub2(edgeStart, start);
|
|
const float scale = Cross2(delta, edge) / denominator;
|
|
const float edgeScale = Cross2(delta, direction) / denominator;
|
|
if (edgeScale < 0.0f || edgeScale > 1.0f) continue;
|
|
if (scale < firstScale) {
|
|
firstScale = scale;
|
|
if (edgeNums != nullptr) edgeNums[0] = index;
|
|
}
|
|
if (scale > secondScale) {
|
|
secondScale = scale;
|
|
if (edgeNums != nullptr) edgeNums[1] = index;
|
|
}
|
|
++intersections;
|
|
}
|
|
return intersections >= 2;
|
|
}
|
|
|
|
void idObstacleAvoidanceCore::FindPathAroundObstacles(obstaclePath_t& path,
|
|
avoidDebugLine_t* const outputDebugLines, const int outputMaxDebugLines,
|
|
avoidDebugText_t* const outputDebugText, const int outputMaxDebugText,
|
|
std::uint8_t* const temp, const int tempSize, const idBounds&,
|
|
const idVec3&, const idVec3& lastDir, const idVec4& lastCorner,
|
|
const float obstacleRadius, const float frameMoveDist,
|
|
const float, const int queryFlags, const obstacleRoute_t& route,
|
|
const obstacleBox_t* const obstacles, const int numObstacles,
|
|
const obstacleVertex_t* const vertices, const int numVertices,
|
|
const obstacleEdge_t* const edges, const int numEdges,
|
|
const obstacleCorner_t*, const int, const obstacleTrace_t*,
|
|
const int, float*) {
|
|
debugLines = outputDebugLines;
|
|
maxDebugLines = outputMaxDebugLines;
|
|
numDebugLines = 0;
|
|
debugText = outputDebugText;
|
|
maxDebugText = outputMaxDebugText;
|
|
numDebugText = 0;
|
|
tempMemory = temp;
|
|
tempMemorySize = tempSize;
|
|
flags = queryFlags;
|
|
|
|
const idVec3 start3 = route.numAreas > 0
|
|
? route.areas[0].start : path.startPosOutsideObstacles;
|
|
const idVec3 goal3 = route.numAreas > 0
|
|
? route.areas[route.numAreas - 1].end : path.seekPos[0];
|
|
const idVec2 start(start3.x, start3.y);
|
|
const idVec2 goal(goal3.x, goal3.y);
|
|
path.numSeekPos = 0;
|
|
path.firstObstacle = -1;
|
|
path.firstObstaclePosition.Zero();
|
|
path.startPosOutsideObstacles = start3;
|
|
path.seekPosOutsideObstacles = goal3;
|
|
path.startPosObstacle = -1;
|
|
path.seekPosObstacle = -1;
|
|
path.targetDist = Length2(Sub2(goal, start));
|
|
path.pathLength = path.targetDist;
|
|
path.hasValidPath = true;
|
|
path.startPosValid = true;
|
|
path.seekPosValid = true;
|
|
|
|
int blocking = -1;
|
|
float blockingRadius = 0.0f;
|
|
for (int index = 0; index < numObstacles; ++index) {
|
|
const obstacleBox_t& obstacle = obstacles[index];
|
|
if (obstacle.soft && (queryFlags & 0x200) == 0) continue;
|
|
const float radius = std::sqrt(obstacle.extents.x * obstacle.extents.x
|
|
+ obstacle.extents.y * obstacle.extents.y) + obstacleRadius;
|
|
if (SegmentPointDistanceSqr(start, goal,
|
|
idVec2(obstacle.center.x, obstacle.center.y))
|
|
< radius * radius) {
|
|
blocking = index;
|
|
blockingRadius = radius;
|
|
break;
|
|
}
|
|
}
|
|
if (blocking < 0) {
|
|
for (int index = 0; index < numEdges; ++index) {
|
|
const int first = edges[index].vertNums[0];
|
|
const int second = edges[index].vertNums[1];
|
|
if (first < 0 || second < 0 || first >= numVertices
|
|
|| second >= numVertices) continue;
|
|
const idVec2 edgeStart(vertices[first].position.x,
|
|
vertices[first].position.y);
|
|
const idVec2 edgeEnd(vertices[second].position.x,
|
|
vertices[second].position.y);
|
|
if (SegmentIntersection(start, goal, edgeStart, edgeEnd)) {
|
|
blocking = numObstacles + index;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (blocking >= 0 && blocking < numObstacles) {
|
|
const obstacleBox_t& obstacle = obstacles[blocking];
|
|
const idVec2 center(obstacle.center.x, obstacle.center.y);
|
|
idVec2 direction = Normalize2(Sub2(goal, start));
|
|
idVec2 perpendicular(-direction.y, direction.x);
|
|
const idVec2 candidates[2] = {
|
|
idVec2(center.x + perpendicular.x * blockingRadius,
|
|
center.y + perpendicular.y * blockingRadius),
|
|
idVec2(center.x - perpendicular.x * blockingRadius,
|
|
center.y - perpendicular.y * blockingRadius)
|
|
};
|
|
const float firstLength = Length2(Sub2(candidates[0], start))
|
|
+ Length2(Sub2(goal, candidates[0]));
|
|
const float secondLength = Length2(Sub2(candidates[1], start))
|
|
+ Length2(Sub2(goal, candidates[1]));
|
|
const idVec2 corner = firstLength <= secondLength
|
|
? candidates[0] : candidates[1];
|
|
path.seekPos[0].Set(corner.x, corner.y, start3.z);
|
|
path.seekPos[1] = goal3;
|
|
path.numSeekPos = 2;
|
|
path.firstObstacle = obstacle.id;
|
|
path.firstObstaclePosition = obstacle.center;
|
|
path.pathLength = (std::min)(firstLength, secondLength);
|
|
path.nextCorner.Set(corner.x, corner.y, start3.z,
|
|
static_cast<float>(obstacle.id));
|
|
AddDebugLine(COLOR_RED, start, corner);
|
|
AddDebugLine(COLOR_GREEN, corner, goal);
|
|
} else if (blocking >= numObstacles) {
|
|
path.hasValidPath = false;
|
|
path.seekPos[0] = start3;
|
|
path.numSeekPos = 1;
|
|
} else {
|
|
path.seekPos[0] = goal3;
|
|
path.numSeekPos = 1;
|
|
path.nextCorner.Set(goal3.x, goal3.y, goal3.z, -1.0f);
|
|
AddDebugLine(COLOR_GREEN, start, goal);
|
|
}
|
|
path.done = true;
|
|
path.useRadarForward = Length2(idVec2(lastDir.x, lastDir.y)) > 0.0f;
|
|
path.radarForward = lastDir;
|
|
if (lastCorner.w >= 0.0f && path.firstObstacle < 0) {
|
|
path.nextCorner = lastCorner;
|
|
}
|
|
if (frameMoveDist > 0.0f && path.numSeekPos > 0) {
|
|
const idVec2 move = Sub2(idVec2(path.seekPos[0].x,
|
|
path.seekPos[0].y), start);
|
|
const float moveLength = Length2(move);
|
|
if (moveLength > frameMoveDist) {
|
|
const float scale = frameMoveDist / moveLength;
|
|
path.seekPos[0].x = start.x + move.x * scale;
|
|
path.seekPos[0].y = start.y + move.y * scale;
|
|
}
|
|
}
|
|
}
|