Vector3 obstacleAvoidance() { Vector3 force = Vector3.Zero; makeFeelers(); List <Sphere> tagged = new List <Sphere>(); float minBoxLength = 20.0f; float boxLength = minBoxLength + ((fighter.velocity.Length() / fighter.maxSpeed) * minBoxLength * 2.0f); if (float.IsNaN(boxLength)) { System.Console.WriteLine("NAN"); } // Matt Bucklands Obstacle avoidance // First tag obstacles in range foreach (Entity child in XNAGame.Instance.Children) { if (child is Obstacle) { Obstacle obstacle = (Obstacle)child; Vector3 toCentre = fighter.Position - obstacle.Position; float dist = toCentre.Length(); if (dist < boxLength) { tagged.Add(obstacle); } } } float distToClosestIP = float.MaxValue; Sphere closestIntersectingObstacle = null; Vector3 localPosOfClosestObstacle = Vector3.Zero; Vector3 intersection = Vector3.Zero; Matrix localTransform = Matrix.Invert(fighter.worldTransform); foreach (Obstacle o in tagged) { Vector3 localPos = Vector3.Transform(o.Position, localTransform); //Vector3 localPos = o.pos - fighter.pos; // 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 expandedRadius = fighter.BoundingSphere.Radius + o.Radius; 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); tempSphere.Position = localPos; // Create a ray Ray ray = new Ray(); ray.pos = new Vector3(0, 0, 0); ray.look = fighter.basis; // 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.Length(); 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 expandedRadius = fighter.BoundingSphere.Radius + o.Radius; force.X = (expandedRadius - Math.Abs(localPosOfClosestObstacle.X)) * multiplier; force.Y = (expandedRadius - -Math.Abs(localPosOfClosestObstacle.X)) * multiplier; if (localPosOfClosestObstacle.X > 0) { force.X = -force.X; } if (localPosOfClosestObstacle.Y > 0) { force.Y = -force.Y; } /*if (fighter.pos.X < o.pos.X) * { * force.X = -force.X; * } * * if (fighter.pos.Y < o.pos.Y) * { * force.Y = -force.Y; * }*/ Line.DrawLine(fighter.Position, fighter.Position + fighter.Look * boxLength, Color.BlueViolet); //apply a braking force proportional to the obstacle's distance from //the vehicle. const float brakingWeight = 40.0f; force.Z = (closestIntersectingObstacle.Radius - localPosOfClosestObstacle.Z) * brakingWeight; //finally, convert the steering vector from local to world space force = Vector3.Transform(force, fighter.worldTransform); } } fighter.DrawFeelers = false; fighter.DrawAxis = false; return(force); }
public static void setUpStateMachineDemo() { List<Entity> children = XNAGame.Instance().Children; Ground ground = new Ground(); children.Add(ground); XNAGame.Instance().Ground = ground; AIFighter aiFighter = new AIFighter(); aiFighter.pos = new Vector3(-20, 50, 50); aiFighter.maxSpeed = 16.0f; aiFighter.SwicthState(new IdleState(aiFighter)); aiFighter.Path.DrawPath = true; children.Add(aiFighter); Fighter fighter = new Fighter(); fighter.ModelName = "ship2"; fighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.arrive); fighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.obstacle_avoidance); fighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.wall_avoidance); fighter.pos = new Vector3(10, 50, 0); fighter.targetPos = aiFighter.pos + new Vector3(-50, 0, -80); children.Add(fighter); Fighter camFighter = new Fighter(); camFighter.Leader = fighter; camFighter.offset = new Vector3(0, 5, 10); camFighter.pos = fighter.pos + camFighter.offset; camFighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.offset_pursuit); camFighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.wall_avoidance); camFighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.obstacle_avoidance); XNAGame.Instance().CamFighter = camFighter; children.Add(camFighter); XNAGame.Instance().Leader = fighter; Camera camera = XNAGame.Instance().Camera; camera.pos = new Vector3(0.0f, 60.0f, 100.0f); Obstacle o = new Obstacle(4); o.pos = new Vector3(0, 50, -10); children.Add(o); o = new Obstacle(4); o.pos = new Vector3(50, 0, -90) + aiFighter.pos; children.Add(o); }
public static void setUpCylonchase() { List<Entity> children = XNAGame.Instance().Children; Fighter cylonScout = new Fighter("models/Cylon/CylonRaider"); cylonScout.pos = new Vector3(10, 20, 20); cylonScout.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.seek); cylonScout.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.obstacle_avoidance); cylonScout.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.wall_avoidance); cylonScout.targetPos = new Vector3(0, 100, -450); children.Add(cylonScout); XNAGame.Instance().Leader = cylonScout; // Add some Obstacles Obstacle o = new Obstacle(4); o.pos = new Vector3(0, 10, -10); children.Add(o); o = new Obstacle(17); o.pos = new Vector3(-10, 16, -80); children.Add(o); o = new Obstacle(10); o.pos = new Vector3(10, 15, -120); children.Add(o); o = new Obstacle(12); o.pos = new Vector3(5, -10, -150); children.Add(o); o = new Obstacle(20); o.pos = new Vector3(-2, 5, -200); children.Add(o); o = new Obstacle(10); o.pos = new Vector3(-25, -20, -250); children.Add(o); o = new Obstacle(10); o.pos = new Vector3(20, -20, -250); children.Add(o); o = new Obstacle(35); o.pos = new Vector3(-10, -30, -300); children.Add(o); // Now make a fleet int fleetSize = 5; float xOff = 6; float zOff = 6; for (int i = 2; i < fleetSize; i++) { for (int j = 0; j < i; j++) { float z = (i - 1) * +zOff; Fighter fleet = new Fighter(); fleet.Leader = cylonScout; fleet.offset = new Vector3((xOff * (-i / 2.0f)) + (j * xOff), 0, z); fleet.pos = cylonScout.pos + fleet.offset; fleet.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.offset_pursuit); fleet.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.wall_avoidance); fleet.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.obstacle_avoidance); children.Add(fleet); } } Fighter camFighter = new Fighter(); camFighter.Leader = cylonScout; camFighter.pos = new Vector3(0, 15, fleetSize * zOff); camFighter.offset = new Vector3(0, 5, fleetSize * zOff); camFighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.offset_pursuit); camFighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.wall_avoidance); camFighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.obstacle_avoidance); XNAGame.Instance().CamFighter = camFighter; children.Add(camFighter); Ground ground = new Ground(); children.Add(ground); XNAGame.Instance().Ground = ground; foreach (Entity child in children) { child.pos.Y += 100; } }
public static void setUpFlockingDemo() { Params.Load("flocking.properties"); List<Entity> children = XNAGame.Instance().Children; // Create an EliteFighter instance to be the pursuer float range = Params.GetFloat("world_range"); Fighter fighter = new Fighter(); fighter.ModelName = "ship2"; //fighter.pos = new Vector3(10.0f, 10.0f, 10.0f); //fighter.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.pursuit); // Create Params.GetFloat("num_boids") boids and turn on the appropriate steering behaviours.. float boids = Params.GetFloat("num_boids"); for (int i = 0; i < boids; i++) { Fighter boid = new Fighter(); boid.ModelName = "fighter"; boid.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.alignment); boid.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.cohesion); boid.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.wander); boid.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.sphere_constrain); boid.SteeringBehaviours.turnOn(SteeringBehaviours.behaviour_type.separation); // use Vector3 pos = randomPosition(range); to generate random positions Vector3 pos = randomPosition(range); boid.pos = pos; //fighter.targetPos = boid.pos + new Vector3(-50, 0, -80); children.Add(boid); } //children.Add(fighter); int numObstacles = 5; float dist = (range * 2) / numObstacles; for (float x = - range ; x < range ; x+= dist) { for (float z = - range ; z < range ; z += dist) { Obstacle o = new Obstacle(20); o.pos = new Vector3(x, 0, z); o.Color = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()); o.ShouldDraw = true; children.Add(o); } } // Create a camera fighter // Give it a "Leader" to follow and turn on offset pursuit //XNAGame.Instance().CamFighter = XXX? Camera camera = XNAGame.Instance().Camera; camera.pos = new Vector3(0.0f, 60.0f, 200.0f); }