/// <summary>
/// Calculates the force necessary to avoid the closest spherical obstacle
/// </summary>
/// <returns>
/// Force necessary to avoid an obstacle, or Vector3.zero
/// </returns>
/// <remarks>
/// This method will iterate through all detected spherical obstacles that
/// are within MinTimeToCollision, and steer to avoid the closest one to the
/// vehicle. It's not ideal, as that means the vehicle might crash into
/// another obstacle while avoiding the closest one, but it'll do.
/// </remarks>
protected override Vector3 CalculateForce()
{
Vector3 avoidance = Vector3.zero;
if (Vehicle.Radar.Obstacles == null || Vehicle.Radar.Obstacles.Count == 0)
{
return(avoidance);
}
PathIntersection nearest = new PathIntersection(null);
/*
* While we could just calculate line as (Velocity * predictionTime)
* and save ourselves the substraction, this allows other vehicles to
* override PredictFuturePosition for their own ends.
*/
Vector3 futurePosition = Vehicle.PredictFuturePosition(_minTimeToCollision);
Vector3 line = (futurePosition - Vehicle.Position);
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
Profiler.BeginSample("Find nearest intersection");
foreach (var o in Vehicle.Radar.Obstacles)
{
SphericalObstacle sphere = o as SphericalObstacle;
PathIntersection next = FindNextIntersectionWithSphere(sphere, line);
if (!nearest.intersect ||
(next.intersect &&
next.distance < nearest.distance))
{
nearest = next;
}
}
Profiler.EndSample();
// when a nearest intersection was found
Profiler.BeginSample("Calculate avoidance");
if (nearest.intersect &&
nearest.distance < line.magnitude)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, nearest.obstacle.center, Color.red);
#endif
// compute avoidance steering force: take offset from obstacle to me,
// take the component of that which is lateral (perpendicular to my
// forward direction), set length to maxForce, add a bit of forward
// component (in capture the flag, we never want to slow down)
Vector3 offset = Vehicle.Position - nearest.obstacle.center;
avoidance = OpenSteerUtility.perpendicularComponent(offset, transform.forward);
avoidance.Normalize();
avoidance *= Vehicle.MaxForce;
avoidance += transform.forward * Vehicle.MaxForce * _avoidanceForceFactor;
}
Profiler.EndSample();
return(avoidance);
}
/// <summary>
/// Checks for sphere collision.
/// </summary>
/// <param name="obstacle">The obstacle.</param>
/// <param name="tolerance">The tolerance.</param>
/// <returns>
/// <c>true</c> if it collides, <c>false</c> otherwise.
/// </returns>
public bool CollidesWith(SphericalObstacle obstacle, double tolerance = 0.0d)
{
Vector3 difference = Center - obstacle.Center;
double distance =
System.Math.Sqrt(System.Math.Pow(difference.X, 2) + System.Math.Pow(difference.Y, 2) + System.Math.Pow(difference.Z, 2));
double sumRadius = Radius + obstacle.Radius;
return(distance < (sumRadius + tolerance));
}
private void OnDrawGizmos()
{
if (base.Vehicle != null)
{
using (IEnumerator <Obstacle> enumerator = base.Vehicle.Radar.Obstacles.GetEnumerator())
{
while (enumerator.MoveNext())
{
Obstacle current = enumerator.get_Current();
SphericalObstacle sphericalObstacle = current as SphericalObstacle;
Gizmos.set_color(Color.get_red());
Gizmos.DrawWireSphere(sphericalObstacle.center, sphericalObstacle.radius);
}
}
}
}
protected override Vector3 CalculateForce()
{
Vector3 vector = Vector3.get_zero();
if (base.Vehicle.Radar.Obstacles == null || base.Vehicle.Radar.Obstacles.Count == 0)
{
return(vector);
}
SteerForSphericalObstacleAvoidance.PathIntersection pathIntersection = new SteerForSphericalObstacleAvoidance.PathIntersection(null);
Vector3 vector2 = base.Vehicle.PredictFuturePosition(this._minTimeToCollision);
Vector3 line = vector2 - base.Vehicle.Position;
using (IEnumerator <Obstacle> enumerator = base.Vehicle.Radar.Obstacles.GetEnumerator())
{
while (enumerator.MoveNext())
{
Obstacle current = enumerator.get_Current();
SphericalObstacle obs = current as SphericalObstacle;
SteerForSphericalObstacleAvoidance.PathIntersection pathIntersection2 = this.FindNextIntersectionWithSphere(obs, line);
if (!pathIntersection.intersect || (pathIntersection2.intersect && pathIntersection2.distance < pathIntersection.distance))
{
pathIntersection = pathIntersection2;
}
}
}
if (pathIntersection.intersect && pathIntersection.distance < line.get_magnitude())
{
Debug.DrawLine(base.Vehicle.Position, pathIntersection.obstacle.center, Color.get_red());
Vector3 source = base.Vehicle.Position - pathIntersection.obstacle.center;
vector = OpenSteerUtility.perpendicularComponent(source, base.get_transform().get_forward());
vector.Normalize();
vector *= base.Vehicle.MaxForce;
vector += base.get_transform().get_forward() * base.Vehicle.MaxForce * this._avoidanceForceFactor;
}
return(vector);
}
/// <summary>
/// Load content for the screen.
/// </summary>
/// <param name="GraphicInfo"></param>
/// <param name="factory"></param>
/// <param name="contentManager"></param>
protected override void LoadContent(GraphicInfo GraphicInfo, GraphicFactory factory, IContentManager contentManager)
{
base.LoadContent(GraphicInfo, factory, contentManager);
RegisterDebugDrawCommand rc = new RegisterDebugDrawCommand(ddrawer);
CommandProcessor.getCommandProcessor().SendCommandAssyncronous(rc);
path.AddCircularObstacle(Vector3.Zero, 10);
path.AddCircularObstacle(new Vector3(0, 20, 0), 15);
path.AddCircularObstacle(new Vector3(200), 10);
foreach (var item in path.Obstacles)
{
SphericalObstacle SphericalObstacle = item as SphericalObstacle;
DebugSphere s = new DebugSphere(SphericalObstacle.Center, SphericalObstacle.Radius, Color.Blue);
ddrawer.AddShape(s);
}
//DebugSphere.WireFrameEnabled = true;
DebugLines dls = new DebugLines();
ddrawer.AddShape(dls);
for (int i = 0; i < path.PolyPath.pointCount - 1; i++)
{
dls.AddLine(path.PolyPath.points[i], path.PolyPath.points[i] + Vector3.Up * 200, Color.Brown);
dls.AddLine(path.PolyPath.points[i], path.PolyPath.points[i + 1], Color.Red);
}
dls.AddLine(path.PolyPath.points[path.PolyPath.pointCount - 1], path.PolyPath.points[path.PolyPath.pointCount - 1] + Vector3.Up * 200, Color.Brown);
PlugIn = new PedestrianPlugIn(this.World, path,
(pd) =>
{
SimpleModel simpleModel = new SimpleModel(factory, "Model//block");
simpleModel.SetTexture(factory.CreateTexture2DColor(1, 1, Color.Green), TextureType.DIFFUSE);
///Physic info (position, rotation and scale are set here)
GhostObject tmesh = new GhostObject();
///Forward Shader (look at this shader construction for more info)
ForwardXNABasicShader shader = new ForwardXNABasicShader();
///Deferred material
ForwardMaterial fmaterial = new ForwardMaterial(shader);
///The object itself
IObject obj = new IObject(fmaterial, simpleModel, tmesh);
obj.IObjectAttachment.Add(new SteerAtachment(pd));
return(obj);
});
PlugIn.Init();
{
SimpleModel simpleModel = new SimpleModel(factory, "Model//block");
simpleModel.SetTexture(factory.CreateTexture2DColor(1, 1, Color.White), TextureType.DIFFUSE);
///Physic info (position, rotation and scale are set here)
BoxObject tmesh = new BoxObject(Vector3.Zero, 1, 1, 1, 10, new Vector3(1000, 1, 1000), Matrix.Identity, MaterialDescription.DefaultBepuMaterial());
tmesh.isMotionLess = true;
///Forward Shader (look at this shader construction for more info)
ForwardXNABasicShader shader = new ForwardXNABasicShader();
///Deferred material
ForwardMaterial fmaterial = new ForwardMaterial(shader);
///The object itself
this.World.AddObject(new IObject(fmaterial, simpleModel, tmesh));
}
///add a camera
this.World.CameraManager.AddCamera(new CameraFirstPerson(GraphicInfo));
}
public SteerForSphericalObstacleAvoidance.PathIntersection FindNextIntersectionWithSphere(SphericalObstacle obs, Vector3 line)
{
Vector3 vector = base.Vehicle.Position - obs.center;
SteerForSphericalObstacleAvoidance.PathIntersection result = new SteerForSphericalObstacleAvoidance.PathIntersection(obs);
obs.annotatePosition();
Debug.DrawLine(base.Vehicle.Position, base.Vehicle.Position + line, Color.get_cyan());
float sqrMagnitude = line.get_sqrMagnitude();
float num = 2f * Vector3.Dot(line, vector);
float num2 = obs.center.get_sqrMagnitude();
num2 += base.Vehicle.Position.get_sqrMagnitude();
num2 -= 2f * Vector3.Dot(obs.center, base.Vehicle.Position);
num2 -= Mathf.Pow(obs.radius + base.Vehicle.ScaledRadius, 2f);
float num3 = num * num - 4f * sqrMagnitude * num2;
if (num3 >= 0f)
{
result.intersect = true;
Vector3 vector2 = Vector3.get_zero();
if (num3 == 0f)
{
float num4 = -num / (2f * sqrMagnitude);
vector2 = num4 * line;
}
else
{
float num5 = (-num + Mathf.Sqrt(num3)) / (2f * sqrMagnitude);
float num6 = (-num - Mathf.Sqrt(num3)) / (2f * sqrMagnitude);
if (num5 < 0f && num6 < 0f)
{
result.intersect = false;
}
else
{
vector2 = ((Mathf.Abs(num5) >= Mathf.Abs(num6)) ? (num6 * line) : (num5 * line));
}
}
Debug.DrawRay(base.Vehicle.Position, vector2, Color.get_red());
result.distance = vector2.get_magnitude();
}
return(result);
}
public PathIntersection(SphericalObstacle obstacle)
{
this.obstacle = obstacle;
this.intersect = false;
this.distance = 3.40282347E+38f;
}
public PathIntersection(SphericalObstacle obstacle)
{
this.obstacle = obstacle;
intersect = false;
distance = float.MaxValue;
}
/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="obs">
/// A spherical obstacle to check against <see cref="SphericalObstacle"/>
/// </param>
/// <param name="line">
/// Line that we expect we'll follow to our future destination
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
public PathIntersection FindNextIntersectionWithSphere(SphericalObstacle obs, Vector3 line)
{
/*
* This routine is based on the Paul Bourke's derivation in:
* Intersection of a Line and a Sphere (or circle)
* http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/
*
* Retaining the same variable values used in that description.
*
*/
float a, b, c, bb4ac;
var toCenter = Vehicle.Position - obs.center;
// initialize pathIntersection object
var intersection = new PathIntersection(obs);
#if ANNOTATE_AVOIDOBSTACLES
obs.annotatePosition();
Debug.DrawLine(Vehicle.Position, Vehicle.Position + line, Color.cyan);
#endif
// computer line-sphere intersection parameters
a = line.sqrMagnitude;
b = 2 * Vector3.Dot(line, toCenter);
c = obs.center.sqrMagnitude;
c += Vehicle.Position.sqrMagnitude;
c -= 2 * Vector3.Dot(obs.center, Vehicle.Position);
c -= Mathf.Pow(obs.radius + Vehicle.ScaledRadius, 2);
bb4ac = b * b - 4 * a * c;
if (bb4ac >= 0)
{
intersection.intersect = true;
Vector3 closest = Vector3.zero;
if (bb4ac == 0)
{
// Only one intersection
var mu = -b / (2 * a);
closest = mu * line;
}
else
{
// More than one intersection
var mu1 = (-b + Mathf.Sqrt(bb4ac)) / (2 * a);
var mu2 = (-b - Mathf.Sqrt(bb4ac)) / (2 * a);
/*
* If the results are negative, the obstacle is behind us.
*
* If one result is negative and the other one positive,
* that would indicate that one intersection is behind us while
* the other one ahead of us, which would mean that we're
* just overlapping the obstacle, so we should still avoid.
*/
if (mu1 < 0 && mu2 < 0)
{
intersection.intersect = false;
}
else
{
closest = (Mathf.Abs(mu1) < Mathf.Abs(mu2)) ? mu1 * line : mu2 * line;
}
}
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawRay(Vehicle.Position, closest, Color.red);
#endif
intersection.distance = closest.magnitude;
}
return(intersection);
}
