/* 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;
}
void KX_ObstacleSimulationTOI_rays::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
const float maxDeltaAngle)
{
MT_Vector2 vel(activeObst->dvel[0], activeObst->dvel[1]);
float vmax = (float) vel.length();
float odir = (float) atan2(vel.y(), vel.x());
MT_Vector2 ddir = vel;
ddir.normalize();
float bestScore = FLT_MAX;
float bestDir = odir;
float bestToi = 0;
TOICircle tc;
tc.n = m_maxSamples;
tc.minToi = m_minToi;
tc.maxToi = m_maxToi;
const int iforw = m_maxSamples/2;
const float aoff = (float)iforw / (float)m_maxSamples;
size_t nobs = m_obstacles.size();
for (int iter = 0; iter < m_maxSamples; ++iter)
{
// Calculate sample velocity
const float ndir = ((float)iter/(float)m_maxSamples) - aoff;
const float dir = odir+ndir*M_PI*2;
MT_Vector2 svel;
svel.x() = cosf(dir) * vmax;
svel.y() = sinf(dir) * vmax;
// Find min time of impact and exit amongst all obstacles.
float tmin = m_maxToi;
float tmine = 0;
for (int i = 0; i < nobs; ++i)
{
KX_Obstacle* ob = m_obstacles[i];
bool res = filterObstacle(activeObst, activeNavMeshObj, ob, m_levelHeight);
if (!res)
continue;
float htmin,htmax;
if (ob->m_shape == KX_OBSTACLE_CIRCLE)
{
MT_Vector2 vab;
if (vlen(ob->vel) < 0.01f*0.01f)
{
// Stationary, use VO
vab = svel;
}
else
{
// Moving, use RVO
vab = 2*svel - vel - ob->vel;
}
if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad,
vab, ob->m_pos, ob->m_rad, htmin, htmax))
continue;
}
else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
{
MT_Point3 p1 = ob->m_pos;
MT_Point3 p2 = ob->m_pos2;
//apply world transform
if (ob->m_type == KX_OBSTACLE_NAV_MESH)
{
KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
p1 = navmeshobj->TransformToWorldCoords(p1);
p2 = navmeshobj->TransformToWorldCoords(p2);
}
if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel,
p1, p2, ob->m_rad, htmin, htmax))
continue;
}
if (htmin > 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
else if (htmax > 0.0f)
{
// The agent overlaps the obstacle, keep track of first safe exit.
if (htmax > tmine)
tmine = htmax;
}
}
// Calculate sample penalties and final score.
const float apen = m_velWeight * fabsf(ndir);
const float tpen = m_toiWeight * (1.0f/(0.0001f+tmin/m_maxToi));
const float cpen = m_collisionWeight * (tmine/m_minToi)*(tmine/m_minToi);
const float score = apen + tpen + cpen;
// Update best score.
if (score < bestScore)
{
bestDir = dir;
bestToi = tmin;
bestScore = score;
}
tc.dir[iter] = dir;
tc.toi[iter] = tmin;
tc.toie[iter] = tmine;
}
if (vlen(activeObst->vel) > 0.1)
{
// Constrain max turn rate.
float cura = atan2(activeObst->vel[1],activeObst->vel[0]);
float da = bestDir - cura;
if (da < -M_PI) da += (float)M_PI*2;
if (da > M_PI) da -= (float)M_PI*2;
if (da < -maxDeltaAngle)
{
bestDir = cura - maxDeltaAngle;
bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
}
else if (da > maxDeltaAngle)
{
bestDir = cura + maxDeltaAngle;
bestToi = min(bestToi, interpolateToi(bestDir, tc.dir, tc.toi, tc.n));
}
}
// Adjust speed when time of impact is less than min TOI.
if (bestToi < m_minToi)
vmax *= bestToi/m_minToi;
// New steering velocity.
activeObst->nvel[0] = cosf(bestDir) * vmax;
activeObst->nvel[1] = sinf(bestDir) * vmax;
}
static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
KX_Obstacles& obstacles, float levelHeight, const float vmax,
const float* spos, const float cs, const int nspos, float* res,
float maxToi, float velWeight, float curVelWeight, float sideWeight,
float toiWeight)
{
vset(res, 0,0);
const float ivmax = 1.0f / vmax;
float adir[2], adist;
vcpy(adir, activeObst->pvel);
if (vlen(adir) > 0.01f)
vnorm(adir);
else
vset(adir,0,0);
float activeObstPos[2];
vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y());
adist = vdot(adir, activeObstPos);
float minPenalty = FLT_MAX;
for (int n = 0; n < nspos; ++n)
{
float vcand[2];
vcpy(vcand, &spos[n*2]);
// Find min time of impact and exit amongst all obstacles.
float tmin = maxToi;
float side = 0;
int nside = 0;
for (int i = 0; i < obstacles.size(); ++i)
{
KX_Obstacle* ob = obstacles[i];
bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
if (!res)
continue;
float htmin, htmax;
if (ob->m_shape==KX_OBSTACLE_CIRCLE)
{
float vab[2];
// Moving, use RVO
vscale(vab, vcand, 2);
vsub(vab, vab, activeObst->vel);
vsub(vab, vab, ob->vel);
// Side
// NOTE: dp, and dv are constant over the whole calculation,
// they can be precomputed per object.
const float* pa = activeObstPos;
float pb[2];
vset(pb, ob->m_pos.x(), ob->m_pos.y());
const float orig[2] = {0,0};
float dp[2],dv[2],np[2];
vsub(dp,pb,pa);
vnorm(dp);
vsub(dv,ob->dvel, activeObst->dvel);
const float a = triarea(orig, dp,dv);
if (a < 0.01f)
{
np[0] = -dp[1];
np[1] = dp[0];
}
else
{
np[0] = dp[1];
np[1] = -dp[0];
}
side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f);
nside++;
if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad,
htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
}
else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
{
MT_Point3 p1 = ob->m_pos;
MT_Point3 p2 = ob->m_pos2;
//apply world transform
if (ob->m_type == KX_OBSTACLE_NAV_MESH)
{
KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
p1 = navmeshobj->TransformToWorldCoords(p1);
p2 = navmeshobj->TransformToWorldCoords(p2);
}
float p[2], q[2];
vset(p, p1.x(), p1.y());
vset(q, p2.x(), p2.y());
// NOTE: the segments are assumed to come from a navmesh which is shrunken by
// the agent radius, hence the use of really small radius.
// This can be handle more efficiently by using seg-seg test instead.
// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
const float r = 0.01f; // agent->rad
if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad))
{
float sdir[2], snorm[2];
vsub(sdir, q, p);
snorm[0] = sdir[1];
snorm[1] = -sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (vdot(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
htmax = 10.0f;
}
else
{
if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
}
if (htmin >= 0.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 vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
const float spen = sideWeight * side;
const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));
const float penalty = vpen + vcpen + spen + tpen;
if (penalty < minPenalty)
{
minPenalty = penalty;
vcpy(res, vcand);
}
}
}
static int sweepCircleSegment(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
const MT_Vector3& pa, const MT_Vector3& pb, const MT_Scalar sr,
float& tmin, float &tmax)
{
// equation parameters
MT_Vector2 c0(pos0.x(), pos0.y());
MT_Vector2 sa(pa.x(), pa.y());
MT_Vector2 sb(pb.x(), pb.y());
MT_Vector2 L = sb-sa;
MT_Vector2 H = c0-sa;
MT_Scalar radius = r0+sr;
float l2 = L.length2();
float r2 = radius * radius;
float dl = perp(v, L);
float hl = perp(H, L);
float a = dl * dl;
float b = 2.0f * hl * dl;
float c = hl * hl - (r2 * l2);
float d = (b*b) - (4.0f * a * c);
// infinite line missed by infinite ray.
if (d < 0.0f)
return 0;
d = sqrtf(d);
tmin = (-b - d) / (2.0f * a);
tmax = (-b + d) / (2.0f * a);
// line missed by ray range.
/* if (tmax < 0.0f || tmin > 1.0f)
return 0;*/
// find what part of the ray was collided.
MT_Vector2 Pedge;
Pedge = c0+v*tmin;
H = Pedge - sa;
float e0 = MT_dot(H, L) / l2;
Pedge = c0 + v*tmax;
H = Pedge - sa;
float e1 = MT_dot(H, L) / l2;
if (e0 < 0.0f || e1 < 0.0f)
{
float ctmin, ctmax;
if (sweepCircleCircle(pos0, r0, v, pa, sr, ctmin, ctmax))
{
if (e0 < 0.0f && ctmin > tmin)
tmin = ctmin;
if (e1 < 0.0f && ctmax < tmax)
tmax = ctmax;
}
else
{
return 0;
}
}
if (e0 > 1.0f || e1 > 1.0f)
{
float ctmin, ctmax;
if (sweepCircleCircle(pos0, r0, v, pb, sr, ctmin, ctmax))
{
if (e0 > 1.0f && ctmin > tmin)
tmin = ctmin;
if (e1 > 1.0f && ctmax < tmax)
tmax = ctmax;
}
else
{
return 0;
}
}
return 1;
}