TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-13 23:54:35 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

Updated recast to 1.5.1

Browse source
This commit is contained in:
Johxz 2016-12-11 13:17:15 -06:00
parent 630949514a
commit c7e5b35744
55 changed files with 3277 additions and 1460 deletions
Download patch file Download diff file Expand all files Collapse all files

27
Engine/lib/recast/DetourCrowd/CMakeLists.txt
View file

@ -1,27 +0,0 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(detourcrowd_SRCS
Source/DetourPathCorridor.cpp
Source/DetourLocalBoundary.cpp
Source/DetourObstacleAvoidance.cpp
Source/DetourPathQueue.cpp
Source/DetourCrowd.cpp
Source/DetourProximityGrid.cpp
)
SET(detourcrowd_HDRS
Include/DetourPathCorridor.h
Include/DetourCrowd.h
Include/DetourObstacleAvoidance.h
Include/DetourLocalBoundary.h
Include/DetourProximityGrid.h
Include/DetourPathQueue.h
)
INCLUDE_DIRECTORIES(Include
../Detour/Include
../DetourTileCache
../Recast/Include
)
ADD_LIBRARY(DetourCrowd ${detourcrowd_SRCS} ${detourcrowd_HDRS})

53
Engine/lib/recast/DetourCrowd/Include/DetourCrowd.h
View file

@ -45,6 +45,12 @@ static const int DT_CROWDAGENT_MAX_CORNERS = 4;
/// dtCrowdAgentParams::obstacleAvoidanceType
static const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8;
/// The maximum number of query filter types supported by the crowd manager.
/// @ingroup crowd
/// @see dtQueryFilter, dtCrowd::getFilter() dtCrowd::getEditableFilter(),
/// dtCrowdAgentParams::queryFilterType
static const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16;
/// Provides neighbor data for agents managed by the crowd.
/// @ingroup crowd
/// @see dtCrowdAgent::neis, dtCrowd
@ -87,6 +93,9 @@ struct dtCrowdAgentParams
/// [Limits: 0 <= value <= #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]
unsigned char obstacleAvoidanceType;
/// The index of the query filter used by this agent.
unsigned char queryFilterType;
/// User defined data attached to the agent.
void* userData;
};
@ -106,12 +115,15 @@ enum MoveRequestState
/// @ingroup crowd
struct dtCrowdAgent
{
/// 1 if the agent is active, or 0 if the agent is in an unused slot in the agent pool.
unsigned char active;
/// True if the agent is active, false if the agent is in an unused slot in the agent pool.
bool active;
/// The type of mesh polygon the agent is traversing. (See: #CrowdAgentState)
unsigned char state;
/// True if the agent has valid path (targetState == DT_CROWDAGENT_TARGET_VALID) and the path does not lead to the requested position, else false.
bool partial;
/// The path corridor the agent is using.
dtPathCorridor corridor;
@ -131,10 +143,10 @@ struct dtCrowdAgent
float desiredSpeed;
float npos[3]; ///< The current agent position. [(x, y, z)]
float disp[3];
float dvel[3]; ///< The desired velocity of the agent. [(x, y, z)]
float nvel[3];
float vel[3]; ///< The actual velocity of the agent. [(x, y, z)]
float disp[3]; ///< A temporary value used to accumulate agent displacement during iterative collision resolution. [(x, y, z)]
float dvel[3]; ///< The desired velocity of the agent. Based on the current path, calculated from scratch each frame. [(x, y, z)]
float nvel[3]; ///< The desired velocity adjusted by obstacle avoidance, calculated from scratch each frame. [(x, y, z)]
float vel[3]; ///< The actual velocity of the agent. The change from nvel -> vel is constrained by max acceleration. [(x, y, z)]
/// The agent's configuration parameters.
dtCrowdAgentParams params;
@ -161,7 +173,7 @@ struct dtCrowdAgent
struct dtCrowdAgentAnimation
{
unsigned char active;
bool active;
float initPos[3], startPos[3], endPos[3];
dtPolyRef polyRef;
float t, tmax;
@ -206,8 +218,9 @@ class dtCrowd
int m_maxPathResult;
float m_ext[3];
dtQueryFilter m_filter;
dtQueryFilter m_filters[DT_CROWD_MAX_QUERY_FILTER_TYPE];
float m_maxAgentRadius;
int m_velocitySampleCount;
@ -218,7 +231,7 @@ class dtCrowd
void updateMoveRequest(const float dt);
void checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt);
inline int getAgentIndex(const dtCrowdAgent* agent) const { return agent - m_agents; }
inline int getAgentIndex(const dtCrowdAgent* agent) const { return (int)(agent - m_agents); }
bool requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos);
@ -251,9 +264,14 @@ public:
/// @return The requested agent.
const dtCrowdAgent* getAgent(const int idx);
/// Gets the specified agent from the pool.
/// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()]
/// @return The requested agent.
dtCrowdAgent* getEditableAgent(const int idx);
/// The maximum number of agents that can be managed by the object.
/// @return The maximum number of agents.
const int getAgentCount() const;
int getAgentCount() const;
/// Adds a new agent to the crowd.
/// @param[in] pos The requested position of the agent. [(x, y, z)]
@ -301,11 +319,11 @@ public:
/// Gets the filter used by the crowd.
/// @return The filter used by the crowd.
const dtQueryFilter* getFilter() const { return &m_filter; }
inline const dtQueryFilter* getFilter(const int i) const { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
/// Gets the filter used by the crowd.
/// @return The filter used by the crowd.
dtQueryFilter* getEditableFilter() { return &m_filter; }
inline dtQueryFilter* getEditableFilter(const int i) { return (i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE) ? &m_filters[i] : 0; }
/// Gets the search extents [(x, y, z)] used by the crowd for query operations.
/// @return The search extents used by the crowd. [(x, y, z)]
@ -325,6 +343,11 @@ public:
/// Gets the query object used by the crowd.
const dtNavMeshQuery* getNavMeshQuery() const { return m_navquery; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
dtCrowd(const dtCrowd&);
dtCrowd& operator=(const dtCrowd&);
};
/// Allocates a crowd object using the Detour allocator.
@ -429,4 +452,4 @@ This value is often based on the agent radius. E.g. radius * 30
A higher value will result in agents trying to stay farther away from each other at
the cost of more difficult steering in tight spaces.
*/
*/

7
Engine/lib/recast/DetourCrowd/Include/DetourLocalBoundary.h
View file

@ -40,7 +40,7 @@ class dtLocalBoundary
dtPolyRef m_polys[MAX_LOCAL_POLYS];
int m_npolys;
void addSegment(const float dist, const float* seg);
void addSegment(const float dist, const float* s);
public:
dtLocalBoundary();
@ -56,6 +56,11 @@ public:
inline const float* getCenter() const { return m_center; }
inline int getSegmentCount() const { return m_nsegs; }
inline const float* getSegment(int i) const { return m_segs[i].s; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
dtLocalBoundary(const dtLocalBoundary&);
dtLocalBoundary& operator=(const dtLocalBoundary&);
};
#endif // DETOURLOCALBOUNDARY_H

11
Engine/lib/recast/DetourCrowd/Include/DetourObstacleAvoidance.h
View file

@ -58,6 +58,10 @@ public:
inline float getSampleCollisionTimePenalty(const int i) const { return m_tpen[i]; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
dtObstacleAvoidanceDebugData(const dtObstacleAvoidanceDebugData&);
dtObstacleAvoidanceDebugData& operator=(const dtObstacleAvoidanceDebugData&);
int m_nsamples;
int m_maxSamples;
float* m_vel;
@ -122,17 +126,18 @@ public:
const dtObstacleSegment* getObstacleSegment(const int i) { return &m_segments[i]; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
dtObstacleAvoidanceQuery(const dtObstacleAvoidanceQuery&);
dtObstacleAvoidanceQuery& operator=(const dtObstacleAvoidanceQuery&);
void prepare(const float* pos, const float* dvel);
float processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float* vel, const float* dvel,
const float minPenalty,
dtObstacleAvoidanceDebugData* debug);
dtObstacleCircle* insertCircle(const float dist);
dtObstacleSegment* insertSegment(const float dist);
dtObstacleAvoidanceParams m_params;
float m_invHorizTime;
float m_vmax;

13
Engine/lib/recast/DetourCrowd/Include/DetourPathCorridor.h
View file

@ -92,14 +92,16 @@ public:
/// @param[in] npos The desired new position. [(x, y, z)]
/// @param[in] navquery The query object used to build the corridor.
/// @param[in] filter The filter to apply to the operation.
void movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
/// @return Returns true if move succeeded.
bool movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
/// Moves the target from the curent location to the desired location, adjusting the corridor
/// as needed to reflect the change.
/// @param[in] npos The desired new target position. [(x, y, z)]
/// @param[in] navquery The query object used to build the corridor.
/// @param[in] filter The filter to apply to the operation.
void moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
/// @return Returns true if move succeeded.
bool moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter);
/// Loads a new path and target into the corridor.
/// @param[in] target The target location within the last polygon of the path. [(x, y, z)]
@ -129,7 +131,12 @@ public:
/// The number of polygons in the current corridor path.
/// @return The number of polygons in the current corridor path.
inline int getPathCount() const { return m_npath; }
inline int getPathCount() const { return m_npath; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
dtPathCorridor(const dtPathCorridor&);
dtPathCorridor& operator=(const dtPathCorridor&);
};
int dtMergeCorridorStartMoved(dtPolyRef* path, const int npath, const int maxPath,

4
Engine/lib/recast/DetourCrowd/Include/DetourPathQueue.h
View file

@ -70,6 +70,10 @@ public:
inline const dtNavMeshQuery* getNavQuery() const { return m_navquery; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
dtPathQueue(const dtPathQueue&);
dtPathQueue& operator=(const dtPathQueue&);
};
#endif // DETOURPATHQUEUE_H

10
Engine/lib/recast/DetourCrowd/Include/DetourProximityGrid.h
View file

@ -21,7 +21,6 @@
class dtProximityGrid
{
int m_maxItems;
float m_cellSize;
float m_invCellSize;
@ -44,7 +43,7 @@ public:
dtProximityGrid();
~dtProximityGrid();
bool init(const int maxItems, const float cellSize);
bool init(const int poolSize, const float cellSize);
void clear();
@ -59,7 +58,12 @@ public:
int getItemCountAt(const int x, const int y) const;
inline const int* getBounds() const { return m_bounds; }
inline const float getCellSize() const { return m_cellSize; }
inline float getCellSize() const { return m_cellSize; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
dtProximityGrid(const dtProximityGrid&);
dtProximityGrid& operator=(const dtProximityGrid&);
};
dtProximityGrid* dtAllocProximityGrid();

128
Engine/lib/recast/DetourCrowd/Source/DetourCrowd.cpp
View file

@ -17,7 +17,6 @@
//
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include <float.h>
#include <stdlib.h>
@ -27,6 +26,7 @@
#include "DetourNavMeshQuery.h"
#include "DetourObstacleAvoidance.h"
#include "DetourCommon.h"
#include "DetourMath.h"
#include "DetourAssert.h"
#include "DetourAlloc.h"
@ -206,7 +206,7 @@ static int getNeighbours(const float* pos, const float height, const float range
// Check for overlap.
float diff[3];
dtVsub(diff, pos, ag->npos);
if (fabsf(diff[1]) >= (height+ag->params.height)/2.0f)
if (dtMathFabsf(diff[1]) >= (height+ag->params.height)/2.0f)
continue;
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
@ -441,14 +441,14 @@ bool dtCrowd::init(const int maxAgents, const float maxAgentRadius, dtNavMesh* n
for (int i = 0; i < m_maxAgents; ++i)
{
new(&m_agents[i]) dtCrowdAgent();
m_agents[i].active = 0;
m_agents[i].active = false;
if (!m_agents[i].corridor.init(m_maxPathResult))
return false;
}
for (int i = 0; i < m_maxAgents; ++i)
{
m_agentAnims[i].active = 0;
m_agentAnims[i].active = false;
}
// The navquery is mostly used for local searches, no need for large node pool.
@ -474,7 +474,7 @@ const dtObstacleAvoidanceParams* dtCrowd::getObstacleAvoidanceParams(const int i
return 0;
}
const int dtCrowd::getAgentCount() const
int dtCrowd::getAgentCount() const
{
return m_maxAgents;
}
@ -484,12 +484,23 @@ const int dtCrowd::getAgentCount() const
/// Agents in the pool may not be in use. Check #dtCrowdAgent.active before using the returned object.
const dtCrowdAgent* dtCrowd::getAgent(const int idx)
{
if (idx < 0 || idx >= m_maxAgents)
return 0;
return &m_agents[idx];
}
///
/// Agents in the pool may not be in use. Check #dtCrowdAgent.active before using the returned object.
dtCrowdAgent* dtCrowd::getEditableAgent(const int idx)
{
if (idx < 0 || idx >= m_maxAgents)
return 0;
return &m_agents[idx];
}
void dtCrowd::updateAgentParameters(const int idx, const dtCrowdAgentParams* params)
{
if (idx < 0 || idx > m_maxAgents)
if (idx < 0 || idx >= m_maxAgents)
return;
memcpy(&m_agents[idx].params, params, sizeof(dtCrowdAgentParams));
}
@ -512,18 +523,25 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
if (idx == -1)
return -1;
dtCrowdAgent* ag = &m_agents[idx];
// Find nearest position on navmesh and place the agent there.
float nearest[3];
dtPolyRef ref;
m_navquery->findNearestPoly(pos, m_ext, &m_filter, &ref, nearest);
ag->corridor.reset(ref, nearest);
ag->boundary.reset();
dtCrowdAgent* ag = &m_agents[idx];
updateAgentParameters(idx, params);
// Find nearest position on navmesh and place the agent there.
float nearest[3];
dtPolyRef ref = 0;
dtVcopy(nearest, pos);
dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest);
if (dtStatusFailed(status))
{
dtVcopy(nearest, pos);
ref = 0;
}
ag->corridor.reset(ref, nearest);
ag->boundary.reset();
ag->partial = false;
ag->topologyOptTime = 0;
ag->targetReplanTime = 0;
ag->nneis = 0;
@ -542,7 +560,7 @@ int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
ag->active = 1;
ag->active = true;
return idx;
}
@ -555,13 +573,13 @@ void dtCrowd::removeAgent(const int idx)
{
if (idx >= 0 && idx < m_maxAgents)
{
m_agents[idx].active = 0;
m_agents[idx].active = false;
}
}
bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float* pos)
{
if (idx < 0 || idx > m_maxAgents)
if (idx < 0 || idx >= m_maxAgents)
return false;
dtCrowdAgent* ag = &m_agents[idx];
@ -588,7 +606,7 @@ bool dtCrowd::requestMoveTargetReplan(const int idx, dtPolyRef ref, const float*
/// The request will be processed during the next #update().
bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
{
if (idx < 0 || idx > m_maxAgents)
if (idx < 0 || idx >= m_maxAgents)
return false;
if (!ref)
return false;
@ -610,7 +628,7 @@ bool dtCrowd::requestMoveTarget(const int idx, dtPolyRef ref, const float* pos)
bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
{
if (idx < 0 || idx > m_maxAgents)
if (idx < 0 || idx >= m_maxAgents)
return false;
dtCrowdAgent* ag = &m_agents[idx];
@ -627,7 +645,7 @@ bool dtCrowd::requestMoveVelocity(const int idx, const float* vel)
bool dtCrowd::resetMoveTarget(const int idx)
{
if (idx < 0 || idx > m_maxAgents)
if (idx < 0 || idx >= m_maxAgents)
return false;
dtCrowdAgent* ag = &m_agents[idx];
@ -635,6 +653,7 @@ bool dtCrowd::resetMoveTarget(const int idx)
// Initialize request.
ag->targetRef = 0;
dtVset(ag->targetPos, 0,0,0);
dtVset(ag->dvel, 0,0,0);
ag->targetPathqRef = DT_PATHQ_INVALID;
ag->targetReplan = false;
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
@ -683,9 +702,9 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
dtPolyRef reqPath[MAX_RES]; // The path to the request location
int reqPathCount = 0;
// Quick seach towards the goal.
// Quick search towards the goal.
static const int MAX_ITER = 20;
m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filter);
m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filters[ag->params.queryFilterType]);
m_navquery->updateSlicedFindPath(MAX_ITER, 0);
dtStatus status = 0;
if (ag->targetReplan) // && npath > 10)
@ -705,7 +724,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
if (reqPath[reqPathCount-1] != ag->targetRef)
{
// Partial path, constrain target position inside the last polygon.
status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos);
status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
if (dtStatusFailed(status))
reqPathCount = 0;
}
@ -729,6 +748,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
ag->boundary.reset();
ag->partial = false;
if (reqPath[reqPathCount-1] == ag->targetRef)
{
@ -752,7 +772,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
{
dtCrowdAgent* ag = queue[i];
ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
ag->corridor.getTarget(), ag->targetPos, &m_filter);
ag->corridor.getTarget(), ag->targetPos, &m_filters[ag->params.queryFilterType]);
if (ag->targetPathqRef != DT_PATHQ_INVALID)
ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
}
@ -802,7 +822,12 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
if (dtStatusFailed(status) || !nres)
valid = false;
if (dtStatusDetail(status, DT_PARTIAL_RESULT))
ag->partial = true;
else
ag->partial = false;
// Merge result and existing path.
// The agent might have moved whilst the request is
// being processed, so the path may have changed.
@ -849,7 +874,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
{
// Partial path, constrain target position inside the last polygon.
float nearest[3];
status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest);
status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
if (dtStatusSucceed(status))
dtVcopy(targetPos, nearest);
else
@ -906,7 +931,7 @@ void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagent
for (int i = 0; i < nqueue; ++i)
{
dtCrowdAgent* ag = queue[i];
ag->corridor.optimizePathTopology(m_navquery, &m_filter);
ag->corridor.optimizePathTopology(m_navquery, &m_filters[ag->params.queryFilterType]);
ag->topologyOptTime = 0;
}
@ -923,9 +948,6 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
ag->targetReplanTime += dt;
@ -936,19 +958,21 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
float agentPos[3];
dtPolyRef agentRef = ag->corridor.getFirstPoly();
dtVcopy(agentPos, ag->npos);
if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
if (!m_navquery->isValidPolyRef(agentRef, &m_filters[ag->params.queryFilterType]))
{
// Current location is not valid, try to reposition.
// TODO: this can snap agents, how to handle that?
float nearest[3];
dtVcopy(nearest, agentPos);
agentRef = 0;
m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
m_navquery->findNearestPoly(ag->npos, m_ext, &m_filters[ag->params.queryFilterType], &agentRef, nearest);
dtVcopy(agentPos, nearest);
if (!agentRef)
{
// Could not find location in navmesh, set state to invalid.
ag->corridor.reset(0, agentPos);
ag->partial = false;
ag->boundary.reset();
ag->state = DT_CROWDAGENT_STATE_INVALID;
continue;
@ -964,14 +988,20 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
replan = true;
}
// If the agent does not have move target or is controlled by velocity, no need to recover the target nor replan.
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Try to recover move request position.
if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
{
if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filter))
if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filters[ag->params.queryFilterType]))
{
// Current target is not valid, try to reposition.
float nearest[3];
m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filter, &ag->targetRef, nearest);
dtVcopy(nearest, ag->targetPos);
ag->targetRef = 0;
m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filters[ag->params.queryFilterType], &ag->targetRef, nearest);
dtVcopy(ag->targetPos, nearest);
replan = true;
}
@ -979,12 +1009,13 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
{
// Failed to reposition target, fail moverequest.
ag->corridor.reset(agentRef, agentPos);
ag->partial = false;
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
}
}
// If nearby corridor is not valid, replan.
if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filters[ag->params.queryFilterType]))
{
// Fix current path.
// ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
@ -1020,7 +1051,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
dtCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths.
checkPathValidity(agents, nagents, dt);
@ -1051,10 +1082,10 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filter))
!ag->boundary.isValid(m_navquery, &m_filters[ag->params.queryFilterType]))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
m_navquery, &m_filters[ag->params.queryFilterType]);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
@ -1076,14 +1107,14 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filters[ag->params.queryFilterType]);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
{
const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filters[ag->params.queryFilterType]);
// Copy data for debug purposes.
if (debugIdx == i)
@ -1118,7 +1149,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = ag - m_agents;
const int idx = (int)(ag - m_agents);
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
// Adjust the path over the off-mesh connection.
@ -1128,7 +1159,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->active = true;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
@ -1200,7 +1231,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
continue;
if (distSqr > dtSqr(separationDist))
continue;
const float dist = sqrtf(distSqr);
const float dist = dtMathSqrtf(distSqr);
const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
dtVmad(disp, disp, diff, weight/dist);
@ -1318,7 +1349,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = sqrtf(dist);
dist = dtMathSqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
@ -1363,7 +1394,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
continue;
// Move along navmesh.
ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
ag->corridor.movePosition(ag->npos, m_navquery, &m_filters[ag->params.queryFilterType]);
// Get valid constrained position back.
dtVcopy(ag->npos, ag->corridor.getPos());
@ -1371,6 +1402,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
ag->partial = false;
}
}
@ -1387,7 +1419,7 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
anim->active = false;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
@ -1413,5 +1445,3 @@ void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
}
}

110
Engine/lib/recast/DetourCrowd/Source/DetourObstacleAvoidance.cpp
View file

@ -18,10 +18,10 @@
#include "DetourObstacleAvoidance.h"
#include "DetourCommon.h"
#include "DetourMath.h"
#include "DetourAlloc.h"
#include "DetourAssert.h"
#include <string.h>
#include <math.h>
#include <float.h>
#include <new>
@ -44,7 +44,7 @@ static int sweepCircleCircle(const float* c0, const float r0, const float* v,
float d = b*b - a*c;
if (d < 0.0f) return 0; // no intersection.
a = 1.0f / a;
const float rd = dtSqrt(d);
const float rd = dtMathSqrtf(d);
tmin = (b - rd) * a;
tmax = (b + rd) * a;
return 1;
@ -58,7 +58,7 @@ static int isectRaySeg(const float* ap, const float* u,
dtVsub(v,bq,bp);
dtVsub(w,ap,bp);
float d = dtVperp2D(u,v);
if (fabsf(d) < 1e-6f) return 0;
if (dtMathFabsf(d) < 1e-6f) return 0;
d = 1.0f/d;
t = dtVperp2D(v,w) * d;
if (t < 0 || t > 1) return 0;
@ -262,7 +262,7 @@ void dtObstacleAvoidanceQuery::addCircle(const float* pos, const float rad,
void dtObstacleAvoidanceQuery::addSegment(const float* p, const float* q)
{
if (m_nsegments > m_maxSegments)
if (m_nsegments >= m_maxSegments)
return;
dtObstacleSegment* seg = &m_segments[m_nsegments++];
@ -281,7 +281,7 @@ void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel)
const float* pa = pos;
const float* pb = cir->p;
const float orig[3] = {0,0};
const float orig[3] = {0,0,0};
float dv[3];
dtVsub(cir->dp,pb,pa);
dtVnormalize(cir->dp);
@ -311,11 +311,30 @@ void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel)
}
}
/* Calculate the collision penalty for a given velocity vector
*
* @param vcand sampled velocity
* @param dvel desired velocity
* @param minPenalty threshold penalty for early out
*/
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float* vel, const float* dvel,
const float minPenalty,
dtObstacleAvoidanceDebugData* debug)
{
// penalty for straying away from the desired and current velocities
const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
// find the threshold hit time to bail out based on the early out penalty
// (see how the penalty is calculated below to understnad)
float minPen = minPenalty - vpen - vcpen;
float tThresold = (m_params.weightToi / minPen - 0.1f) * m_params.horizTime;
if (tThresold - m_params.horizTime > -FLT_EPSILON)
return minPenalty; // already too much
// Find min time of impact and exit amongst all obstacles.
float tmin = m_params.horizTime;
float side = 0;
@ -350,7 +369,11 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
return minPenalty;
}
}
}
@ -383,15 +406,17 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
return minPenalty;
}
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
const float vpen = m_params.weightDesVel * (dtVdist2D(vcand, dvel) * m_invVmax);
const float vcpen = m_params.weightCurVel * (dtVdist2D(vcand, vel) * m_invVmax);
const float spen = m_params.weightSide * side;
const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
@ -414,7 +439,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = 1.0f / vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
dtVset(nvel, 0,0,0);
@ -440,7 +465,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+cs/2)) continue;
const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, debug);
const float penalty = processSample(vcand, cs, pos,rad,vel,dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
@ -454,6 +479,28 @@ int dtObstacleAvoidanceQuery::sampleVelocityGrid(const float* pos, const float r
}
// vector normalization that ignores the y-component.
inline void dtNormalize2D(float* v)
{
float d = dtMathSqrtf(v[0] * v[0] + v[2] * v[2]);
if (d==0)
return;
d = 1.0f / d;
v[0] *= d;
v[2] *= d;
}
// vector normalization that ignores the y-component.
inline void dtRorate2D(float* dest, const float* v, float ang)
{
float c = cosf(ang);
float s = sinf(ang);
dest[0] = v[0]*c - v[2]*s;
dest[2] = v[0]*s + v[2]*c;
dest[1] = v[1];
}
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel,
const dtObstacleAvoidanceParams* params,
@ -464,7 +511,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = 1.0f / vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
dtVset(nvel, 0,0,0);
@ -482,8 +529,15 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS);
const float da = (1.0f/nd) * DT_PI*2;
const float dang = atan2f(dvel[2], dvel[0]);
const float ca = cosf(da);
const float sa = sinf(da);
// desired direction
float ddir[6];
dtVcopy(ddir, dvel);
dtNormalize2D(ddir);
dtRorate2D (ddir+3, ddir, da*0.5f); // rotated by da/2
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
@ -492,16 +546,35 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
for (int j = 0; j < nr; ++j)
{
const float r = (float)(nr-j)/(float)nr;
float a = dang + (j&1)*0.5f*da;
for (int i = 0; i < nd; ++i)
pat[npat*2+0] = ddir[(j%2)*3] * r;
pat[npat*2+1] = ddir[(j%2)*3+2] * r;
float* last1 = pat + npat*2;
float* last2 = last1;
npat++;
for (int i = 1; i < nd-1; i+=2)
{
pat[npat*2+0] = cosf(a)*r;
pat[npat*2+1] = sinf(a)*r;
// get next point on the "right" (rotate CW)
pat[npat*2+0] = last1[0]*ca + last1[1]*sa;
pat[npat*2+1] = -last1[0]*sa + last1[1]*ca;
// get next point on the "left" (rotate CCW)
pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
last1 = pat + npat*2;
last2 = last1 + 2;
npat += 2;
}
if ((nd&1) == 0)
{
pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
npat++;
a += da;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
float res[3];
@ -523,7 +596,7 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug);
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
@ -541,4 +614,3 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo
return ns;
}

53
Engine/lib/recast/DetourCrowd/Source/DetourPathCorridor.cpp
View file

@ -436,7 +436,7 @@ depends on local polygon density, query search extents, etc.
The resulting position will differ from the desired position if the desired position is not on the navigation mesh,
or it can't be reached using a local search.
*/
void dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
bool dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
dtAssert(m_path);
dtAssert(m_npath);
@ -446,15 +446,19 @@ void dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, c
static const int MAX_VISITED = 16;
dtPolyRef visited[MAX_VISITED];
int nvisited = 0;
navquery->moveAlongSurface(m_path[0], m_pos, npos, filter,
result, visited, &nvisited, MAX_VISITED);
m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// Adjust the position to stay on top of the navmesh.
float h = m_pos[1];
navquery->getPolyHeight(m_path[0], result, &h);
result[1] = h;
dtVcopy(m_pos, result);
dtStatus status = navquery->moveAlongSurface(m_path[0], m_pos, npos, filter,
result, visited, &nvisited, MAX_VISITED);
if (dtStatusSucceed(status)) {
m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// Adjust the position to stay on top of the navmesh.
float h = m_pos[1];
navquery->getPolyHeight(m_path[0], result, &h);
result[1] = h;
dtVcopy(m_pos, result);
return true;
}
return false;
}
/**
@ -470,7 +474,7 @@ The expected use case is that the desired target will be 'near' the current corr
The resulting target will differ from the desired target if the desired target is not on the navigation mesh, or it can't be reached using a local search.
*/
void dtPathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
bool dtPathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
dtAssert(m_path);
dtAssert(m_npath);
@ -480,17 +484,22 @@ void dtPathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navqu
static const int MAX_VISITED = 16;
dtPolyRef visited[MAX_VISITED];
int nvisited = 0;
navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter,
result, visited, &nvisited, MAX_VISITED);
m_npath = dtMergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// TODO: should we do that?
// Adjust the position to stay on top of the navmesh.
/* float h = m_target[1];
navquery->getPolyHeight(m_path[m_npath-1], result, &h);
result[1] = h;*/
dtVcopy(m_target, result);
dtStatus status = navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter,
result, visited, &nvisited, MAX_VISITED);
if (dtStatusSucceed(status))
{
m_npath = dtMergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// TODO: should we do that?
// Adjust the position to stay on top of the navmesh.
/* float h = m_target[1];
navquery->getPolyHeight(m_path[m_npath-1], result, &h);
result[1] = h;*/
dtVcopy(m_target, result);
return true;
}
return false;
}
/// @par

3
Engine/lib/recast/DetourCrowd/Source/DetourPathQueue.cpp
View file

@ -185,6 +185,7 @@ dtStatus dtPathQueue::getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pa
if (m_queue[i].ref == ref)
{
PathQuery& q = m_queue[i];
dtStatus details = q.status & DT_STATUS_DETAIL_MASK;
// Free request for reuse.
q.ref = DT_PATHQ_INVALID;
q.status = 0;
@ -192,7 +193,7 @@ dtStatus dtPathQueue::getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pa
int n = dtMin(q.npath, maxPath);
memcpy(path, q.path, sizeof(dtPolyRef)*n);
*pathSize = n;
return DT_SUCCESS;
return details | DT_SUCCESS;
}
}
return DT_FAILURE;

19
Engine/lib/recast/DetourCrowd/Source/DetourProximityGrid.cpp
View file

@ -16,11 +16,11 @@
// 3. This notice may not be removed or altered from any source distribution.
//
#include <math.h>
#include <string.h>
#include <new>
#include "DetourProximityGrid.h"
#include "DetourCommon.h"
#include "DetourMath.h"
#include "DetourAlloc.h"
#include "DetourAssert.h"
@ -47,7 +47,6 @@ inline int hashPos2(int x, int y, int n)
dtProximityGrid::dtProximityGrid() :
m_maxItems(0),
m_cellSize(0),
m_pool(0),
m_poolHead(0),
@ -103,10 +102,10 @@ void dtProximityGrid::addItem(const unsigned short id,
const float minx, const float miny,
const float maxx, const float maxy)
{
const int iminx = (int)floorf(minx * m_invCellSize);
const int iminy = (int)floorf(miny * m_invCellSize);
const int imaxx = (int)floorf(maxx * m_invCellSize);
const int imaxy = (int)floorf(maxy * m_invCellSize);
const int iminx = (int)dtMathFloorf(minx * m_invCellSize);
const int iminy = (int)dtMathFloorf(miny * m_invCellSize);
const int imaxx = (int)dtMathFloorf(maxx * m_invCellSize);
const int imaxy = (int)dtMathFloorf(maxy * m_invCellSize);
m_bounds[0] = dtMin(m_bounds[0], iminx);
m_bounds[1] = dtMin(m_bounds[1], iminy);
@ -137,10 +136,10 @@ int dtProximityGrid::queryItems(const float minx, const float miny,
const float maxx, const float maxy,
unsigned short* ids, const int maxIds) const
{
const int iminx = (int)floorf(minx * m_invCellSize);
const int iminy = (int)floorf(miny * m_invCellSize);
const int imaxx = (int)floorf(maxx * m_invCellSize);
const int imaxy = (int)floorf(maxy * m_invCellSize);
const int iminx = (int)dtMathFloorf(minx * m_invCellSize);
const int iminy = (int)dtMathFloorf(miny * m_invCellSize);
const int imaxx = (int)dtMathFloorf(maxx * m_invCellSize);
const int imaxy = (int)dtMathFloorf(maxy * m_invCellSize);
int n = 0;