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);
}