bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h)
{
float v0[3], v1[3], v2[3];
dtVsub(v0, c,a);
dtVsub(v1, b,a);
dtVsub(v2, p,a);
const float dot00 = dtVdot2D(v0, v0);
const float dot01 = dtVdot2D(v0, v1);
const float dot02 = dtVdot2D(v0, v2);
const float dot11 = dtVdot2D(v1, v1);
const float dot12 = dtVdot2D(v1, v2);
// Compute barycentric coordinates
const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
const float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// The (sloppy) epsilon is needed to allow to get height of points which
// are interpolated along the edges of the triangles.
static const float EPS = 1e-4f;
// If point lies inside the triangle, return interpolated ycoord.
if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
{
h = a[1] + v0[1]*u + v1[1]*v;
return true;
}
return false;
}
static void projectPoly(const float* axis, const float* poly, const int npoly,
float& rmin, float& rmax)
{
rmin = rmax = dtVdot2D(axis, &poly[0]);
for (int i = 1; i < npoly; ++i)
{
const float d = dtVdot2D(axis, &poly[i*3]);
rmin = dtMin(rmin, d);
rmax = dtMax(rmax, d);
}
}
static int sweepCircleCircle(const float* c0, const float r0, const float* v,
const float* c1, const float r1,
float& tmin, float& tmax)
{
static const float EPS = 0.0001f;
float s[3];
dtVsub(s,c1,c0);
float r = r0+r1;
float c = dtVdot2D(s,s) - r*r;
float a = dtVdot2D(v,v);
if (a < EPS) return 0; // not moving
// Overlap, calc time to exit.
float b = dtVdot2D(v,s);
float d = b*b - a*c;
if (d < 0.0f) return 0; // no intersection.
a = 1.0f / a;
const float rd = dtMathSqrtf(d);
tmin = (b - rd) * a;
tmax = (b + rd) * a;
return 1;
}
/* 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;
int nside = 0;
for (int i = 0; i < m_ncircles; ++i)
{
const dtObstacleCircle* cir = &m_circles[i];
// RVO
float vab[3];
dtVscale(vab, vcand, 2);
dtVsub(vab, vab, vel);
dtVsub(vab, vab, cir->vel);
// Side
side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f);
nside++;
float htmin = 0, htmax = 0;
if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
{
tmin = htmin;
if (tmin < tThresold)
return minPenalty;
}
}
}
for (int i = 0; i < m_nsegments; ++i)
{
const dtObstacleSegment* seg = &m_segments[i];
float htmin = 0;
if (seg->touch)
{
// Special case when the agent is very close to the segment.
float sdir[3], snorm[3];
dtVsub(sdir, seg->q, seg->p);
snorm[0] = -sdir[2];
snorm[2] = sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (dtVdot2D(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
}
else
{
if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
// 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 spen = m_params.weightSide * side;
const float tpen = m_params.weightToi * (1.0f/(0.1f+tmin*m_invHorizTime));
const float penalty = vpen + vcpen + spen + tpen;
// Store different penalties for debug viewing
if (debug)
debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
return penalty;
}
float dtObstacleAvoidanceQuery::processSample(const float* vcand, const float cs,
const float* pos, const float rad,
const float vmax, const float* vel, const float* dvel,
dtObstacleAvoidanceDebugData* debug)
{
// Find min time of impact and exit amongst all obstacles.
float tmin = m_horizTime;
float side = 0;
int nside = 0;
for (int i = 0; i < m_ncircles; ++i)
{
const dtObstacleCircle* cir = &m_circles[i];
// RVO
float vab[3];
dtVscale(vab, vcand, 2);
dtVsub(vab, vab, vel);
dtVsub(vab, vab, cir->vel);
// Side
side += dtClamp(dtMin(dtVdot2D(cir->dp,vab)*0.5f+0.5f, dtVdot2D(cir->np,vab)*2), 0.0f, 1.0f);
nside++;
float htmin = 0, htmax = 0;
if (!sweepCircleCircle(pos,rad, vab, cir->p,cir->rad, htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
}
for (int i = 0; i < m_nsegments; ++i)
{
const dtObstacleSegment* seg = &m_segments[i];
float htmin = 0;
if (seg->touch)
{
// Special case when the agent is very close to the segment.
float sdir[3], snorm[3];
dtVsub(sdir, seg->q, seg->p);
snorm[0] = -sdir[2];
snorm[2] = sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (dtVdot2D(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
}
else
{
if (!isectRaySeg(pos, vcand, seg->p, seg->q, htmin))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
const float ivmax = 1.0f / vmax;
const float vpen = m_weightDesVel * (dtVdist2D(vcand, dvel) * ivmax);
const float vcpen = m_weightCurVel * (dtVdist2D(vcand, vel) * ivmax);
const float spen = m_weightSide * side;
const float tpen = m_weightToi * (1.0f/(0.1f+tmin / m_horizTime));
const float penalty = vpen + vcpen + spen + tpen;
// Store different penalties for debug viewing
if (debug)
debug->addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
return penalty;
}
