internal virtual bool sweepCircleCircle(float[] c0, float r0, float[] v, float[] c1, float r1, ref float tmin, ref float tmax)
{
//float[] s = vSub(c1, c0);
DetourCommon.vSub(sccs, c1, c0);
float r = r0 + r1;
float c = DetourCommon.vDot2D(sccs, sccs) - r * r;
float a = DetourCommon.vDot2D(v, v);
if (a < EPS)
{
return(false); // not moving
}
// Overlap, calc time to exit.
float b = DetourCommon.vDot2D(v, sccs);
float d = b * b - a * c;
if (d < 0.0f)
{
return(false); // no intersection.
}
a = 1.0f / a;
float rd = (float)Math.Sqrt(d);
tmin = (b - rd) * a;
tmax = (b + rd) * a;
return(true);
}
public virtual void trimInvalidPath(long safeRef, float[] safePos, NavMeshQuery navquery, QueryFilter filter)
{
// Keep valid path as far as possible.
int n = 0;
while (n < m_path.Count && navquery.isValidPolyRef(m_path[n], filter))
{
n++;
}
if (n == 0)
{
// The first polyref is bad, use current safe values.
DetourCommon.vCopy(m_pos, safePos);
m_path.Clear();
m_path.Add(safeRef);
}
else if (n < m_path.Count)
{
m_path = m_path.GetRange(0, n);
// The path is partially usable.
}
navquery.closestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target, temp);
// Clamp target pos to last poly
DetourCommon.vCopy(m_target, temp);
}
public StraightPathItem(float[] pos, int flags, long @ref)
{
this.pos = new float[3];
DetourCommon.vCopy(this.pos, pos);
this.flags = flags;
this.@ref = @ref;
}
internal virtual bool isectRaySeg(float[] ap, float[] u, float[] bp, float[] bq, ref float t)
{
//float[] v = vSub(bq, bp);
//float[] w = vSub(ap, bp);
DetourCommon.vSub(srsv, bq, bp);
DetourCommon.vSub(srsw, ap, bp);
//vSub(srsw, bq, bp);
float d = DetourCommon.vPerp2D(u, srsv);
if (Math.Abs(d) < 1e-6f)
{
return(false);
}
d = 1.0f / d;
t = DetourCommon.vPerp2D(srsv, srsw) * d;
if (t < 0 || t > 1)
{
return(false);
}
float s = DetourCommon.vPerp2D(u, srsw) * d;
if (s < 0 || s > 1)
{
return(false);
}
return(true);
}
public virtual bool moveOverOffmeshConnection(long offMeshConRef, long[] refs, float[] start, float[] end, NavMeshQuery navquery)
{
// Advance the path up to and over the off-mesh connection.
long prevRef = 0, polyRef = m_path[0];
int npos = 0;
while (npos < m_path.Count && polyRef != offMeshConRef)
{
prevRef = polyRef;
polyRef = m_path[npos];
npos++;
}
if (npos == m_path.Count)
{
// Could not find offMeshConRef
return(false);
}
// Prune path
m_path = m_path.GetRange(npos, m_path.Count - npos);
refs[0] = prevRef;
refs[1] = polyRef;
NavMesh nav = navquery.AttachedNavMesh;
nav.getOffMeshConnectionPolyEndPoints(refs[0], refs[1], ref start, ref end);
DetourCommon.vCopy(m_pos, end);
return(true);
}
internal virtual void integrate(float dt)
{
// Fake dynamic constraint.
float maxDelta = @params.maxAcceleration * dt;
//float[] dv = vSub(nvel, vel);
DetourCommon.vSub(dv, nvel, vel);
float ds = DetourCommon.vLen(dv);
if (ds > maxDelta)
{
//dv = vScale(dv, maxDelta / ds);
DetourCommon.vScale(dv, dv, maxDelta / ds);
}
//vel = vAdd(vel, dv);
DetourCommon.vAdd(vel, vel, dv);
// Integrate
if (DetourCommon.vLen(vel) > 0.0001f)
{
//npos = vMad(npos, vel, dt);
DetourCommon.vMad(npos, npos, vel, dt);
}
else
{
DetourCommon.vSet(vel, 0, 0, 0);
}
}
/* protected void addSegment(float dist, float[] s) {
* // Insert neighbour based on the distance.
* Segment seg = new Segment();
* System.arraycopy(s, 0, seg.s, 0, 6);
* seg.d = dist;
* if (m_segs.isEmpty()) {
* m_segs.add(seg);
* } else if (dist >= m_segs.get(m_segs.size() - 1).d) {
* if (m_segs.size() >= MAX_LOCAL_SEGS) {
* return;
* }
* m_segs.add(seg);
* } else {
* // Insert inbetween.
* int i;
* for (i = 0; i < m_segs.size(); ++i)
* if (dist <= m_segs.get(i).d)
* break;
* m_segs.add(i, seg);
* }
* while (m_segs.size() > MAX_LOCAL_SEGS) {
* m_segs.remove(m_segs.size() - 1);
* }
* }*/
public virtual void update(long @ref, float[] pos, float collisionQueryRange, NavMeshQuery navquery, QueryFilter filter)
{
if (@ref == 0)
{
reset();
return;
}
DetourCommon.vCopy(m_center, pos);
// First query non-overlapping polygons.
navquery.findLocalNeighbourhood(@ref, pos, collisionQueryRange, filter, m_polys, null, ref m_npolys, MAX_LOCAL_POLYS);
// Secondly, store all polygon edges.
m_nsegs = 0;
int nsegs = 0;
float t = 0;
for (int j = 0; j < m_npolys; ++j)
{
navquery.getPolyWallSegments(m_polys[j], filter, segs, null, ref nsegs, MAX_SEGS_PER_POLY);
for (int k = 0; k < nsegs; ++k)
{
Array.Copy(segs, k * 6, s, 0, 6);
// Skip too distant segments.
float dist = DetourCommon.distancePtSegSqr2D(pos, s, 0, 3, ref t);
if (dist > DetourCommon.sqr(collisionQueryRange))
{
continue;
}
addSegment(dist, s);
}
}
}
public virtual void calcSmoothSteerDirection(float[] dir)
{
if (corners.Count > 0)
{
int ip0 = 0;
int ip1 = Math.Min(1, corners.Count - 1);
float[] p0 = corners[ip0].Pos;
float[] p1 = corners[ip1].Pos;
//float[] dir0 = vSub(p0, npos);
//float[] dir1 = vSub(p1, npos);
DetourCommon.vSub(dir0, p0, npos);
DetourCommon.vSub(dir1, p1, npos);
dir0[1] = 0;
dir1[1] = 0;
float len0 = DetourCommon.vLen(dir0);
float len1 = DetourCommon.vLen(dir1);
if (len1 > 0.001f)
{
//dir1 = vScale(dir1, 1.0f / len1);
DetourCommon.vScale(dir1, dir1, 1.0f / len1);
}
dir[0] = dir0[0] - dir1[0] * len0 * 0.5f;
dir[1] = 0;
dir[2] = dir0[2] - dir1[2] * len0 * 0.5f;
DetourCommon.vNormalize(dir);
}
}
/// <summary>
/// Resets the path corridor to the specified position. </summary>
/// <param name="ref"> The polygon reference containing the position. </param>
/// <param name="pos"> The new position in the corridor. [(x, y, z)] </param>
public virtual void reset(long @ref, float[] pos)
{
m_path.Clear();
m_path.Add(@ref);
DetourCommon.vCopy(m_pos, pos);
DetourCommon.vCopy(m_target, pos);
}
/// <summary>
/// Attempts to optimize the path if the specified point is visible from the
/// current position.
///
/// Inaccurate locomotion or dynamic obstacle avoidance can force the agent
/// position significantly outside the original corridor. Over time this can
/// result in the formation of a non-optimal corridor. Non-optimal paths can
/// also form near the corners of tiles.
///
/// This function uses an efficient local visibility search to try to
/// optimize the corridor between the current position and @p next.
///
/// The corridor will change only if @p next is visible from the current
/// position and moving directly toward the point is better than following
/// the existing path.
///
/// The more inaccurate the agent movement, the more beneficial this function
/// becomes. Simply adjust the frequency of the call to match the needs to
/// the agent.
///
/// This function is not suitable for long distance searches.
/// </summary>
/// <param name="next">
/// The point to search toward. [(x, y, z]) </param>
/// <param name="pathOptimizationRange">
/// The maximum range to search. [Limit: > 0] </param>
/// <param name="navquery">
/// The query object used to build the corridor. </param>
/// <param name="filter">
/// The filter to apply to the operation. </param>
public virtual void optimizePathVisibility(float[] next, float pathOptimizationRange, NavMeshQuery navquery, QueryFilter filter)
{
// Clamp the ray to max distance.
float dist = DetourCommon.vDist2D(m_pos, next);
// If too close to the goal, do not try to optimize.
if (dist < 0.01f)
{
return;
}
// Overshoot a little. This helps to optimize open fields in tiled
// meshes.
dist = Math.Min(dist + 0.01f, pathOptimizationRange);
// Adjust ray length.
//float[] delta = vSub(next, m_pos);
//float[] goal = vMad(m_pos, delta, pathOptimizationRange / dist);
DetourCommon.vSub(delta, next, m_pos);
DetourCommon.vMad(goal, m_pos, delta, pathOptimizationRange / dist);
RaycastHit rc = navquery.raycast(m_path[0], m_pos, goal, filter, 0, 0);
if (rc.path.Count > 1 && rc.t > 0.99f)
{
m_path = mergeCorridorStartShortcut(m_path, rc.path);
}
}
public virtual void calcStraightSteerDirection(float[] dir)
{
if (corners.Count > 0)
{
DetourCommon.vSub(dir, corners[0].Pos, npos);
dir[1] = 0;
DetourCommon.vNormalize(dir);
}
}
/// <summary>
/// Moves the position from the current location to the desired location,
/// adjusting the corridor as needed to reflect the change.
///
/// Behavior:
///
/// - The movement is constrained to the surface of the navigation mesh. -
/// The corridor is automatically adjusted (shorted or lengthened) in order
/// to remain valid. - The new position will be located in the adjusted
/// corridor's first polygon.
///
/// The expected use case is that the desired position will be 'near' the
/// current corridor. What is considered 'near' depends on local polygon
/// density, query search extents, etc.
///
/// The resulting position will differ from the desired position if the
/// desired position is not on the navigation mesh, or it can't be reached
/// using a local search.
/// </summary>
/// <param name="npos">
/// The desired new position. [(x, y, z)] </param>
/// <param name="navquery">
/// The query object used to build the corridor. </param>
/// <param name="filter">
/// Thefilter to apply to the operation. </param>
public virtual void movePosition(float[] npos, NavMeshQuery navquery, QueryFilter filter)
{
// Move along navmesh and update new position.
MoveAlongSurfaceResult masResult = navquery.moveAlongSurface(m_path[0], m_pos, npos, filter);
m_path = mergeCorridorStartMoved(m_path, masResult.Visited);
// Adjust the position to stay on top of the navmesh.
DetourCommon.vCopy(m_pos, masResult.ResultPos);
m_pos[1] = navquery.getPolyHeight(m_path[0], masResult.ResultPos, 0, m_pos[1]);
}
public virtual void addSegment(float[] p, float[] q)
{
if (m_nsegments >= m_maxSegments)
{
return;
}
ObstacleSegment seg = m_segments[m_nsegments++];
DetourCommon.vCopy(seg.p, p);
DetourCommon.vCopy(seg.q, q);
}
public virtual int sampleVelocityGrid(float[] pos, float rad, float vmax, float[] vel, float[] dvel, float[] nvel, ObstacleAvoidanceParams @params, ObstacleAvoidanceDebugData debug)
{
prepare(pos, dvel, rad);
m_params = @params;
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : float.MaxValue;
//float[] nvel = new float[3];
DetourCommon.vSet(nvel, 0f, 0f, 0f);
if (debug != null)
{
debug.reset();
}
float cvx = dvel[0] * m_params.velBias;
float cvz = dvel[2] * m_params.velBias;
float cs = vmax * 2 * (1 - m_params.velBias) / (m_params.gridSize - 1);
float half = (m_params.gridSize - 1) * cs * 0.5f;
float minPenalty = float.MaxValue;
int ns = 0;
for (int y = 0; y < m_params.gridSize; ++y)
{
for (int x = 0; x < m_params.gridSize; ++x)
{
//float[] vcand = new float[3];
svgvcand[0] = cvx + x * cs - half;
svgvcand[1] = 0f;
svgvcand[2] = cvz + y * cs - half;
if (DetourCommon.sqr(svgvcand[0]) + DetourCommon.sqr(svgvcand[2]) > DetourCommon.sqr(vmax + cs / 2))
{
continue;
}
float penalty = processSample(svgvcand, cs, pos, rad, vel, dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
DetourCommon.vCopy(nvel, svgvcand);
}
}
}
return(ns);
}
public virtual void addCircle(float[] pos, float rad, float[] vel, float[] dvel)
{
if (m_ncircles >= m_maxCircles)
{
return;
}
ObstacleCircle cir = m_circles[m_ncircles++];
DetourCommon.vCopy(cir.p, pos);
cir.rad = rad;
DetourCommon.vCopy(cir.vel, vel);
DetourCommon.vCopy(cir.dvel, dvel);
}
internal virtual void setTarget(long @ref, float[] pos)
{
targetRef = @ref;
DetourCommon.vCopy(targetPos, pos);
targetPathqRef = PathQueue.DT_PATHQ_INVALID;
if (targetRef != 0)
{
targetState = MoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING;
}
else
{
targetState = MoveRequestState.DT_CROWDAGENT_TARGET_FAILED;
}
}
/// <summary>
/// Moves the target from the curent location to the desired location,
/// adjusting the corridor as needed to reflect the change. Behavior: - The
/// movement is constrained to the surface of the navigation mesh. - The
/// corridor is automatically adjusted (shorted or lengthened) in order to
/// remain valid. - The new target will be located in the adjusted corridor's
/// last polygon.
///
/// The expected use case is that the desired target will be 'near' the
/// current corridor. What is considered 'near' depends on local polygon
/// density, query search extents, etc. The resulting target will differ from
/// the desired target if the desired target is not on the navigation mesh,
/// or it can't be reached using a local search.
/// </summary>
/// <param name="npos">
/// The desired new target position. [(x, y, z)] </param>
/// <param name="navquery">
/// The query object used to build the corridor. </param>
/// <param name="filter">
/// The filter to apply to the operation. </param>
public virtual void moveTargetPosition(float[] npos, NavMeshQuery navquery, QueryFilter filter)
{
// Move along navmesh and update new position.
MoveAlongSurfaceResult masResult = navquery.moveAlongSurface(m_path[m_path.Count - 1], m_target, npos, filter);
m_path = mergeCorridorEndMoved(m_path, masResult.Visited);
// TODO: should we do that?
// Adjust the position to stay on top of the navmesh.
/*
* float h = m_target[1]; navquery->getPolyHeight(m_path[m_npath-1],
* result, &h); result[1] = h;
*/
DetourCommon.vCopy(m_target, masResult.ResultPos);
}
internal virtual float getDistanceToGoal(float range)
{
if (corners.Count == 0)
{
return(range);
}
bool endOfPath = ((corners[corners.Count - 1].Flags & NavMeshQuery.DT_STRAIGHTPATH_END) != 0) ? true : false;
if (endOfPath)
{
return(Math.Min(DetourCommon.vDist2D(npos, corners[corners.Count - 1].Pos), range));
}
return(range);
}
public virtual void fixPathStart(long safeRef, float[] safePos)
{
DetourCommon.vCopy(m_pos, safePos);
if (m_path.Count < 3 && m_path.Count > 0)
{
long p = m_path[m_path.Count - 1];
m_path.Clear();
m_path.Add(safeRef);
m_path.Add(0L);
m_path.Add(p);
}
else
{
m_path.Clear();
m_path.Add(safeRef);
m_path.Add(0L);
}
}
private void prepare(float[] pos, float[] dvel, float radius)
{
// Prepare obstacles
ObstacleCircle cir;
float[] pa;
float[] pb;
float a;
for (int i = 0; i < m_ncircles; ++i)
{
cir = m_circles[i];
// Side
pa = pos;
pb = cir.p;
DetourCommon.vSub(cir.dp, pb, pa);
DetourCommon.vNormalize(cir.dp);
DetourCommon.vSub(ppdv, cir.dvel, dvel);
a = DetourCommon.triArea2D(pporig, cir.dp, ppdv);
if (a < 0.01f)
{
cir.np[0] = -cir.dp[2];
cir.np[2] = cir.dp[0];
}
else
{
cir.np[0] = cir.dp[2];
cir.np[2] = -cir.dp[0];
}
}
ObstacleSegment seg;
float t;
for (int i = 0; i < m_nsegments; ++i)
{
seg = m_segments[i];
t = 0;
seg.touch = DetourCommon.distancePtSegSqr2D(pos, seg.p, seg.q, ref t) < DetourCommon.sqr(R);
}
}
internal virtual void normalizeArray(float[] arr, int n)
{
// Normalize penaly range.
float minPen = float.MaxValue;
float maxPen = -float.MaxValue;
for (int i = 0; i < n; ++i)
{
minPen = Math.Min(minPen, arr[i]);
maxPen = Math.Max(maxPen, arr[i]);
}
float penRange = maxPen - minPen;
float s = penRange > 0.001f ? (1.0f / penRange) : 1;
for (int i = 0; i < n; ++i)
{
arr[i] = DetourCommon.clamp((arr[i] - minPen) * s, 0.0f, 1.0f);
}
}
internal virtual bool overOffmeshConnection(float radius)
{
if (corners.Count == 0)
{
return(false);
}
bool offMeshConnection = ((corners[corners.Count - 1].Flags & NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0) ? true : false;
if (offMeshConnection)
{
float distSq = DetourCommon.vDist2DSqr(npos, corners[corners.Count - 1].Pos);
if (distSq < radius * radius)
{
return(true);
}
}
return(false);
}
public ProximityGrid(int poolSize, float cellsize)
{
this.m_cellSize = cellsize;
this.m_invCellSize = 1.0f / m_cellSize;
//this.m_invCellSize = cellsize;
m_bucketsSize = DetourCommon.nextPow2(poolSize);
m_buckets = new int[m_bucketsSize];
m_poolSize = poolSize;
m_poolHead = 0;
m_pool = new Item[poolSize];
for (int i = 0; i < poolSize; i++)
{
m_pool[i] = new Item(this);
}
clear();
}
/// <summary>
/// Finds the corners in the corridor from the position toward the target.
/// (The straightened path.)
///
/// This is the function used to plan local movement within the corridor. One
/// or more corners can be detected in order to plan movement. It performs
/// essentially the same function as #dtNavMeshQuery::findStraightPath.
///
/// Due to internal optimizations, the maximum number of corners returned
/// will be (@p maxCorners - 1) For example: If the buffers are sized to hold
/// 10 corners, the function will never return more than 9 corners. So if 10
/// corners are needed, the buffers should be sized for 11 corners.
///
/// If the target is within range, it will be the last corner and have a
/// polygon reference id of zero. </summary>
/// <param name="filter">
///
/// @param[in] navquery The query object used to build the corridor. </param>
/// <returns> Corners </returns>
public virtual IList <StraightPathItem> findCorners(int maxCorners, NavMeshQuery navquery, QueryFilter filter)
{
//JAVA TO C# CONVERTER WARNING: The original Java variable was marked 'final':
//ORIGINAL LINE: final float MIN_TARGET_DIST = sqr(0.01f);
float MIN_TARGET_DIST = DetourCommon.sqr(0.01f);
IList <StraightPathItem> path = navquery.findStraightPath(m_pos, m_target, m_path, maxCorners, 0);
// Prune points in the beginning of the path which are too close.
StraightPathItem spi;
for (int i = 0; i < path.Count; i++)
{
spi = path[i];
if ((spi.Flags & NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 || DetourCommon.vDist2DSqr(spi.Pos, m_pos) > MIN_TARGET_DIST)
{
break;
}
path.RemoveAt(i);
i -= 1;
}
return(path);
}
protected internal virtual long request(long startRef, long endRef, float[] startPos, float[] endPos, QueryFilter filter)
{
// Find empty slot
int slot = -1;
for (int i = 0; i < MAX_QUEUE; ++i)
{
if (m_queue[i].@ref == DT_PATHQ_INVALID)
{
slot = i;
break;
}
}
// Could not find slot.
if (slot == -1)
{
return(DT_PATHQ_INVALID);
}
long @ref = m_nextHandle++;
if (m_nextHandle == DT_PATHQ_INVALID)
{
m_nextHandle++;
}
PathQuery q = m_queue[slot];
q.@ref = @ref;
DetourCommon.vCopy(q.startPos, startPos);
q.startRef = startRef;
DetourCommon.vCopy(q.endPos, endPos);
q.endRef = endRef;
q.status = null;
q.filter = filter;
q.keepAlive = 0;
return(@ref);
}
/// <summary>
/// Calculate the collision penalty for a given velocity vector
/// </summary>
/// <param name="vcand"> sampled velocity </param>
/// <param name="dvel"> desired velocity </param>
/// <param name="minPenalty"> threshold penalty for early out </param>
public virtual float processSample(float[] vcand, float cs, float[] pos, float rad, float[] vel, float[] dvel, float minPenalty, ObstacleAvoidanceDebugData debug)
{
// penalty for straying away from the desired and current velocities
float vpen = m_params.weightDesVel * (DetourCommon.vDist2D(vcand, dvel) * m_invVmax);
float vcpen = m_params.weightCurVel * (DetourCommon.vDist2D(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 = (float)(((double)m_params.weightToi / (double)minPen - 0.1) * m_params.horizTime);
if (tThresold - m_params.horizTime > -float.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;
ObstacleCircle cir;
//RefFloat htmin = new RefFloat();
//RefFloat htmax = new RefFloat();
for (int i = 0; i < m_ncircles; ++i)
{
cir = m_circles[i];
// RVO
//float[] vab = vScale(vcand, 2);
//vab = vSub(vab, vel);
//vab = vSub(vab, cir.vel);
DetourCommon.vScale(psvab, vcand, 2);
DetourCommon.vSub(psvab, psvab, vel);
DetourCommon.vSub(psvab, psvab, cir.vel);
// Side
side += DetourCommon.clamp(Math.Min(DetourCommon.vDot2D(cir.dp, psvab) * 0.5f + 0.5f, DetourCommon.vDot2D(cir.np, psvab) * 2), 0.0f, 1.0f);
nside++;
float htmin = 0;
float htmax = 0;
if (!sweepCircleCircle(pos, rad, psvab, cir.p, cir.rad, ref htmin, ref htmax))
{
continue;
}
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
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);
}
}
}
}
ObstacleSegment seg;
for (int i = 0; i < m_nsegments; ++i)
{
seg = m_segments[i];
float htmin = 0;
if (seg.touch)
{
// Special case when the agent is very close to the segment.
//float[] sdir = vSub(seg.q, seg.p);
//float[] snorm = new float[3];
DetourCommon.vSub(pssdir, seg.q, seg.p);
DetourCommon.vSet(pssnorm, 0, 0, 0);
pssnorm[0] = -pssdir[2];
pssnorm[2] = pssdir[0];
// If the velocity is pointing towards the segment, no collision.
if (DetourCommon.vDot2D(pssnorm, vcand) < 0.0001f)
{
continue;
}
// Else immediate collision.
htmin = 0.0f;
}
else
{
if (!isectRaySeg(pos, vcand, seg.p, seg.q, ref htmin))
{
continue;
}
}
// Avoid less when facing walls.
htmin = 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 != 0)
{
side /= nside;
}
float spen = m_params.weightSide * side;
float tpen = m_params.weightToi * (1.0f / (0.1f + tmin * m_invHorizTime));
float penalty = vpen + vcpen + spen + tpen;
// Store different penalties for debug viewing
if (debug != null)
{
debug.addSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
}
return(penalty);
}
/// <summary>
/// Loads a new path and target into the corridor. The current corridor
/// position is expected to be within the first polygon in the path. The
/// target is expected to be in the last polygon.
///
/// @warning The size of the path must not exceed the size of corridor's path
/// buffer set during #init(). </summary>
/// <param name="target">
/// The target location within the last polygon of the path. [(x,
/// y, z)] </param>
/// <param name="path">
/// The path corridor. </param>
public virtual void setCorridor(float[] target, List <long> path)
{
DetourCommon.vCopy(m_target, target);
m_path = new List <long>(path);
}
public virtual int sampleVelocityAdaptive(float[] pos, float rad, float vmax, float[] vel, float[] dvel, float[] nvel, ObstacleAvoidanceParams @params, ObstacleAvoidanceDebugData debug)
{
prepare(pos, dvel, rad);
m_params = @params;
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : float.MaxValue;
//float[] nvel = new float[3];
DetourCommon.vSet(nvel, 0f, 0f, 0f);
if (debug != null)
{
debug.reset();
}
// Build sampling pattern aligned to desired velocity.
//float[] pat = new float[(DT_MAX_PATTERN_DIVS * DT_MAX_PATTERN_RINGS + 1) * 2];
DetourCommon.vResetArray(svapat);
int npat = 0;
int ndivs = m_params.adaptiveDivs;
int nrings = m_params.adaptiveRings;
int depth = m_params.adaptiveDepth;
int nd = DetourCommon.clamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
int nr = DetourCommon.clamp(nrings, 1, DT_MAX_PATTERN_RINGS);
//int nd2 = nd / 2;
float da = (1.0f / nd) * DT_PI * 2;
float ca = (float)Math.Cos(da);
float sa = (float)Math.Sin(da);
// desired direction
//float[] ddir = new float[6];
DetourCommon.vResetArray(svaddir);
DetourCommon.vCopy(svaddir, dvel);
dtNormalize2D(svaddir);
//float[] rotated = dtRotate2D(ddir, da * 0.5f); // rotated by da/2
dtRotate2D(svarotated, svaddir, da * 0.5f);
svaddir[3] = svarotated[0];
svaddir[4] = svarotated[1];
svaddir[5] = svarotated[2];
// Always add sample at zero
svapat[npat * 2 + 0] = 0;
svapat[npat * 2 + 1] = 0;
npat++;
float r;
int last1, last2;
for (int j = 0; j < nr; ++j)
{
r = (float)(nr - j) / (float)nr;
svapat[npat * 2 + 0] = svaddir[(j % 2) * 3] * r;
svapat[npat * 2 + 1] = svaddir[(j % 2) * 3 + 2] * r;
last1 = npat * 2;
last2 = last1;
npat++;
for (int i = 1; i < nd - 1; i += 2)
{
// get next point on the "right" (rotate CW)
svapat[npat * 2 + 0] = svapat[last1] * ca + svapat[last1 + 1] * sa;
svapat[npat * 2 + 1] = -svapat[last1] * sa + svapat[last1 + 1] * ca;
// get next point on the "left" (rotate CCW)
svapat[npat * 2 + 2] = svapat[last2] * ca - svapat[last2 + 1] * sa;
svapat[npat * 2 + 3] = svapat[last2] * sa + svapat[last2 + 1] * ca;
last1 = npat * 2;
last2 = last1 + 2;
npat += 2;
}
if ((nd & 1) == 0)
{
svapat[npat * 2 + 2] = svapat[last2] * ca - svapat[last2 + 1] * sa;
svapat[npat * 2 + 3] = svapat[last2] * sa + svapat[last2 + 1] * ca;
npat++;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
//float[] res = new float[3];
DetourCommon.vSet(svares, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0;
float minPenalty;
float penalty;
for (int k = 0; k < depth; ++k)
{
minPenalty = float.MaxValue;
//float[] bvel = new float[3];
DetourCommon.vSet(svabvel, 0, 0, 0);
for (int i = 0; i < npat; ++i)
{
//float[] vcand = new float[3];
svavcand[0] = svares[0] + svapat[i * 2 + 0] * cr;
svavcand[1] = 0f;
svavcand[2] = svares[2] + svapat[i * 2 + 1] * cr;
if (DetourCommon.sqr(svavcand[0]) + DetourCommon.sqr(svavcand[2]) > DetourCommon.sqr(vmax + 0.001f))
{
continue;
}
penalty = processSample(svavcand, cr / 10, pos, rad, vel, dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
DetourCommon.vCopy(svabvel, svavcand);
}
}
DetourCommon.vCopy(svares, svabvel);
cr *= 0.5f;
}
DetourCommon.vCopy(nvel, svares);
return(ns);
}
public virtual void reset()
{
this.posOverPoly = false;
DetourCommon.vResetArray(this.closest);
}
public virtual float getCost(float[] pa, float[] pb, long prevRef, MeshTile prevTile, Poly prevPoly, long curRef, MeshTile curTile, Poly curPoly, long nextRef, MeshTile nextTile, Poly nextPoly)
{
return(DetourCommon.vDist(pa, pb) * m_areaCost[curPoly.Area]);
}
