diff --git a/DetourCrowd/Include/DetourObstacleAvoidance.h b/DetourCrowd/Include/DetourObstacleAvoidance.h index 8ff6211..72aa5d8 100644 --- a/DetourCrowd/Include/DetourObstacleAvoidance.h +++ b/DetourCrowd/Include/DetourObstacleAvoidance.h @@ -128,6 +128,7 @@ private: 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); diff --git a/DetourCrowd/Source/DetourObstacleAvoidance.cpp b/DetourCrowd/Source/DetourObstacleAvoidance.cpp index 0fad9ef..c3676c3 100644 --- a/DetourCrowd/Source/DetourObstacleAvoidance.cpp +++ b/DetourCrowd/Source/DetourObstacleAvoidance.cpp @@ -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 = ((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; 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 = dtSqrt(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); @@ -481,9 +528,17 @@ 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 int nd2 = nd / 2; const float da = (1.0f/nd) * DT_PI*2; - const float dang = dtMathAtan2f(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 +547,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%1)*3] * r; + pat[npat*2+1] = ddir[(j%1)*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 +597,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) { @@ -540,5 +614,4 @@ int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const flo dtVcopy(nvel, res); return ns; -} - +} \ No newline at end of file