public Vector AvoidCollisions(Vector IntendedVelocity) { //store the target that collides then, and other stuff //that we will need and can avoid recalculating //store the first collision time float ShortestTime = float.NegativeInfinity; Unit FirstTarget = null; float FirstMinSeperation = 0; float FirstDistance = 0; Vector FirstRelativePos = new Vector(); Vector FirstRelativeVel = new Vector(); var UnitsInRange = GetAllUnitsInAvoidanceRange(); float Distance = 0; Vector RelativePos = new Vector(); float CollisionRadiusToAvoid = 0F; foreach (var unit in UnitsInRange) { //calc min coll. range float CollisionRadius = (float)this.Radius + (float)unit.Radius; //Calculate time to collision RelativePos = unit.Location.ToVector().Subtract(this.Location.ToVector()); Distance = RelativePos.Len(); Vector RelativeVel = unit.Velocity.Subtract(this.Velocity); //Vector RelativeVel = unit.Velocity.Subtract(IntendedVelocity); float RelativeSpeed = RelativeVel.Mag(); float TimeToCollision = VectorOps.dot(RelativePos, RelativeVel) / (RelativeSpeed * RelativeSpeed); TimeToCollision = -TimeToCollision; if (TimeToCollision == double.NaN || TimeToCollision > 10 || TimeToCollision < 0) { continue; } if (ShortestTime < TimeToCollision) { ShortestTime = TimeToCollision; } //Check if it is going to be a collision at all //var MyPositionAtCollision = (this.Location.World.ToVector()).Add2(this.Velocity.Mult2(TimeToCollision)); var MyPositionAtCollision = (this.Location.World.ToVector()).Add2(IntendedVelocity.Mult2(TimeToCollision)); var HisPositionAtCollision = (unit.Location.World.ToVector()).Add2(unit.Velocity.Mult2(TimeToCollision)); float MinSeperation = (MyPositionAtCollision.Subtract(HisPositionAtCollision)).Len(); //float MinSeperation = Distance - RelativeSpeed * ShortestTime; if (MinSeperation > CollisionRadius) { continue; //skip to the next iteration } if (TimeToCollision > 0 && TimeToCollision <= ShortestTime) { ShortestTime = TimeToCollision; FirstTarget = unit; FirstMinSeperation = MinSeperation; FirstDistance = Distance; FirstRelativePos = RelativePos; FirstRelativeVel = RelativeVel; CollisionRadiusToAvoid = CollisionRadius; } } if (FirstTarget == null) { return(new Vector()); //if we have no target, exit } //if we're going to hit, or if we're hitting already, then do steering based on current pos if (FirstMinSeperation <= 0 || Distance < CollisionRadiusToAvoid) { RelativePos = FirstTarget.Location.ToVector().Subtract(this.Location.ToVector()); } else { Vector vMultipliedFirstRelativeVel = FirstRelativeVel.Mult2(ShortestTime); RelativePos = FirstRelativePos.Add2(vMultipliedFirstRelativeVel); } RelativePos.Normalize(); RelativePos.Mult(MaxSpeed); RelativePos.Y = -RelativePos.Y; RelativePos.X = -RelativePos.X; //float dot = VectorOps.dot(RelativePos.Normalize2(), Velocity.Normalize2()); //if (dot >= 0.01 || dot <= -0.01) //means our velocity is close to perpendicular to collision line ////if (true) //{ // //todo: consider the special case of midVector2 // //otherwise the target is to my right // RelativePos = this.Location.World.ToVector().Add2(GetNormal(Velocity.ToLine())); //} return(RelativePos); //apply steering }