//Returns true if sphere s intersects OBB b, false otherwise.
//The point p on the OBB closest to the sphere center is also returned
bool TestCircleOBB(CircleObstacle obstacle, SimpleActor.OBB box, out Vector2 closestPoint)
{
// Find point p on OBB closest to sphere center
closestPoint = ClosestPtPointOBB(obstacle.Center, box);
// Sphere and OBB intersect if the (squared) distance from sphere
// center to point p is less than the (squared) sphere radius
Vector2 v = closestPoint - obstacle.Center;
return(v.sqrMagnitude <= obstacle.Radius * obstacle.Radius);
}
void AvoidObstacle()
{
int obstaclesCount = 0;
int obstacleToAvoid = 0;
Vector2[] closestPoints = new Vector2[10];
CircleObstacle[] closestObstacles = new CircleObstacle[10];
foreach (CircleObstacle circleObj in circleObjs)
{
Vector2 closestPoint;
if (TestCircleOBB(circleObj, _actor.Box, out closestPoint))
{
closestPoints[obstaclesCount] = closestPoint;
closestObstacles[obstaclesCount] = circleObj;
obstaclesCount++;
}
}
//Debug.Log(obstaclesCount);
for (int i = 0; i < obstaclesCount; i++)
{
Vector2 toClosest = closestPoints[obstacleToAvoid] - _actor.Center;
Vector2 toClose = closestPoints[i] - _actor.Center;
if (toClose.sqrMagnitude > toClosest.sqrMagnitude)
{
obstacleToAvoid = i;
}
}
if (obstaclesCount != 0)
{
_lateral_velocity = -Vector3.Project((closestObstacles[obstacleToAvoid].Center - _actor.Center), _actor.Box._local_axes[1]);
_break_velocity = -Vector3.Project((closestPoints[obstacleToAvoid] - _actor.Center), _actor.Box._local_axes[0]);
_break_velocity = (_actor.Box._extents[0] * 2 - _break_velocity.magnitude) * _break_velocity.normalized;
}
else
{
_lateral_velocity = Vector2.zero;
_break_velocity = Vector2.zero;
}
}