public override Vector3d AvoidEdges(IAgent agent, double distance)
{
Point3d refPosition = agent.Position;
double maxSpeed = agent.MaxSpeed;
Vector3d velocity = agent.Velocity;
bool avoided = false;
Vector3d desired = velocity;
if (refPosition.X < minX + distance)
{
//desired = new Vector3d(maxSpeed, velocity.Y, velocity.Z);
desired.X = maxSpeed;
avoided = true;
}
else if (refPosition.X >= maxX - distance)
{
//desired = new Vector3d(-maxSpeed, velocity.Y, velocity.Z);
desired.X = -maxSpeed;
avoided = true;
}
if (refPosition.Y < minY + distance)
{
//desired = new Vector3d(velocity.X, maxSpeed, velocity.Z);
desired.Y = maxSpeed;
avoided = true;
}
else if (refPosition.Y >= maxY - distance)
{
//desired = new Vector3d(velocity.X, -maxSpeed, velocity.Z);
desired.Y = -maxSpeed;
avoided = true;
}
if (refPosition.Z < minZ + distance)
{
//desired = new Vector3d(velocity.X, maxSpeed, velocity.Z);
desired.Z = maxSpeed;
avoided = true;
}
else if (refPosition.Z >= maxZ - distance)
{
//desired = new Vector3d(velocity.X, -maxSpeed, velocity.Z);
desired.Z = -maxSpeed;
avoided = true;
}
if (avoided) return desired;
return Vector3d.Zero;
}