private bool Remove(Node node, IFixedAgent fixedAgent)
{
var n = node.linkedList;
if (n == null)
{
return(false);
}
if (n == fixedAgent)
{
node.linkedList = n.Next;
node.count--;
return(true);
}
int count = 0;
while (n.Next != null && count < LeafSize + 1)
{
//count++;
if (n.Next == fixedAgent)
{
n.Next = fixedAgent.Next;
node.count--;
return(true);
}
n = n.Next;
}
return(false);
}
private void Add(Node node, IFixedAgent fixedAgent)
{
fixedAgent.Next = node.linkedList;
node.linkedList = fixedAgent;
node.count++;
_agentRootDic[fixedAgent] = node;
}
public bool Remove(IFixedAgent fixedAgent)
{
Node node = null;
if (_agentRootDic.TryGetValue(fixedAgent, out node))
{
return(Remove(node, fixedAgent));
}
return(false);
}
public void Insert <T>(IFixedAgent fixedAgent)
{
var r = _bounds;
var p = new Vector2d(fixedAgent.Position.x, fixedAgent.Position.z);
fixedAgent.Next = null;
var node = _root;
while (true)
{
if (!node.HasChild)
{
if (node.count < LeafSize)
{
Add(node, fixedAgent);
break;
}
else
{
Distribute(node, r);
}
}
else
{
var c = r.center;
if (p.x > c.x)
{
if (p.y > c.y)
{
node = node.child11;
r = Utility.MinMaxRect(c.x, c.y, r.xMax, r.yMax);
}
else
{
node = node.child10;
r = Utility.MinMaxRect(c.x, r.yMin, r.xMax, c.y);
}
}
else
{
if (p.y > c.y)
{
node = node.child01;
r = Utility.MinMaxRect(r.xMin, c.y, c.x, r.yMax);
}
else
{
node = node.child00;
r = Utility.MinMaxRect(r.xMin, r.yMin, c.x, c.y);
}
}
}
}
}
private long QueryRec <K>(Node node, IFixedAgent fixedAgent, long radius, List <K> fixedAgents, Utility.FixedRect r) where K : IFixedAgent
{
Vector2d p = new Vector2d(fixedAgent.Position.x, fixedAgent.Position.z);
//找到一个子节点
if (!node.HasChild)
{
var a = node.linkedList;
while (a != null)
{
if (a is K && a != fixedAgent && Vector2d.SqrDistance(p, new Vector2d(a.Position.x, a.Position.z)) < radius.Mul(radius))
{
fixedAgents.Add((K)a);
}
a = a.Next;
}
}
else
{
//搜索子节点
var c = r.center;
if (p.x - radius < c.x)
{
if (p.y - radius < c.y)
{
radius = QueryRec(node.child00, fixedAgent, radius, fixedAgents,
Utility.MinMaxRect(r.xMin, r.yMin, c.x, c.y));
}
if (p.y + radius > c.y)
{
radius = QueryRec(node.child01, fixedAgent, radius, fixedAgents,
Utility.MinMaxRect(r.xMin, c.y, c.x, r.yMax));
}
}
if (p.x + radius > c.x)
{
if (p.y - radius < c.y)
{
radius = QueryRec(node.child10, fixedAgent, radius, fixedAgents,
Utility.MinMaxRect(c.x, r.yMin, r.xMax, c.y));
}
if (p.y + radius > c.y)
{
radius = QueryRec(node.child11, fixedAgent, radius, fixedAgents,
Utility.MinMaxRect(c.x, c.y, r.xMax, r.yMax));
}
}
}
return(radius);
}
public void Relocate(IFixedAgent fixedAgent)
{
Node node;
if (_agentRootDic.TryGetValue(fixedAgent, out node))
{
if (!node.rect.ContainsPoint(fixedAgent.Position))
{
if (Remove(node, fixedAgent))
{
Insert <T>(fixedAgent);
}
}
}
}
public long InsertAgentNeighbour(IFixedAgent fixedAgent, long rangeSq)
{
if (this == fixedAgent)
{
return(rangeSq);
}
var dist = (fixedAgent.Position.x - Position.x).Mul(fixedAgent.Position.x - Position.x)
+ (fixedAgent.Position.z - Position.z).Mul(fixedAgent.Position.z - Position.z);
if (dist < rangeSq)
{
if (AgentNeighbors.Count < 10)
{
AgentNeighbors.Add(fixedAgent);
AgentNeighborSqrDists.Add(dist);
}
var i = AgentNeighbors.Count - 1;
if (dist < AgentNeighborSqrDists[i])
{
while (i != 0 && dist < AgentNeighborSqrDists[i - 1])
{
AgentNeighbors[i] = AgentNeighbors[i - 1];
AgentNeighborSqrDists[i] = AgentNeighborSqrDists[i - 1];
i--;
}
AgentNeighbors[i] = fixedAgent;
AgentNeighborSqrDists[i] = dist;
}
if (AgentNeighbors.Count == 10)
{
rangeSq = AgentNeighborSqrDists[AgentNeighbors.Count - 1];
}
}
return(rangeSq);
}
public void Query <K>(IFixedAgent fixedAgent, long radius, List <K> fixedAgents) where K : IFixedAgent
{
QueryRec(_root, fixedAgent, radius, fixedAgents, _bounds);
}
public void Query(IFixedAgent fixedAgent, long radius, List <IFixedAgent> fixedAgents)
{
QueryRec(_root, fixedAgent, radius, fixedAgents, _bounds);
}
public PathIntersection(IFixedAgent obstacle)
{
Obstacle = obstacle;
Intersect = false;
Distance = float.MaxValue;
}
/// <summary>
/// Finds a vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="vehicle">
/// The vehicle to evaluate.
/// </param>
/// <param name="futureVehiclePosition">
/// The position where we expect the vehicle to be soon
/// </param>
/// <param name="obstacle">
/// A spherical obstacle to check against <see cref="DetectableObject"/>
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
/// <remarks>We could probably spin out this function to an independent tool class</remarks>
public static PathIntersection FindNextIntersectionWithSphere(IFixedAgent vehicle, Vector3 futureVehiclePosition,
IFixedAgent obstacle)
{
// this mainly follows http://www.lighthouse3d.com/tutorials/maths/ray-sphere-intersection/
var intersection = new PathIntersection(obstacle);
var combinedRadius = vehicle.Radius.ToFloat() + obstacle.Radius.ToFloat();
var movement = futureVehiclePosition - vehicle.Position.ToVector3();
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehicle.Position;
// this is the length of vehicleToObstacle projected onto direction
var projectionLength = Vector3.Dot(direction, vehicleToObstacle.ToVector3());
// if the projected obstacle center lies further away than our movement + both radius, we're not going to collide
if (projectionLength > movement.magnitude + combinedRadius)
{
//print("no collision - 1");
return(intersection);
}
// the foot of the perpendicular
var projectedObstacleCenter = vehicle.Position.ToVector3() + projectionLength * direction;
// distance of the obstacle to the pathe the vehicle is going to take
var obstacleDistanceToPath = (obstacle.Position.ToVector3() - projectedObstacleCenter).magnitude;
//print("obstacleDistanceToPath: " + obstacleDistanceToPath);
// if the obstacle is further away from the movement, than both radius, there's no collision
if (obstacleDistanceToPath > combinedRadius)
{
//print("no collision - 2");
return(intersection);
}
// use pythagorean theorem to calculate distance out of the sphere (if you do it 2D, the line through the circle would be a chord and we need half of its length)
var halfChord = Mathf.Sqrt(combinedRadius * combinedRadius + obstacleDistanceToPath * obstacleDistanceToPath);
// if the projected obstacle center lies opposite to the movement direction (aka "behind")
if (projectionLength < 0)
{
// behind and further away than both radius -> no collision (we already passed)
if (vehicleToObstacle.magnitude > combinedRadius)
{
return(intersection);
}
var intersectionPoint = projectedObstacleCenter - direction * halfChord;
intersection.Intersect = true;
intersection.Distance = (intersectionPoint - vehicle.Position.ToVector3()).magnitude;
return(intersection);
}
// calculate both intersection points
var intersectionPoint1 = projectedObstacleCenter - direction * halfChord;
var intersectionPoint2 = projectedObstacleCenter + direction * halfChord;
// pick the closest one
var intersectionPoint1Distance = (intersectionPoint1 - vehicle.Position.ToVector3()).magnitude;
var intersectionPoint2Distance = (intersectionPoint2 - vehicle.Position.ToVector3()).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
return(intersection);
}
