Vector3 ObstacleAvoidance()
{
Vector3 force = Vector3.zero;
makeFeelers();
List <GameObject> tagged = new List <GameObject>();
float minBoxLength = 20.0f;
float boxLength = minBoxLength + ((velocity.magnitude / maxSpeed) * minBoxLength * 2.0f);
if (float.IsNaN(boxLength))
{
System.Console.WriteLine("NAN");
}
// Matt Bucklands Obstacle avoidance
// First tag obstacles in range
GameObject[] obstacles = GameObject.FindGameObjectsWithTag("obstacle");
if (obstacles.Length == 0)
{
return(Vector3.zero);
}
foreach (GameObject obstacle in obstacles)
{
Vector3 toCentre = transform.position - obstacle.transform.position;
float dist = toCentre.magnitude;
if (dist < boxLength)
{
tagged.Add(obstacle);
}
}
float distToClosestIP = float.MaxValue;
GameObject closestIntersectingObstacle = null;
Vector3 localPosOfClosestObstacle = Vector3.zero;
Vector3 intersection = Vector3.zero;
foreach (GameObject o in tagged)
{
Vector3 localPos = transform.InverseTransformPoint(o.transform.position);
// If the local position has a positive Z value then it must lay
// behind the agent. (in which case it can be ignored)
if (localPos.z >= 0)
{
// If the distance from the x axis to the object's position is less
// than its radius + half the width of the detection box then there
// is a potential intersection.
float obstacleRadius = o.transform.localScale.x / 2;
float expandedRadius = GetRadius() + obstacleRadius;
if ((Math.Abs(localPos.y) < expandedRadius) && (Math.Abs(localPos.x) < expandedRadius))
{
// Now to do a ray/sphere intersection test. The center of the
// Create a temp Entity to hold the sphere in local space
Sphere tempSphere = new Sphere(expandedRadius, localPos);
// Create a ray
BGE.Geom.Ray ray = new BGE.Geom.Ray();
ray.pos = new Vector3(0, 0, 0);
ray.look = Vector3.forward;
// Find the point of intersection
/*if (tempSphere.closestRayIntersects(ray, Vector3.zero, ref intersection) == false)
* {
* return Vector3.zero;
* }*/
// Now see if its the closest, there may be other intersecting spheres
float dist = intersection.magnitude;
if (dist < distToClosestIP)
{
dist = distToClosestIP;
closestIntersectingObstacle = o;
localPosOfClosestObstacle = localPos;
}
}
}
if (closestIntersectingObstacle != null)
{
// Now calculate the force
// Calculate Z Axis braking force
float multiplier = 200 * (1.0f + (boxLength - localPosOfClosestObstacle.z) / boxLength);
//calculate the lateral force
float obstacleRadius = closestIntersectingObstacle.GetComponent <Renderer>().bounds.extents.magnitude;
float expandedRadius = GetRadius() + obstacleRadius;
force.x = (expandedRadius - Math.Abs(localPosOfClosestObstacle.x)) * multiplier;
force.y = (expandedRadius - -Math.Abs(localPosOfClosestObstacle.y)) * multiplier;
if (localPosOfClosestObstacle.x > 0)
{
force.x = -force.x;
}
if (localPosOfClosestObstacle.y > 0)
{
force.y = -force.y;
}
if (Params.drawDebugLines)
{
LineDrawer.DrawLine(transform.position, transform.position + transform.forward * boxLength, debugLineColour);
}
//apply a braking force proportional to the obstacle's distance from
//the vehicle.
const float brakingWeight = 40.0f;
force.z = (obstacleRadius -
localPosOfClosestObstacle.z) *
brakingWeight;
//finally, convert the steering vector from local to world space
force = transform.TransformPoint(force);
}
}
return(force);
}
Vector3 ObstacleAvoidance()
{
Vector3 force = Vector3.zero;
makeFeelers();
List<GameObject> tagged = new List<GameObject>();
float minBoxLength = 20.0f;
float boxLength = minBoxLength + ((velocity.magnitude / Params.GetFloat("max_speed")) * minBoxLength * 2.0f);
if (float.IsNaN(boxLength))
{
System.Console.WriteLine("NAN");
}
// Matt Bucklands Obstacle avoidance
// First tag obstacles in range
GameObject[] obstacles = GameObject.FindGameObjectsWithTag("obstacle");
if (obstacles.Length == 0)
{
return Vector3.zero;
}
foreach (GameObject obstacle in obstacles)
{
Vector3 toCentre = transform.position - obstacle.transform.position;
float dist = toCentre.magnitude;
if (dist < boxLength)
{
tagged.Add(obstacle);
}
}
float distToClosestIP = float.MaxValue;
GameObject closestIntersectingObstacle = null;
Vector3 localPosOfClosestObstacle = Vector3.zero;
Vector3 intersection = Vector3.zero;
foreach (GameObject o in tagged)
{
Vector3 localPos = transform.InverseTransformPoint(o.transform.position);
// If the local position has a positive Z value then it must lay
// behind the agent. (in which case it can be ignored)
if (localPos.z >= 0)
{
// If the distance from the x axis to the object's position is less
// than its radius + half the width of the detection box then there
// is a potential intersection.
float obstacleRadius = o.transform.localScale.x / 2;
float expandedRadius = myRadius + obstacleRadius;
if ((Math.Abs(localPos.y) < expandedRadius) && (Math.Abs(localPos.x) < expandedRadius))
{
// Now to do a ray/sphere intersection test. The center of the
// Create a temp Entity to hold the sphere in local space
Sphere tempSphere = new Sphere(expandedRadius, localPos);
// Create a ray
Ray ray = new Ray();
ray.pos = new Vector3(0, 0, 0);
ray.look = Vector3.forward;
// Find the point of intersection
if (tempSphere.closestRayIntersects(ray, Vector3.zero, ref intersection) == false)
{
return Vector3.zero;
}
// Now see if its the closest, there may be other intersecting spheres
float dist = intersection.magnitude;
if (dist < distToClosestIP)
{
dist = distToClosestIP;
closestIntersectingObstacle = o;
localPosOfClosestObstacle = localPos;
}
}
}
if (closestIntersectingObstacle != null)
{
// Now calculate the force
// Calculate Z Axis braking force
float multiplier = 200 * (1.0f + (boxLength - localPosOfClosestObstacle.z) / boxLength);
//calculate the lateral force
float obstacleRadius = closestIntersectingObstacle.GetComponent<Renderer>().bounds.extents.magnitude;
float expandedRadius = myRadius + obstacleRadius;
force.x = (expandedRadius - Math.Abs(localPosOfClosestObstacle.x)) * multiplier;
force.y = (expandedRadius - -Math.Abs(localPosOfClosestObstacle.y)) * multiplier;
if (localPosOfClosestObstacle.x > 0)
{
force.x = -force.x;
}
if (localPosOfClosestObstacle.y > 0)
{
force.y = -force.y;
}
Debug.DrawLine(transform.position, transform.position + transform.forward * boxLength, debugLineColour);
//apply a braking force proportional to the obstacle's distance from
//the vehicle.
const float brakingWeight = 40.0f;
force.z = (obstacleRadius -
localPosOfClosestObstacle.z) *
brakingWeight;
//finally, convert the steering vector from local to world space
force = transform.TransformPoint(force);
}
}
return force;
}
private bool RayTrace(Vector3 point0, Vector3 point1)
{
foreach (GameObject o in obstacles)
{
float radius = o.renderer.bounds.extents.magnitude;
Sphere sphere = new Sphere(radius, o.transform.position);
Ray ray = new Ray();
ray.look = point1 - point0;
ray.look.Normalize();
ray.pos = point0;
Vector3 intersectionPoint = new Vector3();
if (sphere.closestRayIntersects(ray, point0, ref intersectionPoint))
{
float dist = (intersectionPoint - point0).magnitude;
float rayLength = (point1 - point0).magnitude;
if (dist < rayLength)
{
return true;
}
}
}
return false;
}
