public bool QueryCallback(Entity node)
{
var velocityObstacle = _velocityObstacleLookup[node];
var neighbour = new VelocityObstacle(velocityObstacle);
// todo should probably take in to account neighbour radius here, it could be very large
var distSq = math.lengthsq(_position - neighbour.Position);
if (distSq < _rangeSq)
{
neighbour.Dist = distSq;
if (_neighbours.Length < _maxResults)
{
_neighbours.Add(neighbour);
}
var i = _neighbours.Length - 1;
while (i != 0 && distSq < _neighbours[i - 1].Dist)
{
_neighbours[i] = _neighbours[--i];
}
_neighbours[i] = neighbour;
if (_neighbours.Length == _maxResults)
{
_rangeSq = _neighbours[_neighbours.Length - 1].Dist;
}
}
return(true);
}
//TODO this function will take as input the agent and its
//_desired_ velocity (that can be computed by the current goal)
//and will return the best _desired_ velocity compatible with the VO obstacles
//computed previously
public void Update(AIController agent)
{
for (int i = 0; i < _forbiddenDirections.Count; i++)
{
_forbiddenDirections[i] = 0;
}
//for (int i = 0; i < _interestingDirections.Count; i++)
//{
// _interestingDirections[i] = 0;
//}
List <VelocityObstacle> velocityObstacles = agent.GetVelocityObstacles();
Transform agentTransform = agent.transform;
for (int i = 0; i < velocityObstacles.Count; i++)
{
VelocityObstacle VO = velocityObstacles[i];
Vector3 voLeftLocal = agentTransform.InverseTransformVector(VO.VOLeft);
Vector3 voRightLocal = agentTransform.InverseTransformVector(VO.VORight);
for (int dirIndex = 0; dirIndex < _directionBounds.Count; dirIndex++)
{
DirectionBounds directionBounds = _directionBounds[i];
int rightIndex = -1;
int leftIndex = -1;
if (Vector3.Dot(voRightLocal, directionBounds.minDir) > 0 &&
Vector3.Dot(voRightLocal, directionBounds.maxDir) > 0)
{
if (Vector3.Cross(voRightLocal, directionBounds.minDir).y *Vector3.Cross(voLeftLocal, directionBounds.maxDir).y < 0)
{
rightIndex = dirIndex;
}
}
if (Vector3.Dot(voLeftLocal, directionBounds.minDir) > 0 &&
Vector3.Dot(voLeftLocal, directionBounds.maxDir) > 0)
{
if (Vector3.Cross(voLeftLocal, directionBounds.minDir).y *Vector3.Cross(voLeftLocal, directionBounds.maxDir).y < 0)
{
leftIndex = dirIndex;
}
}
bool leftIndexReached = false;
for (int safetyCounter = 0; safetyCounter < DIRECTION_COUNT && !leftIndexReached; safetyCounter++)
{
int index = (rightIndex + safetyCounter) % DIRECTION_COUNT;
_forbiddenDirections[index] = -1.0f;
leftIndexReached = (index == leftIndex);
}
}
}
//TODO now in _forbiddenDirections we have stored what directions are forbidden
//at this point the agent has to compute what is the closest allowed direction
//to the direction defined by the current goal
Vector3 goalLocal = agentTransform.InverseTransformPoint(agent.GetGoal());
}
bool CheckInsideVO(Vector3 intersection, VelocityObstacle vobj)
{
Line A = LineToClosestPoint(intersection, vobj.sideL, vobj.apex);
Line B = LineToClosestPoint(intersection, vobj.sideR, vobj.apex);
double angle = angleBetween2Lines(A, B) * (180.0 / Mathf.PI);
if (angle > 90 && angle < 180)
{
return(true);
}
else
{
return(false);
}
}
// Update is called once per frame
public void Update()
{
int idVO = 0;
foreach (KinematicData i in DynamicObstacles)
{
VelocityObstacle VO = new VelocityObstacle(Obstacles, DynamicObstacles)
{
Character = i,
detectRadius = 20f
};
Debug.Log("Calculate VO");
List <VelocityObstacle> VOs = VO.calculateVOs();
Debug.Log("Calculated VO");
List <Vector3> intersections = new List <Vector3>();
List <Line> lines = new List <Line>();
float finalDistance = float.MaxValue;
Vector3 finalVelocity = new Vector3();
foreach (VelocityObstacle velocityObstacle in VOs)
{
lines.Add(new Line
{
id = idVO,
origin = velocityObstacle.apex,
end = velocityObstacle.apex + velocityObstacle.sideR * 1000
});
lines.Add(new Line
{
id = idVO,
origin = velocityObstacle.apex,
end = velocityObstacle.apex + velocityObstacle.sideL * 1000
});
}
foreach (Line line1 in lines)
{
foreach (Line line2 in lines)
{
if (line1.id != line2.id)
{
Vector3 intersection = line1.intersectWith(line2);
if (intersection != Vector3.zero)
{
intersections.Add(intersection);
}
}
}
}
foreach (Vector3 intersection in intersections)
{
foreach (VelocityObstacle vobj in VOs)
{
if (CheckInsideVO(intersection, vobj))
{
intersections.Remove(intersection);
break;
}
}
}
foreach (Vector3 candidate in intersections)
{
float distance = GetDistance(i.prefVelocity.x, i.prefVelocity.z, candidate.x, candidate.z);
if (distance < finalDistance)
{
finalDistance = distance;
finalVelocity = candidate;
}
}
if (finalVelocity != Vector3.zero)
{
i.velocity = finalVelocity;
}
//Now that we have all the candidates, we need to check which of the candidates
//is closer to the preferred velocity, and change the velocity of the Character
//to the velocity
Debug.Log(i.velocity);
}
}
