//public CanFlyTo(IMyCubeGrid grid, BoundingSphereD toCheck)
//{
// this.myGrid = grid;
// myLogger = new Logger(grid.DisplayName, "CanFlyTo");
// collisionSpheres = new Spheres(toCheck);
//}
/// <summary>
///
/// </summary>
/// <param name="result"></param>
/// <returns>false iff could not get a result</returns>
public bool next(out collisionAvoidResult result)
{
if (!isValid)
{
result = collisionAvoidResult.NO_WAY_FORWARD;
myLogger.log(Logger.severity.ERROR, "next()", "cannot proceed, not valid");
return(false);
}
isValid = false;
blocksChecked = 0;
BoundingSphereD currentSphere;
while (collisionSpheres.next(out currentSphere))
{
//log("got sphere: "+currentSphere);
if (spheresChecked >= maxSpheres)
{
isValid = true;
result = collisionAvoidResult.NOT_FINISHED;
return(true);
}
spheresChecked++;
relevantSphere = currentSphere;
List <IMyEntity> entitiesInSphere = MyAPIGateway.Entities.GetEntitiesInSphere(ref currentSphere);
if (entitiesInSphere.Count > 0)
{
if (gridDest != null && entitiesInSphere.Contains(gridDest as IMyEntity))
{
result = collisionAvoidResult.NO_OBSTRUCTION;
log(myLogger, "sphere contains grid dest", "next()", Logger.severity.DEBUG);
return(true);
}
else // does not contain gridDest
{
foreach (IMyEntity entity in entitiesInSphere)
{
if (!ignoreCollision(entity, currentSphere))
{
log(myLogger, "obstruction: " + getEntityName(entity));
result = collisionAvoidResult.OBSTRUCTION;
return(true);
}
else if (expensiveTest)
{ // intersection test is expensive, delay
log(myLogger, "performed expensiveTest, delaying next sphere");
isValid = true;
result = collisionAvoidResult.NOT_FINISHED;
return(true);
}
}
}
}
}
// completed successfully with no obstacles
result = collisionAvoidResult.NO_OBSTRUCTION;
return(true);
}
public bool next(out collisionAvoidResult result)
{
if (currentStage == stage.S100_done)
{
result = collisionAvoidResult.NO_WAY_FORWARD;
//log("cannot proceed, stage == done");
return(false);
}
spheresChecked = 0;
//log("started next(result) stage: "+currentStage);
if (currentStage == stage.S0_start)
{
//log("building new CanFlyTo("+wayDest+", "+gridDestination+", "+remoteControl+", "+collisionLength+", "+stopFromDestGrid+")");
currentCFT = new CanFlyTo(wayDest, destGrid, myGridDims, false, ignoreAsteroids); //new CanFlyTo(wayDest, gridDestination, remoteControl, straightRadius, 0, distance_from_RC_to_front);
advanceStage();
}
if (currentStage == stage.S1_straight)
{
if (currentCFT.next(out result))
{
switch (result)
{
case collisionAvoidResult.OBSTRUCTION:
// find new path
collisionSphere = currentCFT.relevantSphere.Center;
advanceStage();
break;
case collisionAvoidResult.NOT_FINISHED:
return(true);
case collisionAvoidResult.NO_OBSTRUCTION:
currentStage = stage.S100_done;
log(myLogger, "OK to fly straight ahead");
return(true);
default:
myLogger.log(Logger.severity.ERROR, "next()", "Error: unsuitable case from canFlyTo.next(): " + result);
result = collisionAvoidResult.NO_WAY_FORWARD;
return(false);
}
}
else
{
myLogger.log(Logger.severity.ERROR, "next()", "Error at Collision.Avoidance.next()." + currentStage + ": CanFlyTo.next() is invalid");
currentStage = stage.S100_done;
return(false);
}
}
log(myLogger, "searching for alternate route, spheres so far: " + spheresChecked);
if (currentStage == stage.S2_setupAlter)
{
routeVector = wayDest - myGridDims.getRCworld();
altRoute = new Vector3D[4];
altRoute[0] = Vector3D.CalculatePerpendicularVector(routeVector);
altRoute[1] = routeVector.Cross(altRoute[0]);
altRoute[2] = Vector3D.Negate(altRoute[0]);
altRoute[3] = Vector3D.Negate(altRoute[1]);
setupAltRoute = true;
multiplier = 10;
advanceStage();
}
while (multiplier < 2000)
{
if (setupAltRoute)
{
//log("setupAtRoute: "+currentStage+", spheres so far: "+spheresChecked);
Vector3D direction;
switch (currentStage)
{
case stage.S30_checkAlt0:
direction = altRoute[0];
break;
case stage.S31_checkAlt1:
direction = altRoute[1];
break;
case stage.S32_checkAlt2:
direction = altRoute[2];
break;
case stage.S33_checkAlt3:
direction = altRoute[3];
break;
default:
direction = new Vector3D();
break;
}
waypoint = collisionSphere + multiplier * Vector3D.Normalize(direction);
double distance = myGridDims.myGrid.WorldAABB.Distance(waypoint);
if (distance < CNS.destinationRadius)
{
log(myLogger, "throwing out waypoint, too close", "next()", Logger.severity.DEBUG);
advanceStage();
continue;
}
currentCFT = new CanFlyTo(waypoint, destGrid, myGridDims, true, ignoreAsteroids);
//currentCFT = new CanFlyTo(waypoint, gridDestination, remoteControl, alternateRadius, alternateProjection, distance_from_RC_to_front);
setupAltRoute = false;
}
if (!currentCFT.next(out result))
{
myLogger.log(Logger.severity.ERROR, "next()", "Error at next()." + currentStage + ": CanFlyTo.next() is invalid");
currentStage = stage.S100_done;
return(false);
}
//log("checking result from canFlyTo: "+result);
switch (result)
{
case collisionAvoidResult.OBSTRUCTION:
setupAltRoute = true;
//string toLog = "at obstruction, stage "+currentStage;
advanceStage();
//log(toLog + " => " + currentStage);
log(myLogger, "obstruction in this alt path", "next()", Logger.severity.DEBUG);
break;
case collisionAvoidResult.NOT_FINISHED:
log(myLogger, "not finished stage: " + currentStage + ", spheres: " + spheresChecked);
return(true);
case collisionAvoidResult.NO_OBSTRUCTION:
if (CNS.setWaypoint(waypoint, true))
{
log(myLogger, "added new waypoint " + waypoint, "next()", Logger.severity.DEBUG);
result = collisionAvoidResult.ALTERNATE_PATH;
currentStage = stage.S100_done;
return(true);
}
else
{
goto case collisionAvoidResult.OBSTRUCTION;
}
default:
myLogger.log(Logger.severity.ERROR, "next()", "Error: unsuitable case from canFlyTo.next(): " + result);
result = collisionAvoidResult.NO_WAY_FORWARD;
return(false);
}
}
//log("Warning: no way forward");// + ", checked " + spheresChecked + " spheres");
result = collisionAvoidResult.NO_WAY_FORWARD;
return(true);
}
