private List <T> ObjectFinder <T>(Grid <T> grid, Location location, double radius, bool includeObstacleRadius = false) where T : BaseObject
{
var searchableObjects = grid.ObjectsNearLocation(location, radius);
var closeObjects = new List <T>();
foreach (var baseobject in searchableObjects)
{
if (!includeObstacleRadius && Utility.Distance(baseobject.Location, location) < radius)
{
closeObjects.Add(baseobject);
}
if (includeObstacleRadius && (baseobject.GetType() == typeof(ObstacleCircle)))
{
ObstacleCircle objectAsObstacle = (ObstacleCircle)Convert.ChangeType(baseobject, typeof(ObstacleCircle));
if (Utility.Distance(baseobject.Location, location) < radius + objectAsObstacle.Radius)
{
closeObjects.Add(baseobject);
}
}
}
return(closeObjects);
}
public float ProcessSample(Vector3 vcand, float cs, Vector3 position, float radius, Vector3 vel, Vector3 desiredVel)
{
//find min time of impact and exit amongst all obstacles
float tmin = parameters.HorizTime;
float side = 0;
int numSide = 0;
for (int i = 0; i < numCircles; i++)
{
ObstacleCircle cir = circles[i];
//RVO
Vector3 vab = vcand * 2;
vab = vab - vel;
vab = vab - cir.Vel;
//side
side += MathHelper.Clamp(Math.Min(Vector3Extensions.Dot2D(ref cir.Dp, ref vab) * 0.5f + 0.5f,
Vector3Extensions.Dot2D(ref cir.Np, ref vab) * 2.0f), 0.0f, 1.0f);
numSide++;
float htmin = 0, htmax = 0;
if (!SweepCircleCircle(position, radius, vab, cir.Position, cir.Radius, ref htmin, ref 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 sometime ahead of us, keep track of nearest obstacle
if (htmin < tmin)
{
tmin = htmin;
}
}
}
for (int i = 0; i < numSegments; i++)
{
ObstacleSegment seg = segments[i];
float htmin = 0;
if (seg.Touch)
{
//special case when the agent is very close to the segment
Vector3 sdir = seg.Q - seg.P;
Vector3 snorm = new Vector3(0, 0, 0);
snorm.X = -sdir.Z;
snorm.Z = sdir.X;
//if the velocity is pointing towards the segment, no collision
if (Vector3Extensions.Dot2D(ref snorm, ref vcand) < 0.0f)
{
continue;
}
//else immediate collision
htmin = 0.0f;
}
else
{
if (!IntersectRaySegment(position, vcand, seg.P, seg.Q, ref htmin))
{
continue;
}
}
//avoid less when facing walls
htmin *= 2.0f;
//the closest obstacle is somewhere ahead of us, keep track of the nearest obstacle
if (htmin < tmin)
{
tmin = htmin;
}
}
//normalize side bias
if (numSide != 0)
{
side /= numSide;
}
float vpen = parameters.WeightDesVel * (Vector3Extensions.Distance2D(vcand, desiredVel) * invVmax);
float vcpen = parameters.WeightCurVel * (Vector3Extensions.Distance2D(vcand, vel) * invVmax);
float spen = parameters.WeightSide * side;
float tpen = parameters.WeightToi * (1.0f / (0.1f + tmin * invHorizTime));
float penalty = vpen + vcpen + spen + tpen;
return(penalty);
}
