/* 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; }