/// <summary>
/// Applies a given steering force to our momentum
/// </summary>
public void ApplySteeringForce(Vector3 force, float elapsedTime)
{
Vector3 adjustedForce = AdjustRawSteeringForce(force, elapsedTime);
// enforce limit on magnitude of steering force
Vector3 clippedForce = Vector3Helpers.TruncateLength(adjustedForce, MaxForce);
// compute acceleration and velocity
acceleration = (clippedForce / Mass);
Vector3 newVelocity = Velocity;
// Euler integrate (per frame) acceleration into velocity
newVelocity += acceleration * elapsedTime;
// enforce speed limit
newVelocity = Vector3Helpers.TruncateLength(newVelocity, MaxSpeed);
// update Speed
Speed = (newVelocity.Length);
// Euler integrate (per frame) velocity into position
Position = (Position + (newVelocity * elapsedTime));
// regenerate local space (by default: align vehicle's forward axis with
// new velocity, but this behavior may be overridden by derived classes.)
RegenerateLocalSpace(newVelocity, elapsedTime);
}
// reset state
public override void Reset()
{
// reset the vehicle
base.Reset();
// steering force is clipped to this magnitude
MaxForce = 27;
// velocity is clipped to this magnitude
MaxSpeed = 9;
// initial slow speed
Speed = (MaxSpeed * 0.3f);
// randomize initial orientation
//RegenerateOrthonormalBasisUF(Vector3Helpers.RandomUnitVector());
Vector3 d = Vector3Helpers.RandomUnitVector();
d.X = Math.Abs(d.X);
d.Y = 0;
d.Z = Math.Abs(d.Z);
RegenerateOrthonormalBasisUF(d);
// randomize initial position
Position = Vector3.UnitX * 10 + (Vector3Helpers.RandomVectorInUnitRadiusSphere() * 20);
// notify proximity database that our position has changed
//FIXME: SimpleVehicle::SimpleVehicle() calls reset() before proximityToken is set
if (proximityToken != null)
{
proximityToken.UpdateForNewPosition(Position);
}
}
// Take action to stay within sphereical boundary. Returns steering
// value (which is normally zero) and may take other side-effecting
// actions such as kinematically changing the Boid's position.
public Vector3 HandleBoundary()
{
// while inside the sphere do noting
if (Position.Length() < worldRadius)
{
return(Vector3.Zero);
}
// once outside, select strategy
switch (boundaryCondition)
{
case 0:
{
// steer back when outside
Vector3 seek = xxxSteerForSeek(Vector3.Zero);
Vector3 lateral = Vector3Helpers.PerpendicularComponent(seek, Forward);
return(lateral);
}
case 1:
{
// wrap around (teleport)
Position = (Vector3Helpers.SphericalWrapAround(Position, Vector3.Zero, worldRadius));
return(Vector3.Zero);
}
}
return(Vector3.Zero); // should not reach here
}
/// <summary>
/// Alternate Flee method. As the standard seek, but a smoother test for desired velocity.
/// </summary>
/// <param name="vehicle"></param>
/// <param name="target"></param>
/// <returns>The Vector to move to</returns>
public static Vector3 AlternateFlee(VehicleActor vehicle, Vector3 target)
{
Vector3 offset = vehicle.Position - target;
Vector3 desiredVelocity = Vector3Helpers.TruncateLength(offset, vehicle.MaximumVelocity);
return(desiredVelocity - vehicle.TrueVelocity);
}
public static void AddOneObstacle()
{
if (obstacleCount < maxObstacleCount)
{
// pick a random center and radius,
// loop until no overlap with other obstacles and the home base
float r;
Vector3 c;
float minClearance;
float requiredClearance = Globals.Seeker.Radius * 4; // 2 x diameter
do
{
r = Utilities.Random(1.5f, 4);
c = Vector3Helpers.RandomVectorOnUnitRadiusXZDisk() * Globals.MaxStartRadius * 1.1f;
minClearance = float.MaxValue;
System.Diagnostics.Debug.WriteLine(String.Format("[{0}, {1}, {2}]", c.X, c.Y, c.Z));
for (int so = 0; so < AllObstacles.Count; so++)
{
minClearance = TestOneObstacleOverlap(minClearance, r, AllObstacles[so].Radius, c, AllObstacles[so].Center);
}
minClearance = TestOneObstacleOverlap(minClearance, r, Globals.HomeBaseRadius - requiredClearance, c, Globals.HomeBaseCenter);
}while (minClearance < requiredClearance);
// add new non-overlapping obstacle to registry
AllObstacles.Add(new SphericalObstacle(r, c));
obstacleCount++;
}
}
public Vector3 SteerToEvadeAllDefenders()
{
Vector3 evade = Vector3.Zero;
float goalDistance = Vector3.Distance(Globals.HomeBaseCenter, Position);
// sum up weighted evasion
foreach (CtfEnemy e in Plugin.CtfEnemies)
{
Vector3 eOffset = e.Position - Position;
float eDistance = eOffset.Length();
float eForwardDistance = Vector3.Dot(Forward, eOffset);
float behindThreshold = Radius * 2;
bool behind = eForwardDistance < behindThreshold;
if ((!behind) || (eDistance < 5))
{
if (eDistance < (goalDistance * 1.2)) //xxx
{
// const float timeEstimate = 0.5f * eDistance / e.speed;//xxx
float timeEstimate = 0.15f * eDistance / e.Speed; //xxx
Vector3 future = e.PredictFuturePosition(timeEstimate);
annotation.CircleXZ(e.Radius, future, Globals.EvadeColor, 20); // xxx
Vector3 offset = future - Position;
Vector3 lateral = Vector3Helpers.PerpendicularComponent(offset, Forward);
float d = lateral.Length();
float weight = -1000 / (d * d);
evade += (lateral / d) * weight;
}
}
}
return(evade);
}
public Vector3 SteeringForSeeker()
{
// determine if obstacle avodiance is needed
bool clearPath = IsPathToGoalClear();
AdjustObstacleAvoidanceLookAhead(clearPath);
Vector3 obstacleAvoidance = SteerToAvoidObstacles(Globals.AvoidancePredictTime, AllObstacles);
// saved for annotation
Avoiding = (obstacleAvoidance != Vector3.Zero);
if (Avoiding)
{
// use pure obstacle avoidance if needed
return(obstacleAvoidance);
}
else
{
// otherwise seek home base and perhaps evade defenders
Vector3 seek = xxxSteerForSeek(Globals.HomeBaseCenter);
if (clearPath)
{
// we have a clear path (defender-free corridor), use pure seek
// xxx experiment 9-16-02
Vector3 s = Vector3Helpers.LimitMaxDeviationAngle(seek, 0.707f, Forward);
annotation.Line(Position, Position + (s * 0.2f), Globals.SeekColor);
return(s);
}
else
{
#if TESTING_CODE
if (false) // xxx testing new evade code xxx
{
// combine seek and (forward facing portion of) evasion
Vector3 evade = steerToEvadeAllDefenders();
Vector3 steer = seek + Vector3.limitMaxDeviationAngle(evade, 0.5f, forward());
// annotation: show evasion steering force
annotation.annotationLine(position(), position() + (steer * 0.2f), Globals.evadeColor);
return(steer);
}
else
#endif
{
Vector3 evade = XXXSteerToEvadeAllDefenders();
Vector3 steer = Vector3Helpers.LimitMaxDeviationAngle(seek + evade, 0.707f, Forward);
annotation.Line(Position, Position + seek, Color.Red);
annotation.Line(Position, Position + evade, Color.Green);
// annotation: show evasion steering force
annotation.Line(Position, Position + (steer * 0.2f), Globals.EvadeColor);
return(steer);
}
}
}
}
public void ParallelComponentTest()
{
var basis = Vector3.UnitY;
var v = Vector3.Normalize(new Vector3(1, 1, 0));
var result = Vector3Helpers.ParallelComponent(v, basis);
AssertVectorEquality(new Vector3(0, v.Y, 0), result);
}
public void PerpendicularComponentTest()
{
var basis = Vector3.UnitY;
var v = Vector3.Normalize(new Vector3(1, 1, 0));
var result = Vector3Helpers.PerpendicularComponent(v, basis);
AssertVectorEquality(new Vector3(v.X, 0, 0), result);
}
private void RandomizeStartingPositionAndHeading()
{
// randomize position on a ring between inner and outer radii
// centered around the home base
float rRadius = RandomHelpers.Random(10, 50);
Vector3 randomOnRing = Vector3Helpers.RandomUnitVectorOnXZPlane() * rRadius;
Position = (Globals.HomeBaseCenter + randomOnRing);
RandomizeHeadingOnXZPlane();
}
// xxx couldn't this be made more compact using localizePosition?
/// <summary>
/// Checks for intersection of the given spherical obstacle with a
/// volume of "likely future vehicle positions": a cylinder along the
/// current path, extending minTimeToCollision seconds along the
/// forward axis from current position.
///
/// If they intersect, a collision is imminent and this function returns
/// a steering force pointing laterally away from the obstacle's center.
///
/// Returns a zero vector if the obstacle is outside the cylinder
/// </summary>
/// <param name="v"></param>
/// <param name="minTimeToCollision"></param>
/// <returns></returns>
public Vector3 SteerToAvoid(IVehicle v, float minTimeToCollision)
{
// Capsule x Sphere collision detection
//http://www.altdev.co/2011/04/26/more-collision-detection-for-dummies/
var capStart = v.Position;
var capEnd = v.PredictFuturePosition(minTimeToCollision);
var alongCap = capEnd - capStart;
var capLength = alongCap.Length();
//If the vehicle is going very slowly then simply test vehicle sphere against obstacle sphere
if (capLength <= 0.05)
{
var distance = Vector3.Distance(Center, v.Position);
if (distance < Radius + v.Radius)
{
return(v.Position - Center);
}
return(Vector3.Zero);
}
var capAxis = alongCap / capLength;
//Project vector onto capsule axis
var b = Utilities.Clamp(Vector3.Dot(Center - capStart, capAxis), 0, capLength);
//Get point on axis (closest point to sphere center)
var r = capStart + capAxis * b;
//Get distance from circle center to closest point
var dist = Vector3.Distance(Center, r);
//Basic sphere sphere collision about the closest point
var inCircle = dist < Radius + v.Radius;
if (!inCircle)
{
return(Vector3.Zero);
}
//avoidance vector calculation
Vector3 avoidance = Vector3Helpers.PerpendicularComponent(v.Position - Center, v.Forward);
//if no avoidance was calculated this is because the vehicle is moving exactly forward towards the sphere, add in some random sideways deflection
if (avoidance == Vector3.Zero)
{
avoidance = -v.Forward + v.Side * 0.01f * RandomHelpers.Random();
}
avoidance = Vector3.Normalize(avoidance);
avoidance *= v.MaxForce;
avoidance += v.Forward * v.MaxForce * 0.75f;
return(avoidance);
}
private void FireMissile(Fighter launcher, Fighter target)
{
if (_missiles.Count(m => m.Target == target) < 3)
{
_missiles.Add(new Missile(_pd, target, Annotations)
{
Position = launcher.Position,
Forward = Vector3.Normalize(launcher.Forward * 0.9f + Vector3Helpers.RandomUnitVector() * 0.1f),
Speed = launcher.Speed,
Color = _team1.Contains(launcher) ? Color.Black : Color.White
});
}
}
public void RandomUnitVectorOnXzPlaneIsAlwaysLengthOne()
{
int set = 0;
for (int i = 0; i < 1000; i++)
{
var v = Vector3Helpers.RandomUnitVectorOnXZPlane();
Assert.IsTrue(Math.Abs(v.Length() - 1) < 0.000001f);
BitsetDirections(v, ref set);
}
// Y is always zero, so we expect to find every direction except positive Y
Assert.AreEqual(59, set);
}
public void RandomUnitVectorIsAlwaysLengthOne()
{
int set = 0;
for (int i = 0; i < 1000; i++)
{
var v = Vector3Helpers.RandomUnitVector();
Assert.IsTrue(Math.Abs(v.Length() - 1) < 0.000001f);
BitsetDirections(v, ref set);
}
// We expect to find every direction
Assert.AreEqual(63, set);
}
public void RandomVectorInUnitRadiusSphereIsAlwaysWithinOneUnitOfOrigin()
{
int set = 0;
for (int i = 0; i < 1000; i++)
{
var v = Vector3Helpers.RandomVectorInUnitRadiusSphere();
Assert.IsTrue(v.Length() <= 1);
BitsetDirections(v, ref set);
}
// We expect to find every direction
Assert.AreEqual(63, set);
}
public void RandomVectorOnUnitRadiusXZDiskIsAlwaysWithinOneUnitOfOrigin()
{
int set = 0;
for (int i = 0; i < 1000; i++)
{
var v = Vector3Helpers.RandomVectorOnUnitRadiusXZDisk();
Assert.IsTrue(v.Length() <= 1);
BitsetDirections(v, ref set);
}
// Y is always zero, so we expect to find every direction except positive Y
Assert.AreEqual(59, set);
}
// measure path curvature (1/turning-radius), maintain smoothed version
void MeasurePathCurvature(float elapsedTime)
{
if (elapsedTime > 0)
{
Vector3 dP = _lastPosition - Position;
Vector3 dF = (_lastForward - Forward) / dP.Length();
Vector3 lateral = Vector3Helpers.PerpendicularComponent(dF, Forward);
float sign = (Vector3.Dot(lateral, Side) < 0) ? 1.0f : -1.0f;
Curvature = lateral.Length() * sign;
Utilities.BlendIntoAccumulator(elapsedTime * 4.0f, Curvature, ref _smoothedCurvature);
_lastForward = Forward;
_lastPosition = Position;
}
}
// reset position
public void RandomizeStartingPositionAndHeading()
{
// randomize position on a ring between inner and outer radii
// centered around the home base
const float inner = 20;
const float outer = 30;
float radius = Utilities.Random(inner, outer);
Vector3 randomOnRing = Vector3Helpers.RandomUnitVectorOnXZPlane() * radius;
Position = (wanderer.Position + randomOnRing);
// randomize 2D heading
RandomizeHeadingOnXZPlane();
}
private Vector3 HandleBoundary()
{
// while inside the sphere do noting
if (Position.Length() < WORLD_RADIUS)
{
return(Vector3.Zero);
}
// steer back when outside
Vector3 seek = SteerForSeek(Vector3.Zero);
Vector3 lateral = Vector3Helpers.PerpendicularComponent(seek, Forward);
return(lateral);
}
// reset position
private void RandomizeStartingPositionAndHeading()
{
// randomize position on a ring between inner and outer radii
// centered around the home base
const float INNER = 20;
const float OUTER = 30;
float radius = RandomHelpers.Random(INNER, OUTER);
Vector3 randomOnRing = Vector3Helpers.RandomUnitVectorOnXZPlane() * radius;
Position = (_wanderer.Position + randomOnRing);
// randomize 2D heading
RandomizeHeadingOnXZPlane();
}
public override void Open()
{
CreateDatabase();
_missiles.Clear();
_team1.Add(new Fighter(_pd, Annotations, FireMissile)
{
Position = new Vector3(20, 0, 0),
Forward = Vector3Helpers.RandomUnitVector(),
Color = Color.Green,
Enemy = _team2
});
_team1.Add(new Fighter(_pd, Annotations, FireMissile)
{
Position = new Vector3(15, 0, 5),
Forward = Vector3Helpers.RandomUnitVector(),
Color = Color.Green,
Enemy = _team2
});
_team1.Add(new Fighter(_pd, Annotations, FireMissile)
{
Position = new Vector3(15, 0, -5),
Forward = Vector3Helpers.RandomUnitVector(),
Color = Color.Green,
Enemy = _team2
});
_team2.Add(new Fighter(_pd, Annotations, FireMissile)
{
Position = new Vector3(-20, 0, 0),
Forward = Vector3Helpers.RandomUnitVector(),
Color = Color.Blue,
Enemy = _team1
});
_team2.Add(new Fighter(_pd, Annotations, FireMissile)
{
Position = new Vector3(-15, 0, 5),
Forward = Vector3Helpers.RandomUnitVector(),
Color = Color.Blue,
Enemy = _team1
});
_team2.Add(new Fighter(_pd, Annotations, FireMissile)
{
Position = new Vector3(-15, 0, -5),
Forward = Vector3Helpers.RandomUnitVector(),
Color = Color.Blue,
Enemy = _team1
});
}
/// <summary>
/// Adjusts the steering force passed to ApplySteeringForce
/// </summary>
public virtual Vector3 AdjustRawSteeringForce(Vector3 force, float deltaTime)
{
float maxAdjustedSpeed = 0.2f * MaxSpeed;
if ((Speed > maxAdjustedSpeed) || (force == Vector3.Zero))
{
return(force);
}
else
{
float range = Speed / maxAdjustedSpeed;
float cosine = Utilities.Interpolate((float)Math.Pow(range, 20), 1.0f, -1.0f);
return(Vector3Helpers.LimitMaxDeviationAngle(force, cosine, Forward));
}
}
public void ClipWithoutConeIsAlwaysWithoutCone()
{
for (int i = 0; i < 5000; i++)
{
var vector = Vector3Helpers.RandomUnitVector();
var basis = Vector3Helpers.RandomUnitVector();
var angle = RandomHelpers.Random(0.1f, PiOver2);
var cosAngle = (float)Math.Cos(angle);
var result = vector.LimitMinDeviationAngle(cosAngle, basis);
var measuredAngle = (float)Math.Acos(Vector3.Dot(result, basis));
Assert.IsTrue(measuredAngle >= angle - 0.0001f);
}
}
public void FindPerpendicularIn3dIsAlwaysPerpendicular()
{
int set = 0;
for (int i = 0; i < 1000; i++)
{
var v = Vector3Helpers.RandomUnitVector();
var perp = v.FindPerpendicularIn3d();
BitsetDirections(perp, ref set);
Assert.AreEqual(0, Vector3.Dot(v, perp));
}
Assert.AreEqual(63, set);
}
public static void AddOneObstacle()
{
if (obstacleCount < maxObstacleCount)
{
// pick a random center and radius,
// loop until no overlap with other obstacles and the home base
//float r = 15;
//Vector3 c = Vector3.Up * r * (-0.5f * maxObstacleCount + obstacleCount);
float r = Utilities.Random(0.5f, 2);
Vector3 c = Vector3Helpers.RandomVectorInUnitRadiusSphere() * worldRadius * 1.1f;
// add new non-overlapping obstacle to registry
AllObstacles.Add(new SphericalObstacle(r, c));
obstacleCount++;
}
}
private static void AddOneObstacle()
{
if (_obstacleCount >= MAX_OBSTACLE_COUNT)
{
return;
}
// pick a random center and radius,
// loop until no overlap with other obstacles and the home base
//float r = 15;
//Vector3 c = Vector3.Up * r * (-0.5f * maxObstacleCount + obstacleCount);
float r = RandomHelpers.Random(0.5f, 2);
Vector3 c = Vector3Helpers.RandomVectorInUnitRadiusSphere() * WORLD_RADIUS * 1.1f;
// add new non-overlapping obstacle to registry
AllObstacles.Add(new SphericalObstacle(r, c));
_obstacleCount++;
}
// reset all instance state
public override void Reset()
{
// reset the vehicle
base.Reset();
// max speed and max steering force (maneuverability)
MaxSpeed = 2.0f;
MaxForce = 8.0f;
// initially stopped
Speed = 0;
// size of bounding sphere, for obstacle avoidance, etc.
Radius = 0.5f; // width = 0.7, add 0.3 margin, take half
// set the path for this Pedestrian to follow
path = Globals.GetTestPath();
// set initial position
// (random point on path + random horizontal offset)
float d = path.TotalPathLength * Utilities.Random();
float r = path.radius;
Vector3 randomOffset = Vector3Helpers.RandomVectorOnUnitRadiusXZDisk() * r;
Position = (path.MapPathDistanceToPoint(d) + randomOffset);
// randomize 2D heading
RandomizeHeadingOnXZPlane();
// pick a random direction for path following (upstream or downstream)
pathDirection = (Utilities.Random() > 0.5) ? -1 : +1;
// trail parameters: 3 seconds with 60 points along the trail
trail = new Trail(3, 60);
// notify proximity database that our position has changed
if (proximityToken != null)
{
proximityToken.UpdateForNewPosition(Position);
}
}
private void RandomizeStartingPositionAndHeading()
{
// randomize position on a ring between inner and outer radii
// centered around the home base
float rRadius = RandomHelpers.Random(Globals.MIN_START_RADIUS, Globals.MAX_START_RADIUS);
Vector3 randomOnRing = Vector3Helpers.RandomUnitVectorOnXZPlane() * rRadius;
Position = (Globals.HomeBaseCenter + randomOnRing);
// are we are too close to an obstacle?
if (MinDistanceToObstacle(Position) < Radius * 5)
{
// if so, retry the randomization (this recursive call may not return
// if there is too little free space)
RandomizeStartingPositionAndHeading();
}
else
{
// otherwise, if the position is OK, randomize 2D heading
RandomizeHeadingOnXZPlane();
}
}
public static void AddOneObstacle(float radius)
{
// pick a random center and radius,
// loop until no overlap with other obstacles and the home base
float r;
Vector3 c;
float minClearance;
float requiredClearance = Globals.Seeker.Radius * 4; // 2 x diameter
do
{
r = RandomHelpers.Random(1.5f, 4);
c = Vector3Helpers.RandomVectorOnUnitRadiusXZDisk() * Globals.MAX_START_RADIUS * 1.1f;
minClearance = AllObstacles.Aggregate(float.MaxValue, (current, t) => TestOneObstacleOverlap(current, r, t.Radius, c, t.Center));
minClearance = TestOneObstacleOverlap(minClearance, r, radius - requiredClearance, c, Globals.HomeBaseCenter);
}while (minClearance < requiredClearance);
// add new non-overlapping obstacle to registry
AllObstacles.Add(new SphericalObstacle(r, c));
ObstacleCount++;
}
/// <summary>
/// Used primarily by SteerToAvoidNeighbors.
/// Hard steer away from another vehicle entity who comes within the
/// seperation distance. Returns Vector zero if no avoidance needed
/// </summary>
/// <param name="vehicle"></param>
/// <param name="minSeparationDistance"></param>
/// <param name="others"></param>
/// <returns>The Vector to move to</returns>
public static Vector3 AvoidCloseNeighbors(VehicleActor vehicle, float minSeparationDistance, List <VehicleActor> others)
{
// for each of the other vehicles...
foreach (VehicleActor other in others)
{
if (other != vehicle)
{
float sumOfRadii = vehicle.BoundingSphereRadius + other.BoundingSphereRadius;
float minCenterToCenter = minSeparationDistance + sumOfRadii;
Vector3 offset = other.Position - vehicle.Position;
float currentDistance = offset.Length();
if (currentDistance < minCenterToCenter)
{
//log AvoidCloseNeighbor(other, minSeparationDistance);
return(Vector3Helpers.PerpendicularComponent(-offset, vehicle.Forward));
}
}
}
// otherwise return zero
return(Vector3.Zero);
}
