From 0122231ccfa3b95bf67539b686155a9ee2e39062 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Mon, 30 Jun 2014 00:49:14 +0300 Subject: [PATCH] bug fix in the original code (div by 0); minPenalty check missed rejecting states. --- DetourCrowd/Source/DetourObstacleAvoidance.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/DetourCrowd/Source/DetourObstacleAvoidance.cpp b/DetourCrowd/Source/DetourObstacleAvoidance.cpp index 664aea5..c3676c3 100644 --- a/DetourCrowd/Source/DetourObstacleAvoidance.cpp +++ b/DetourCrowd/Source/DetourObstacleAvoidance.cpp @@ -327,13 +327,13 @@ float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs // 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); - if (vpen + vcpen > minPenalty) - return vpen + vcpen; // already too much. // 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 = ((double)m_params.weightToi/(double)minPen - 0.1) / (double)m_invHorizTime; + float tThresold = ((double)m_params.weightToi/(double)minPen - 0.1) * (double)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; @@ -439,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); @@ -511,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);