lx/watrbx-game-engine
1
0
Fork 0
mirror of https://github.com/copyrighttxt/watrbx-game-engine.git synced 2026-09-04 20:57:49 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
master
watrbx-game-engine/App/v8datamodel/PathfindingService.cpp
T
watrabi 977f1ff4b8 GEEKING
2025-09-18 17:55:52 -04:00

1081 lines
35 KiB
C++
Raw Permalink Blame History

#include "stdafx.h"
#include "V8DataModel/DataStoreService.h"
#include "V8DataModel/DataModel.h"
#include "V8DataModel/Workspace.h"
#include "V8DataModel/MegaCluster.h"
#include "V8DataModel/PartCookie.h"
#include "v8datamodel/PathfindingService.h"
#include "Util/RobloxGoogleAnalytics.h"
#include "Network/Players.h"
#include "v8world/World.h"
#include "v8world/ContactManagerSpatialHash.h"
#include "Voxel/Grid.Chunk.h"
#include "voxel2/Grid.h"
#include "reflection/EnumConverter.h"
#include "rbx/Profiler.h"
LOGVARIABLE(PathfindingDetail, 0);
LOGVARIABLE(PathfindingPerf, 0);
DYNAMIC_FASTINTVARIABLE(PathfindingMaxDistance, 512);
DYNAMIC_FASTINTVARIABLE(PathfindingJobRunsPerSecond, 10);
DYNAMIC_FASTINTVARIABLE(PathfindingChunksPerInvokation, 10);
DYNAMIC_FASTINTVARIABLE(PathfindingAgeToCollectChunks, 1000);
DYNAMIC_FASTINTVARIABLE(PathfindingCollectPeriod, 100);
DYNAMIC_FASTINTVARIABLE(PathfindingDefaultBucketNum, 2048);
DYNAMIC_FASTINTVARIABLE(PathfindingVerticalChunkClamp, 1);
DYNAMIC_FASTINTVARIABLE(PathfindingSmoothIterations, 5);
DYNAMIC_FASTINTVARIABLE(PathfindingAverageWindow, 7);
RBX_REGISTER_ENUM(Path::PathStatus);
namespace RBX {
const char* const sPathfindingService = "PathfindingService";
const float PATHFINDING_FALL_COST = 1.0f;
const float PATHFINDING_WALK_COST = 1.0f;
const float PATHFINDING_WALK_DIAGLONAL_COST = sqrt(2);
float kMaxWorldCoordinate = (float)((std::numeric_limits<G3D::int16>::max()-2*kVoxelChunkSizeXZ)*Voxel::kCELL_SIZE);
REFLECTION_BEGIN();
static Reflection::PropDescriptor<PathfindingService, float> prop_EmptyCutoff
("EmptyCutoff", category_Data, &PathfindingService::getEmptyCutoff, &PathfindingService::setEmptyCutoff);
static Reflection::BoundYieldFuncDesc<PathfindingService, shared_ptr<Instance>(Vector3, Vector3, float)>
func_computeRawPathAsync(&PathfindingService::computeRawPathAsync, "ComputeRawPathAsync", "start", "finish", "maxDistance", Security::None);
static Reflection::BoundYieldFuncDesc<PathfindingService, shared_ptr<Instance>(Vector3, Vector3, float)>
func_computeSmoothPathAsync(&PathfindingService::computeSmoothPathAsync, "ComputeSmoothPathAsync", "start", "finish", "maxDistance", Security::None);
static Reflection::BoundYieldFuncDesc<Path, int(int)>
func_checkOcclusionAsync(&Path::checkOcclusionAsync, "CheckOcclusionAsync", "start", Security::None);
static Reflection::BoundFuncDesc<Path, shared_ptr<const Reflection::ValueArray>(void) > prop_Points
(&Path::getPointCoordinates, "GetPointCoordinates", Security::None);
REFLECTION_END();
namespace Reflection
{
template<> Reflection::EnumDesc<Path::PathStatus>::EnumDesc()
:RBX::Reflection::EnumDescriptor("PathStatus")
{
addPair(Path::Success, "Success");
addPair(Path::ClosestNoPath, "ClosestNoPath");
addPair(Path::ClosestOutOfRange, "ClosestOutOfRange");
addPair(Path::FailStartNotEmpty, "FailStartNotEmpty");
addPair(Path::FailFinishNotEmpty, "FailFinishNotEmpty");
}
}
REFLECTION_BEGIN();
static Reflection::EnumPropDescriptor<Path, Path::PathStatus> prop_PathStatus
("Status", category_Data, &Path::getStatus, NULL);
REFLECTION_END();
class PathfindingJob : public DataModelJob
{
public:
shared_ptr<PathfindingService> pathfindingService;
double desiredHz;
PathfindingJob(PathfindingService* owner)
: DataModelJob("PathfindingJob", DataModelJob::Write, false, shared_from(DataModel::get(owner)), Time::Interval(0.01)),
pathfindingService(shared_from(owner))
{
desiredHz = DFInt::PathfindingJobRunsPerSecond;
cyclicExecutive = true;
}
/*override*/ Time::Interval sleepTime(const Stats& stats)
{
return computeStandardSleepTime(stats, desiredHz);
}
/*override*/ Job::Error error(const Stats& stats)
{
return computeStandardErrorCyclicExecutiveSleeping(stats, desiredHz);
}
/*override*/ TaskScheduler::StepResult stepDataModelJob(const Stats& stats)
{
pathfindingService->executeThrottledRequests();
return TaskScheduler::Stepped;
}
};
Vector3int16 getDiagonalOffsetByIndex(int dir)
{
switch (dir)
{
case 0: return Vector3int16(1,0,-1);
case 1: return Vector3int16(-1,0,-1);
case 2: return Vector3int16(-1,0,1);
case 3: return Vector3int16(1,0,1);
default:
RBXASSERT(false);
return Vector3int16(0,0,0);
}
}
Vector3int16 getOffsetByIndex(int dir)
{
switch (dir)
{
case 0: return Vector3int16(1,0,0);
case 1: return Vector3int16(0,0,-1);
case 2: return Vector3int16(-1,0,0);
case 3: return Vector3int16(0,0,1);
default:
RBXASSERT(false);
return Vector3int16(0,0,0);
}
}
PathfindingService::PathfindingService()
: lastChunkId(SpatialRegion::Id(std::numeric_limits<G3D::int16>::max(), std::numeric_limits<G3D::int16>::max(), std::numeric_limits<G3D::int16>::max())),
lastChunk(NULL),
currentFrameNum(0),
voxelizer(true /* collisionTransparency */)
{
emptyCutoff = 40/255.0f;
}
void PathfindingService::setEmptyCutoff(float value)
{
if(value < 0.0f || value > 1.0f)
{
throw std::runtime_error("Value is outside expected range");
}
emptyCutoff = value;
}
inline float diffDistance(const Vector3int16& start, const Vector3int16& finish)
{
Vector3int16 delta = finish - start;
return sqrt(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
}
void PathfindingService::PathfindingState::addOpenNode(const Node& node)
{
FASTLOG3(FLog::PathfindingDetail, "Adding open node: %i %i %i", node.pos.x, node.pos.y, node.pos.z);
NodesByPos::iterator it = nodesByPos.find(node.pos);
if(it != nodesByPos.end())
{
FASTLOG1F(FLog::PathfindingDetail, "Open node already exist with cost %f", it->second.totalPath);
if(node.totalPath < it->second.totalPath)
{
FASTLOG1F(FLog::PathfindingDetail, "New node is better, opening node with distance %f", it->second.itWeight->first);
if(!it->second.closed)
{
openNodesByWeight.erase(it->second.itWeight);
it->second.itWeight = openNodesByWeight.end();
}
it->second.closed = false;
it->second.parent = node.parent;
it->second.totalPath = node.totalPath;
}
else
{
return;
}
}
else
{
std::pair<NodesByPos::iterator, bool> result = nodesByPos.insert(NodesByPos::value_type(node.pos, node));
RBXASSERT(result.second);
it = result.first;
}
float rawDistance = diffDistance(finish, node.pos);
float distance = rawDistance + node.totalPath;
FASTLOG3(FLog::PathfindingDetail, "Creating open node: %i %i %i", node.pos.x, node.pos.y, node.pos.z);
FASTLOG2F(FLog::PathfindingDetail, "Distance: %f, raw distance: %f", distance, rawDistance);
FASTLOG3(FLog::PathfindingDetail, "New open node parent: %i %i %i", node.parent.x, node.parent.y, node.parent.z);
OpenNodesByWeight::iterator itWeight = openNodesByWeight.insert(std::make_pair(distance, it));
it->second.itWeight = itWeight;
RBXASSERT_SLOW(checkOpenNodesCounts());
if(rawDistance < closestDistance)
{
FASTLOG1F(FLog::PathfindingDetail, "Distance is less than previous closest distance %f", closestDistance);
closestDistance = rawDistance;
closestCell = node.pos;
closestCost = node.totalPath;
}
}
Vector3 getPoint(const std::vector<Vector3>& points, int index, int size)
{
if (index < 0) {
RBXASSERT((-index) < size);
// Inverse around point zero, so result = point_0 + (point_0 - point_i) = 2*point_0 - point_i
return 2*points[0] - points[-index];
} else if (index >= size)
{
RBXASSERT((2*(size-1) - index) >= 0);
// Inverse around last point (size-1), index is inversed the same way
return 2*points[size-1] - points[2*(size-1) - index];
}
return points[index];
}
void PathfindingService::smoothPath(std::vector<Vector3>& points, unsigned char emptyCutoff, int smoothWindow)
{
int size = points.size();
if(smoothWindow >= size)
{
// Find largest odd number less or equal to size
smoothWindow = (size - 1) | 1;
}
if(smoothWindow == 1)
return;
float aveCoeff = 1.0f / smoothWindow;
int halfAve = smoothWindow / 2;
Vector3 ave = Vector3(0,0,0);
for(int i = -halfAve; i <= halfAve; i++)
{
ave += getPoint(points, i, size) * aveCoeff;
}
std::vector<Vector3> newPoints(points.size());
newPoints[0] = points[0];
newPoints[size-1] = points[size-1];
for(int i = 1; i < size-1; i++)
{
newPoints[i] = points[i];
ave += getPoint(points, i+halfAve, size) * aveCoeff;
ave -= getPoint(points, i-halfAve-1, size) * aveCoeff;
ave.y = points[i].y;
FASTLOG1(FLog::PathfindingDetail, "Processing point %i", i);
FASTLOG3F(FLog::PathfindingDetail, "Average: %f %f %f", ave.x, ave.y, ave.z);
Vector3int16 floor = Vector3int16(fastFloorInt(ave.x-0.5f), ave.y, fastFloorInt(ave.z-0.5f));
Vector3int16 ceil = Vector3int16(fastCeilInt(ave.x-0.5f), ave.y, fastCeilInt(ave.z-0.5f));
FASTLOG4(FLog::PathfindingDetail, "Floor: %i %i, ceil: %i %i", floor.x, floor.z, ceil.x, ceil.z);
if(floor == ceil)
{
if(isSolid(floor, emptyCutoff) != EMPTY)
{
FASTLOG3(FLog::PathfindingDetail, "Floor is same as ceil, but %i %i %i is not empty", floor.x, floor.y, floor.z);
continue;
}
}
else
{
if(isSolid(Vector3int16(ceil.x, ceil.y, ceil.z), emptyCutoff) != EMPTY)
{
FASTLOG3(FLog::PathfindingDetail, "Skipping, %i %i %i is not empty", ceil.x, ceil.y, ceil.z);
continue;
}
if(isSolid(Vector3int16(floor.x, ceil.y, ceil.z), emptyCutoff) != EMPTY)
{
FASTLOG3(FLog::PathfindingDetail, "Skipping, %i %i %i is not empty", floor.x, ceil.y, ceil.z);
continue;
}
if(isSolid(Vector3int16(floor.x, ceil.y, floor.z), emptyCutoff) != EMPTY)
{
FASTLOG3(FLog::PathfindingDetail, "Skipping, %i %i %i is not empty", floor.x, ceil.y, floor.z);
continue;
}
if(isSolid(Vector3int16(ceil.x, ceil.y, floor.z), emptyCutoff) != EMPTY)
{
FASTLOG3(FLog::PathfindingDetail, "Skipping, %i %i %i is not empty", ceil.x, ceil.y, floor.z);
continue;
}
}
Vector3int16 cellBelow = Vector3int16(fastFloorInt(ave.x), fastFloorInt(ave.y-1), fastFloorInt(ave.z));
if(isSolid(cellBelow, emptyCutoff) != SOLID)
{
FASTLOG3(FLog::PathfindingDetail, "Skipping, nottom %i %i %i is not solid", cellBelow.x, cellBelow.y, cellBelow.z);
continue;
}
newPoints[i] = ave;
}
swap(points, newPoints);
}
bool PathfindingService::PathfindingState::checkOpenNodesCounts()
{
int countOpen = 0;
for(NodesByPos::iterator it = nodesByPos.begin(); it != nodesByPos.end(); ++it)
{
if(!it->second.closed)
countOpen++;
}
return countOpen == openNodesByWeight.size();
}
PathfindingService::Node PathfindingService::PathfindingState::popTopOpenNode()
{
RBXASSERT(openNodesByWeight.size() != 0);
OpenNodesByWeight::iterator top = openNodesByWeight.begin();
Node* result = &(top->second->second);
RBXASSERT(result->closed == false);
FASTLOG1F(FLog::PathfindingDetail, "---- Popping top open node, weight: %f", top->first);
FASTLOG3(FLog::PathfindingDetail, "Popping top open node, coordinate: %i %i %i", result->pos.x, result->pos.y, result->pos.z);
openNodesByWeight.erase(top);
result->itWeight = openNodesByWeight.end();
result->closed = true;
RBXASSERT_SLOW(checkOpenNodesCounts());
return *result;
}
bool PathfindingService::adjustCellSideways(Vector3int16& cellVoxel, Vector3& cellWorld, const Vector3int16& start, unsigned char emptyCutoff)
{
int bestDotResult = 0;
int bestDir = -1;
Vector3int16 direction = start - cellVoxel;
FASTLOG3(FLog::PathfindingDetail, "Adjusting cell sideways, cell: %i %i %i", cellVoxel.x, cellVoxel.y, cellVoxel.z);
FASTLOG3(FLog::PathfindingDetail, "Direction to start: %i %i %i", direction.x, direction.y, direction.z);
for(int dir = 0; dir < 4; dir++)
{
Vector3int16 offset = getOffsetByIndex(dir);
Vector3int16 side = cellVoxel + offset;
if(isSolid(side, emptyCutoff) == EMPTY)
{
FASTLOG4(FLog::PathfindingDetail, "Empty for dir %i, side cell: %i %i %i", dir, side.x, side.y, side.z);
int dotResult = direction.dot(offset);
if(bestDir == -1 || dotResult > bestDotResult)
{
FASTLOG1(FLog::PathfindingDetail, "Dot is better than old result: %i, storing as best", dotResult);
bestDir = dir;
bestDotResult = dotResult;
}
}
}
if(bestDir == -1)
return false;
cellVoxel = cellVoxel + getOffsetByIndex(bestDir);
cellWorld = cellWorld + Vector3(getOffsetByIndex(bestDir))*Voxel::kCELL_SIZE;
FASTLOG3(FLog::PathfindingDetail, "Final cell: %i %i %i", cellVoxel.x, cellVoxel.y, cellVoxel.z);
return true;
}
bool PathfindingService::adjustCell(Vector3int16& cellVoxel, Vector3& cellWorld, const Vector3int16& start, unsigned char emptyCutoff)
{
if(isSolid(cellVoxel, emptyCutoff) == EMPTY)
return true;
FASTLOG(FLog::PathfindingDetail, "Trying sides");
if(adjustCellSideways(cellVoxel, cellWorld, start, emptyCutoff))
return true;
FASTLOG(FLog::PathfindingDetail, "Trying up");
Vector3int16 cellVoxelUp = cellVoxel + Vector3int16(0,1,0);
Vector3 cellWorldUp = cellWorld + Vector3(0,Voxel::kCELL_SIZE,0);
if(isSolid(cellVoxelUp, emptyCutoff) == EMPTY)
{
cellVoxel = cellVoxelUp;
cellWorld = cellWorldUp;
return true;
}
FASTLOG(FLog::PathfindingDetail, "Trying up & sides");
if(adjustCellSideways(cellVoxelUp, cellWorldUp, start, emptyCutoff))
{
cellVoxel = cellVoxelUp;
cellWorld = cellWorldUp;
return true;
}
return false;
}
void PathfindingService::collectOldChunks()
{
FASTLOG(FLog::PathfindingPerf, "Starting collecting old chunks");
std::vector<SpatialRegion::Id> chunkIds = chunkMap.getChunks();
FASTLOG1(FLog::PathfindingDetail, "Collecting chunks, size: %u", chunkIds.size());
for(std::vector<SpatialRegion::Id>::iterator it = chunkIds.begin(); it != chunkIds.end(); ++it)
{
Voxel::OccupancyChunk* chunk = chunkMap.find(*it);
if((chunk->age + DFInt::PathfindingAgeToCollectChunks) < currentFrameNum)
{
if(*it == lastChunkId)
{
lastChunkId = SpatialRegion::Id(std::numeric_limits<G3D::int16>::max(), std::numeric_limits<G3D::int16>::max(), std::numeric_limits<G3D::int16>::max());
lastChunk = NULL;
}
FASTLOG3(FLog::PathfindingDetail, "Erasing chunk: %i %i %i", it->value().x, it->value().y, it->value().z);
chunkMap.erase(*it);
}
}
FASTLOG(FLog::PathfindingPerf, "Finish collecting chunks");
}
int manhattanDistance(const Vector3int16& start, const Vector3int16& finish)
{
Vector3int16 diff = finish - start;
return abs(diff.x) + abs(diff.y) + abs(diff.z);
}
int PathfindingService::countOrComputeDirtyChunks(const Vector3int16& startCell, const Vector3int16& finishCell, float maxDistance, bool compute)
{
RBXPROFILER_SCOPE("Voxel", "countOrComputeDirtyChunks");
int minPathDistance = manhattanDistance(startCell, finishCell);
int offset = std::max(0,(int)ceil((maxDistance - minPathDistance*Voxel::kCELL_SIZE) / (2*Voxel::kCELL_SIZE * Voxel::kXZ_CHUNK_SIZE)));
int offsetY = std::max(offset, DFInt::PathfindingVerticalChunkClamp);
DataModel* dataModel = DataModel::get(this);
ContactManager* contactManager = dataModel->getWorkspace()->getWorld()->getContactManager();
MegaClusterInstance* terrain = boost::polymorphic_downcast<MegaClusterInstance*>(dataModel->getWorkspace()->getTerrain());
Vector3int16 startId = SpatialRegion::regionContainingVoxel(startCell).value();
Vector3int16 finishId = SpatialRegion::regionContainingVoxel(finishCell).value();
Vector3int16 minId = startId.min(finishId) + Vector3int16(-offset,-offsetY,-offset);
Vector3int16 maxId = startId.max(finishId) + Vector3int16(offset,offsetY,offset);
FASTLOG3(FLog::PathfindingDetail, "Min voxel chunk: %i %i %i", minId.x, minId.y, minId.z);
FASTLOG3(FLog::PathfindingDetail, "Max voxel chunk: %i %i %i", maxId.x, maxId.y, maxId.z);
int count = 0;
// Voxelize
for(int x = minId.x; x <= maxId.x; x++)
for(int y = minId.y; y <= maxId.y; y++)
for(int z = minId.z; z <= maxId.z; z++)
{
FASTLOG3(FLog::PathfindingDetail, "Voxelizing: %i %i %i", x, y, z);
Voxel::OccupancyChunk* chunk = chunkMap.find(SpatialRegion::Id(x,y,z));
if(!chunk)
{
count++;
if(compute)
{
Voxel::OccupancyChunk& newChunk = chunkMap.insert(SpatialRegion::Id(x,y,z));
newChunk.index = Vector3int32(x,y,z);
newChunk.dirty = false;
newChunk.age = currentFrameNum;
voxelizer.occupancyUpdateChunk(newChunk, terrain, contactManager);
}
}
else if(chunk->dirty)
{
count++;
if(compute)
{
chunk->dirty = false;
chunk->age = currentFrameNum;
voxelizer.occupancyUpdateChunk(*chunk, terrain, contactManager);
}
}
}
FASTLOG1(FLog::PathfindingPerf, "Found %u dirty chunks", count);
return count;
}
PathfindingService::PathfindingState::PathfindingState() : nodesByPos(DFInt::PathfindingDefaultBucketNum)
{
}
void PathfindingService::computePath(CreatePathRequest& request)
{
RBXPROFILER_SCOPE("Voxel", "computePath");
Vector3int16 startCell = request.startCell;
Vector3int16 finishCell = request.finishCell;
unsigned char emptyCutoff = request.emptyCutoff;
int maxDistance = request.maxDistance;
if(!adjustCell(finishCell, request.start, startCell, emptyCutoff)) {
shared_ptr<Path> path = Creatable<Instance>::create<Path>(weak_from(this), Path::FailFinishNotEmpty);
request.resumeFunction(path);
return;
}
if(!adjustCell(startCell, request.finish, finishCell, emptyCutoff)) {
shared_ptr<Path> path = Creatable<Instance>::create<Path>(weak_from(this), Path::FailStartNotEmpty);
request.resumeFunction(path);
return;
}
PathfindingState state;
state.start = startCell;
state.finish = finishCell;
state.maxDistance = ((int)ceil(maxDistance/Voxel::kCELL_SIZE)) * PATHFINDING_WALK_COST;
state.closestCell = startCell;
state.closestDistance = INT_MAX;
Vector3int16 parentCell;
state.addOpenNode(Node(startCell, startCell, 0));
Path::PathStatus status = Path::ClosestNoPath;
while(state.openNodesByWeight.size() > 0)
{
Node node = state.popTopOpenNode();
if(node.pos == finishCell)
{
// Success!!
parentCell = node.parent;
FASTLOG3(FLog::PathfindingDetail, "Found a path from node %i %i %i", parentCell.x, parentCell.y, parentCell.z);
FASTLOG1F(FLog::PathfindingDetail, "Total path: %f", node.totalPath);
status = Path::Success;
break;
}
if(node.totalPath > state.maxDistance)
{
FASTLOG3(FLog::PathfindingDetail, "Found an outside node %i %i %i", node.pos.x, node.pos.y, node.pos.z);
// There's open space outside the range, take closest item
parentCell = state.closestCell;
FASTLOG3(FLog::PathfindingDetail, "Returning closest node %i %i %i", parentCell.x, parentCell.y, parentCell.z);
status = Path::ClosestOutOfRange;
break;
}
SolidState solidUnder = isSolid(node.pos + Vector3int16(0,-1,0), emptyCutoff);
FASTLOG1(FLog::PathfindingDetail, "Solid under: %i", solidUnder);
// If underneath is empty, the only thing to do is fall
if(solidUnder == EMPTY)
{
Vector3int16 newNodePos = node.pos + Vector3int16(0,-1,0);
state.addOpenNode(Node(newNodePos, node.pos, node.totalPath + PATHFINDING_FALL_COST));
}
else if(solidUnder == SOLID)
{
SideState sideState[4] = { BLOCKED, BLOCKED, BLOCKED, BLOCKED };
for(unsigned dir = 0; dir < 4; dir++)
{
Vector3int16 side = node.pos + getOffsetByIndex(dir);
Vector3int16 sideAbove = side + Vector3int16(0,1,0);
FASTLOG4(FLog::PathfindingDetail, "Dir: %u, cell to check: %i %i %i", dir, side.x, side.y, side.z);
SolidState solidAtSameLevel = isSolid(side, emptyCutoff);
SolidState solidAbove = isSolid(sideAbove, emptyCutoff);
FASTLOG2(FLog::PathfindingDetail, "Solid at same level: %i, solid above: %i", solidUnder, solidAbove);
if(solidAtSameLevel == EMPTY)
{
state.addOpenNode(Node(side, node.pos, node.totalPath + PATHFINDING_WALK_COST));
sideState[dir] = NOTBLOCKED;
}
else if(solidAtSameLevel == SOLID && solidAbove == EMPTY)
{
state.addOpenNode(Node(sideAbove, node.pos, node.totalPath + PATHFINDING_WALK_COST));
sideState[dir] = NOTBLOCKEDABOVE;
}
}
// Check diagonal states
for(unsigned dir = 0; dir < 4; dir++)
{
if(sideState[dir] != BLOCKED && sideState[dir] == sideState[(dir+1) % 4] )
{
Vector3int16 diagSide = node.pos + getDiagonalOffsetByIndex(dir);
Vector3int16 diagSideAbove = diagSide + Vector3int16(0,1,0);
FASTLOG4(FLog::PathfindingDetail, "Diag dir: %u, cell to check: %i %i %i", dir, diagSide.x, diagSide.y, diagSide.z);
SolidState solidAtSameLevel = isSolid(diagSide, emptyCutoff);
SolidState solidAbove = isSolid(diagSideAbove, emptyCutoff);
FASTLOG2(FLog::PathfindingDetail, "Solid at same level: %i, solid above: %i", solidUnder, solidAbove);
if(solidAtSameLevel == EMPTY && sideState[dir] == NOTBLOCKED)
{
state.addOpenNode(Node(diagSide, node.pos, node.totalPath + PATHFINDING_WALK_DIAGLONAL_COST));
}
else if(solidAtSameLevel == SOLID && solidAbove == EMPTY && sideState[dir] == NOTBLOCKEDABOVE)
{
state.addOpenNode(Node(diagSideAbove, node.pos, node.totalPath + PATHFINDING_WALK_DIAGLONAL_COST));
}
}
}
}
FASTLOG3(FLog::PathfindingDetail, "Adding closed node at %i %i %i", node.pos.x, node.pos.y, node.pos.z);
RBXASSERT(node.closed);
}
std::vector<Vector3> pathPoints;
shared_ptr<Path> path = Creatable<Instance>::create<Path>(weak_from(this), status);
if(status != Path::Success)
{
FASTLOG3(FLog::PathfindingDetail, "After checking everything, returning closest node %i %i %i", parentCell.x, parentCell.y, parentCell.z);
parentCell = state.closestCell;
}
else
{
pathPoints.push_back(request.isSmooth ? Voxel::worldSpaceToCellSpace(request.finish) : Vector3(request.finishCell)+Vector3(0.5f, 0.5f,0.5f));
}
while(parentCell != startCell)
{
pathPoints.push_back(Vector3(parentCell)+Vector3(0.5f,0.5f,0.5f));
NodesByPos::iterator itOpen = state.nodesByPos.find(parentCell);
RBXASSERT(itOpen != state.nodesByPos.end());
parentCell = itOpen->second.parent;
FASTLOG3(FLog::PathfindingDetail, "Steping back to %i %i %i", parentCell.x, parentCell.y, parentCell.z);
}
pathPoints.push_back(request.isSmooth ? Voxel::worldSpaceToCellSpace(request.start) : Vector3(request.startCell)+Vector3(0.5f, 0.5f, 0.5f));
std::reverse(pathPoints.begin(), pathPoints.end());
if(request.isSmooth)
{
for(int i = 0; i < DFInt::PathfindingSmoothIterations; i++)
smoothPath(pathPoints, emptyCutoff, DFInt::PathfindingAverageWindow);
}
for(std::vector<Vector3>::iterator it = pathPoints.begin(); it != pathPoints.end(); ++it)
{
path->addPoint(*it);
}
FASTLOG1(FLog::PathfindingPerf, "Result: ", state.openNodesByWeight.size() > 0);
request.resumeFunction(path);
}
void PathfindingService::computeRawPathAsync(Vector3 start, Vector3 finish, float maxDistance, boost::function<void(shared_ptr<Instance>) > resumeFunction, boost::function<void(std::string)> errorFunction)
{
computePathAsync(start, finish, maxDistance, false, resumeFunction, errorFunction);
}
void PathfindingService::computeSmoothPathAsync(Vector3 start, Vector3 finish, float maxDistance, boost::function<void(shared_ptr<Instance>) > resumeFunction, boost::function<void(std::string)> errorFunction)
{
computePathAsync(start, finish, maxDistance, true, resumeFunction, errorFunction);
}
void PathfindingService::computePathAsync(Vector3 start, Vector3 finish, float maxDistance, bool isSmooth, boost::function<void(shared_ptr<Instance>) > resumeFunction, boost::function<void(std::string)> errorFunction)
{
{
static boost::once_flag flag = BOOST_ONCE_INIT;
DataModel* dm = DataModel::get(this);
boost::call_once(flag, boost::bind(&RobloxGoogleAnalytics::trackEvent,GA_CATEGORY_GAME, "PathfindingService",
RBX::StringConverter<int>::convertToString(dm->getPlaceID()).c_str(), 0,false));
}
if(maxDistance > DFInt::PathfindingMaxDistance)
{
errorFunction("MaxDistance is too large");
return;
}
if(start.max(finish).max() > kMaxWorldCoordinate)
{
errorFunction("Start or Finish are outside of supported range");
return;
}
if(start.min(finish).min() < -kMaxWorldCoordinate)
{
errorFunction("Start or Finish are outside of supported range");
return;
}
if (!job)
{
job.reset(new PathfindingJob(this));
TaskScheduler::singleton().add(job);
DataModel* dataModel = DataModel::get(this);
ContactManagerSpatialHash* cmSpatialHash = dataModel->getWorkspace()->getWorld()->getContactManager()->getSpatialHash();
cmSpatialHash->registerCoarseMovementCallback(this);
MegaClusterInstance* terrain = static_cast<MegaClusterInstance*>(dataModel->getWorkspace()->getTerrain());
if (terrain)
{
if (terrain->isSmooth())
terrain->getSmoothGrid()->connectListener(this);
else
terrain->getVoxelGrid()->connectListener(this);
}
}
FASTLOG1F(FLog::PathfindingPerf, "Pathfinding request, distance: %f", maxDistance);
FASTLOG4F(FLog::PathfindingDetail, "Pathfinding request, start: %f %f %f, max distance: %f", start.x, start.y, start.z, maxDistance);
FASTLOG3F(FLog::PathfindingDetail, "Pathfinding request, finish: %f %f %f", finish.x, finish.y, finish.z);
Vector3int16 startCell = Voxel::worldToCell_floor(start);
Vector3int16 finishCell = Voxel::worldToCell_floor(finish);
FASTLOG3(FLog::PathfindingDetail, "Starting cell: %i %i %i", startCell.x, startCell.y, startCell.z);
FASTLOG3(FLog::PathfindingDetail, "Finish cell: %i %i %i", finishCell.x, finishCell.y, finishCell.z);
CreatePathRequest request;
request.start = start;
request.finish = finish;
request.startCell = startCell;
request.finishCell = finishCell;
request.maxDistance = maxDistance;
request.resumeFunction = resumeFunction;
request.errorFunction = errorFunction;
request.isSmooth = isSmooth;
request.emptyCutoff = (unsigned char)(emptyCutoff*255.0f);
if(countOrComputeDirtyChunks(startCell, finishCell, maxDistance, false) == 0)
request.execute(this);
else
{
scheduleRequest(shared_ptr<PathfindingRequest>(new CreatePathRequest(request)));
}
FASTLOG(FLog::PathfindingPerf, "Finished computeRawPathAsync");
}
void PathfindingService::scheduleRequest(const shared_ptr<PathfindingRequest>& request)
{
throttledRequests.push_back(request);
}
void PathfindingService::CreatePathRequest::execute(PathfindingService* service)
{
service->computePath(*this);
}
PathfindingService::SolidState PathfindingService::isSolid(const Vector3int16& pos, unsigned char emptyCutoff)
{
SpatialRegion::Id id = SpatialRegion::regionContainingVoxel(pos);
Vector3int16 offset = SpatialRegion::voxelCoordinateRelativeToEnclosingRegion(pos);
Voxel::OccupancyChunk* chunk = getChunkById(id);
if(!chunk)
return OUTSIDE;
unsigned char occupancy = chunk->occupancy[offset.y][offset.z][offset.x];
return occupancy > emptyCutoff ? SOLID : EMPTY;
}
void PathfindingService::executeThrottledRequests()
{
currentFrameNum++;
if((currentFrameNum % DFInt::PathfindingCollectPeriod) == 0)
{
collectOldChunks();
}
if(throttledRequests.size() == 0)
return;
int budget = DFInt::PathfindingChunksPerInvokation;
FASTLOG(FLog::PathfindingPerf, "Started executing throttled requests");
FASTLOG1(FLog::PathfindingDetail, "Number of pending paths: %u, current frame: %u", throttledRequests.size());
do
{
RBXPROFILER_SCOPE("Voxel", "executeRequest");
shared_ptr<PathfindingRequest> request;
throttledRequests.pop_front(&request);
FASTLOG3(FLog::PathfindingDetail, "Popped request, start cell: %i %i %i", request->startCell.x, request->startCell.y, request->startCell.z);
FASTLOG3(FLog::PathfindingDetail, "Popped request, finish cell: %i %i %i", request->finishCell.x, request->finishCell.y, request->finishCell.z);
RBXPROFILER_LABELF("Voxel", "(%d %d %d) -> (%d %d %d)", request->startCell.x, request->startCell.y, request->startCell.z, request->finishCell.x, request->finishCell.y, request->finishCell.z);
FASTLOG(FLog::PathfindingPerf, "Started computing occupancy");
int chunksToCompute = countOrComputeDirtyChunks(request->startCell, request->finishCell, request->maxDistance, true);
budget -= chunksToCompute;
FASTLOG1(FLog::PathfindingPerf, "Finished computing occupancy, %u chunks", chunksToCompute);
FASTLOG2(FLog::PathfindingDetail, "Computed %u chunks, current budget: %i", chunksToCompute, budget);
FASTLOG(FLog::PathfindingPerf, "Started computing path");
request->execute(this);
FASTLOG(FLog::PathfindingPerf, "Finished computing path");
}
while (budget > 0 && throttledRequests.size() > 0);
FASTLOG(FLog::PathfindingPerf, "Finished executing throttled requests");
}
ExtentsInt32 hashToVoxelChunk(const ExtentsInt32& extents, int level)
{
ExtentsInt32 voxelCellCoords = extents.shiftLeft(level + 1);
Vector3int32 voxelShift = Vector3int32(5,4,5);
RBXASSERT((1 << voxelShift.x) == kVoxelChunkSizeXZ);
RBXASSERT((1 << voxelShift.y) == kVoxelChunkSizeY);
RBXASSERT((1 << voxelShift.z) == kVoxelChunkSizeXZ);
ExtentsInt32 voxelChunkCoords = voxelCellCoords.shiftRight(voxelShift);
return voxelChunkCoords;
}
void PathfindingService::markChunksDirty(const ExtentsInt32& extents)
{
for(int x = extents.low.x; x <= extents.high.x; x++)
for(int y = extents.low.y; y <= extents.high.y; y++)
for(int z = extents.low.z; z <= extents.high.z; z++)
{
Voxel::OccupancyChunk* chunk = getChunkById(SpatialRegion::Id(x,y,z));
if(chunk)
chunk->dirty = true;
}
}
void PathfindingService::coarsePrimitiveMovement(Primitive* p, const UpdateInfo& info)
{
PartInstance* part = PartInstance::fromPrimitive(p);
if(part->getCookie() & PartCookie::IS_HUMANOID_PART)
{
return;
}
ExtentsInt32 newVoxelExtents = hashToVoxelChunk(info.newSpatialExtents, info.newLevel);
markChunksDirty(newVoxelExtents);
if(info.updateType == UpdateInfo::UPDATE_TYPE_Change && info.newLevel != info.oldLevel && info.newSpatialExtents != info.oldSpatialExtents)
{
ExtentsInt32 oldVoxelExtents = hashToVoxelChunk(info.oldSpatialExtents, info.oldLevel);
markChunksDirty(oldVoxelExtents);
}
}
void PathfindingService::terrainCellChanged(const Voxel::CellChangeInfo& info)
{
if(info.afterCell.solid.getBlock() == info.beforeCell.solid.getBlock())
return;
SpatialRegion::Id chunkId = SpatialRegion::regionContainingVoxel(info.position);
Voxel::OccupancyChunk* chunk = getChunkById(chunkId);
if(chunk)
chunk->dirty = true;
}
void PathfindingService::onTerrainRegionChanged(const Voxel2::Region& region)
{
if (region.empty())
return;
SpatialRegion::Id regionIdMin = SpatialRegion::regionContainingVoxel(region.begin().toVector3int16());
SpatialRegion::Id regionIdMax = SpatialRegion::regionContainingVoxel(region.end().toVector3int16() - Vector3int16(1, 1, 1));
for (int y = regionIdMin.value().y; y <= regionIdMax.value().y; ++y)
for (int z = regionIdMin.value().z; z <= regionIdMax.value().z; ++z)
for (int x = regionIdMin.value().x; x <= regionIdMax.value().x; ++x)
if (Voxel::OccupancyChunk* chunk = getChunkById(SpatialRegion::Id(x, y, z)))
chunk->dirty = true;
}
void PathfindingService::onServiceProvider(ServiceProvider* oldProvider, ServiceProvider* newProvider)
{
if(oldProvider)
{
DataModel* dataModel = DataModel::get(oldProvider);
if(job)
{
if(dataModel->getWorkspace())
{
MegaClusterInstance* terrain = static_cast<MegaClusterInstance*>(dataModel->getWorkspace()->getTerrain());
if (terrain)
{
if (terrain->isSmooth())
terrain->getSmoothGrid()->disconnectListener(this);
else
terrain->getVoxelGrid()->disconnectListener(this);
}
}
ContactManagerSpatialHash* cmSpatialHash = dataModel->getWorkspace()->getWorld()->getContactManager()->getSpatialHash();
cmSpatialHash->unregisterCoarseMovementCallback(this);
TaskScheduler::singleton().removeBlocking(job);
job.reset();
}
}
}
int PathfindingService::checkPoints(int startPoint, const std::vector<Vector3>& points, unsigned char emptyCutoff)
{
for(std::vector<Vector3>::const_iterator it = points.begin() + startPoint; it != points.end(); ++it)
{
Vector3int16 cell = fastFloorInt16(*it);
if(isSolid(cell, emptyCutoff) != EMPTY)
return it - (points.begin() + startPoint);
}
return -1;
}
Voxel::OccupancyChunk* PathfindingService::getChunkById(const SpatialRegion::Id& id)
{
if(id == lastChunkId)
return lastChunk;
else
{
lastChunk = chunkMap.find(id);
lastChunkId = id;
return lastChunk;
}
}
const char* const sPath = "Path";
Path::Path(weak_ptr<PathfindingService> service, PathStatus status) : service(service), status(status)
{
}
void Path::addPoint(const Vector3& point)
{
points.push_back(point);
}
void Path::reverse()
{
std::reverse(points.begin(), points.end());
}
void Path::OcclusionRequest::execute(PathfindingService* service)
{
int result = service->checkPoints(startPoint, path->getPoints(), emptyCutoff);
resumeFunction(result == -1 ? -1 : (result + startPoint));
}
void Path::checkOcclusionAsync(int startPoint, boost::function<void(int)> resumeFunction, boost::function<void(std::string)> errorFunction)
{
shared_ptr<PathfindingService> pathfindingService = service.lock();
if(!pathfindingService)
{
errorFunction("PathfindingService no longer exists");
return;
}
if(startPoint >= (int)points.size() || startPoint < 0)
{
errorFunction("Start point value is invalid");
return;
}
Vector3 minPoint,maxPoint;
minPoint = points[startPoint];
maxPoint = points[startPoint];
for(int i = startPoint+1; i < (int)points.size(); i++)
{
Vector3 point = points[i];
minPoint = minPoint.min(point);
maxPoint = maxPoint.max(point);
}
Vector3int16 startCell = fastFloorInt16(minPoint);
Vector3int16 finishCell = fastFloorInt16(maxPoint);
int dirtyChunks = pathfindingService->countOrComputeDirtyChunks(startCell, finishCell, 0, false);
OcclusionRequest request;
request.startCell = startCell;
request.finishCell = finishCell;
request.startPoint = startPoint;
request.path = shared_from(this);
request.maxDistance = 0;
request.resumeFunction = resumeFunction;
request.errorFunction = errorFunction;
request.emptyCutoff = (unsigned char)(pathfindingService->getEmptyCutoff() * 255.0f);
if(dirtyChunks == 0)
{
request.execute(pathfindingService.get());
}
else
{
pathfindingService->scheduleRequest(shared_ptr<OcclusionRequest>(new OcclusionRequest(request)));
}
}
shared_ptr<const Reflection::ValueArray> Path::getPointCoordinates()
{
shared_ptr<Reflection::ValueArray> result(new Reflection::ValueArray);
for(std::vector<Vector3>::iterator it = points.begin(); it != points.end(); ++it)
{
Vector3 cellPos = *it;
result->push_back(Voxel::cellSpaceToWorldSpace(cellPos));
}
return result;
}
}
Reference in New Issue View Git Blame Copy Permalink