Vector3 SceneAvoidance() { Vector3 force = Vector3.zero; RaycastHit info; // Check the forward feeler and generate a up force Vector3 feeler = Vector3.forward; feeler = transform.TransformDirection(feeler); if (Physics.Raycast(transform.position, feeler, out info, sceneAvoidanceFeelerDepth)) { // Push me away in the direction of the up vector. // The deeper the penetration the bigger the force float depth = (info.point - feeler).magnitude; force = transform.up * depth; if (drawGizmos) { LineDrawer.DrawLine(transform.position, transform.position + feeler * sceneAvoidanceFeelerDepth, Color.red); } } // Check the bottom feeler and generate an upwards force feeler = Vector3.forward; feeler = Quaternion.AngleAxis(45, Vector3.right) * feeler; feeler = transform.TransformDirection(feeler); if (Physics.Raycast(transform.position, feeler, out info, sceneAvoidanceFeelerDepth)) { // Push me away in the direction of the up vector. // The deeper the penetration the bigger the force float depth = (info.point - feeler).magnitude; force += transform.up * depth; if (drawGizmos) { LineDrawer.DrawLine(transform.position, transform.position + feeler * sceneAvoidanceFeelerDepth, Color.red); } } // Check the top feeler and generate a downward force feeler = Vector3.forward; feeler = Quaternion.AngleAxis(-45, Vector3.right) * feeler; feeler = transform.TransformDirection(feeler); if (Physics.Raycast(transform.position, feeler, out info, sceneAvoidanceFeelerDepth)) { // Push me away in the direction of the up vector. // The deeper the penetration the bigger the force float depth = (info.point - feeler).magnitude; force -= transform.up * depth; if (drawGizmos) { LineDrawer.DrawLine(transform.position, transform.position + feeler * sceneAvoidanceFeelerDepth, Color.red); } } // Check the left feeler and generate a right force feeler = Vector3.forward; feeler = Quaternion.AngleAxis(45, Vector3.right) * feeler; feeler = transform.TransformDirection(feeler); if (Physics.Raycast(transform.position, feeler, out info, sceneAvoidanceFeelerDepth)) { // Push me away in the direction of the up vector. // The deeper the penetration the bigger the force float depth = (info.point - feeler).magnitude; force += transform.right * depth; if (drawGizmos) { LineDrawer.DrawLine(transform.position, transform.position + feeler * sceneAvoidanceFeelerDepth, Color.red); } } // Check the right feeler and generate a left force feeler = Vector3.forward; feeler = Quaternion.AngleAxis(-45, Vector3.right) * feeler; feeler = transform.TransformDirection(feeler); if (Physics.Raycast(transform.position, feeler, out info, sceneAvoidanceFeelerDepth)) { // Push me away in the direction of the up vector. // The deeper the penetration the bigger the force float depth = (info.point - feeler).magnitude; force -= transform.right * depth; if (drawGizmos) { LineDrawer.DrawLine(transform.position, transform.position + feeler * sceneAvoidanceFeelerDepth, Color.red); } } return(force); }
Vector3 ObstacleAvoidance() { Vector3 force = Vector3.zero; //makeFeelers(); List <Obstacle> tagged = new List <Obstacle>(); float boxLength = minBoxLength + ((velocity.magnitude / maxSpeed) * minBoxLength * 2.0f); if (float.IsNaN(boxLength)) { System.Console.WriteLine("NAN"); } Obstacle[] obstacles = BoidManager.Instance.obstacles; // Matt Bucklands Obstacle avoidance // First tag obstacles in range if (obstacles.Length == 0) { return(Vector3.zero); } foreach (Obstacle obstacle in obstacles) { // if (obstacle == null || obstacle.gameObject == gameObject) { continue; } Vector3 toCentre = transform.position - obstacle.transform.position; float dist = toCentre.magnitude; if (dist < boxLength) { tagged.Add(obstacle); } } float distToClosestIP = float.MaxValue; Obstacle closestIntersectingObstacle = null; Vector3 localPosOfClosestObstacle = Vector3.zero; Vector3 intersection = Vector3.zero; foreach (Obstacle 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 obstacleRadius = o.radius; float expandedRadius = radius + 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) { continue; } // 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 float multiplier = 1.0f + (boxLength - localPosOfClosestObstacle.z) / boxLength; //calculate the lateral force float obstacleRadius = closestIntersectingObstacle.radius; // closestIntersectingObstacle.GetComponent<Renderer>().bounds.extents.magnitude; float expandedRadius = radius + obstacleRadius; force.x = (expandedRadius - Math.Abs(localPosOfClosestObstacle.x)) * multiplier; force.y = (expandedRadius - Math.Abs(localPosOfClosestObstacle.y)) * multiplier; // Generate positive or negative direction so we steer around! // Not always in the same direction as in Matt Bucklands book if (localPosOfClosestObstacle.x > 0) { force.x = -force.x; } // If the obstacle is above, steer down if (localPosOfClosestObstacle.y > 0) { force.y = -force.y; } if (drawGizmos) { LineDrawer.DrawLine(transform.position, transform.position + transform.forward * boxLength, Color.grey); } //apply a braking force proportional to the obstacle's distance from //the vehicle. const float brakingWeight = 0.01f; force.z = (expandedRadius - localPosOfClosestObstacle.z) * brakingWeight; //finally, convert the steering vector from local to world space // Dont include position! force = transform.TransformDirection(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 / 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); }