//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); }