public MovementMeasure(Navigator owner, Vector3D?targetDirection = null)
{
this.owner = owner;
this.targetDirection = targetDirection;
myLogger = new Logger(owner.myGrid.DisplayName, "MovementMeasure");
lazy_rotationLengthSquared = new Lazy <double>(() => { return(pitch * pitch + yaw * yaw + roll * roll); });
lazy_currentWaypoint = new Lazy <Vector3D>(() => { return((Vector3D)owner.CNS.getWayDest()); });
lazy_movementSpeed = new Lazy <float>(() => { return(owner.myGrid.Physics.LinearVelocity.Length()); });
lazy_navBlockPosition = new Lazy <Vector3D>(() => { return(owner.getNavigationBlock().GetPosition()); });
lazy_displacementToPoint = new Lazy <RelativeVector3F>(() => { return(RelativeVector3F.createFromWorld(navBlockPos - currentWaypoint, owner.myGrid)); });
lazy_distToWayDest = new Lazy <double>(() => {
switch (owner.CNS.getTypeOfWayDest())
{
case NavSettings.TypeOfWayDest.BLOCK:
case NavSettings.TypeOfWayDest.GRID:
return(distToDestGrid);
default:
return(displacement.getWorld().Length());
}
});
lazy_distToDestGrid = new Lazy <double>(shortestDistanceToDestGrid);
}
/// <summary>
/// scales a movement vector by available thrust force
/// </summary>
/// <param name="displacement">displacement vector</param>
/// <param name="remote">controlling remote</param>
/// <returns>scaled vector</returns>
public RelativeVector3F scaleByForce(RelativeVector3F displacement, IMyCubeBlock remote)
{
Vector3 displacementGrid = displacement.getLocal();
// get force-determinant direction
// for each thrusting direction, compare needed thrust to max available
float minForce = float.MaxValue;
foreach (Base6Directions.Direction direction in Base6Directions.EnumDirections) //Enum.GetValues(typeof(Base6Directions.Direction)))
{
float movementInDirection = displacementGrid.Dot(Base6Directions.GetVector(direction));
if (movementInDirection > 0)
{
minForce = Math.Min(minForce, thrustProfile[direction]);
}
}
// scale thrust to min
Vector3 scaledMovement = Vector3.Zero;
foreach (Base6Directions.Direction direction in Base6Directions.EnumDirections) //Enum.GetValues(typeof(Base6Directions.Direction)))
{
float movementInDirection = displacementGrid.Dot(Base6Directions.GetVector(direction));
if (movementInDirection > 0)
{
float scaleFactor = minForce / thrustProfile[direction];
scaledMovement += movementInDirection * scaleFactor * Base6Directions.GetVector(direction);
}
}
return(RelativeVector3F.createFromLocal(scaledMovement, remote.CubeGrid));
}
/// <param name="centreDestination">where the centre of the grid will end up (local)</param>
private void createCapsule(Vector3 centreDestination, IMyCubeBlock navigationBlock)
{
float longestDistanceSquared = 0;
foreach (Vector3 rejection in rejectionCells)
{
float distanceSquared = (rejection - CentreRejection).LengthSquared();
if (distanceSquared > longestDistanceSquared)
{
longestDistanceSquared = distanceSquared;
}
}
Vector3D P0 = RelativeVector3F.createFromLocal(Centre, myCubeGrid).getWorldAbsolute();
//Vector3D P1 = RelativeVector3F.createFromLocal(centreDestination, myCubeGrid).getWorldAbsolute();
// need to extend capsule past destination by distance between remote and front of ship
Vector3 localPosition = navigationBlock.LocalPosition();
Ray navTowardsDest = new Ray(localPosition, DirectionNorm);
float tMin, tMax;
myCubeGrid.LocalVolume.IntersectRaySphere(navTowardsDest, out tMin, out tMax);
Vector3D P1 = RelativeVector3F.createFromLocal(centreDestination + tMax * DirectionNorm, myCubeGrid).getWorldAbsolute();
float CapsuleRadius = (float)(Math.Pow(longestDistanceSquared, 0.5) + 3 * myCubeGrid.GridSize);
myPath = new Capsule(P0, P1, CapsuleRadius);
}
/// <summary>
/// tries to read fly instruction of form (r), (u), (b)
/// </summary>
/// <param name="result"></param>
/// <param name="instruction"></param>
/// <returns>true iff successful</returns>
private bool flyOldStyle(out RelativeVector3F result, IMyCubeBlock remote, string instruction)
{
log("entered flyOldStyle(result, " + remote.DisplayNameText + ", " + instruction + ")", "flyTo_generic()", Logger.severity.TRACE);
result = null;
string[] coordsString = instruction.Split(',');
if (coordsString.Length != 3)
{
return(false);
}
double[] coordsDouble = new double[3];
for (int i = 0; i < coordsDouble.Length; i++)
{
if (!Double.TryParse(coordsString[i], out coordsDouble[i]))
{
return(false);
}
}
Vector3D fromBlock = new Vector3D(coordsDouble[0], coordsDouble[1], coordsDouble[2]);
result = RelativeVector3F.createFromBlock(fromBlock, remote);
return(true);
}
/// <summary>
/// Set the destination, calulate the profile.
/// </summary>
/// <param name="destination">waypoint or destination to fly to</param>
/// <param name="navigationBlock">usually remote control</param>
public void SetDestination(RelativeVector3F destination, IMyCubeBlock navigationBlock)
{
DirectionNorm = Vector3.Normalize(destination.getLocal() - navigationBlock.Position * myCubeGrid.GridSize);
Vector3 centreDestination = destination.getLocal() + Centre - navigationBlock.Position * myCubeGrid.GridSize;
//myLogger.debugLog("destination.getLocal() = " + destination.getLocal() + ", Centre = " + Centre + ", navigationBlock.Position * myCubeGrid.GridSize = " + navigationBlock.Position * myCubeGrid.GridSize, "SetDestination()");
//myLogger.debugLog("centreDestination = " + centreDestination + ", world = " + RelativeVector3F.createFromLocal(centreDestination, myCubeGrid).getWorldAbsolute(), "SetDestination()");
rejectAll();
createCapsule(centreDestination, navigationBlock);
}
private bool flyTo_generic(out RelativeVector3F result, IMyCubeBlock remote, string instruction)
{
log("entered flyTo_generic(result, " + remote.DisplayNameText + ", " + instruction + ")", "flyTo_generic()", Logger.severity.TRACE);
Vector3 fromGeneric;
if (getVector_fromGeneric(out fromGeneric, instruction))
{
result = RelativeVector3F.createFromBlock(fromGeneric, remote);
return(true);
}
result = null;
return(false);
}
private void buildPitchYaw()
{
isValid__pitchYaw = true;
Vector3D dirNorm;
if (targetDirection == null)
{
Vector3D displacement = currentWaypoint - owner.currentRCblock.GetPosition();
dirNorm = Vector3D.Normalize(displacement);
}
else
{
dirNorm = (Vector3D)targetDirection;
}
IMyCubeBlock NavBlock = owner.getNavigationBlock();
IMyCubeBlock RemBlock = owner.currentRCblock;
RelativeVector3F direction = RelativeVector3F.createFromWorld(dirNorm, owner.myGrid);
Vector3 navDirection = direction.getBlock(owner.getNavigationBlock());
//myLogger.debugLog("navDirection = " + navDirection, "buildPitchYaw()");
Vector3 NavRight = NavBlock.LocalMatrix.Right;
Vector3 RemFrNR = Base6Directions.GetVector(RemBlock.LocalMatrix.GetClosestDirection(ref NavRight));
Vector3 NavUp = NavBlock.LocalMatrix.Up;
Vector3 RemFrNU = Base6Directions.GetVector(RemBlock.LocalMatrix.GetClosestDirection(ref NavUp));
//Vector3 NavBack = NavBlock.LocalMatrix.Backward;
//Vector3 RemFrNB = Base6Directions.GetVector(RemBlock.LocalMatrix.GetClosestDirection(ref NavBack));
//myLogger.debugLog("NavRight = " + NavRight + ", NavUp = " + NavUp, "buildPitchYaw()");
//myLogger.debugLog("RemFrNR = " + RemFrNR + ", RemFrNU = " + RemFrNU, "buildPitchYaw()");
float right = navDirection.X, down = -navDirection.Y, forward = -navDirection.Z;
float pitch = (float)Math.Atan2(down, forward), yaw = (float)Math.Atan2(right, forward);
Vector3 mapped = pitch * RemFrNR + yaw * RemFrNU;
//myLogger.debugLog("mapped " + new Vector3(pitch, yaw, 0) + " to " + mapped, "buildPitchYaw()");
value__pitch = mapped.X;
value__yaw = mapped.Y;
value__roll = mapped.Z;
}
/// <summary>
/// Test the path for obstructions
/// </summary>
/// <exception cref="InterruptException">If interrupted</exception>
/// I considered keeping track of the closest entity, in the event there was no obstruction. This would have been, at best, unreliable due to initial AABB test.
public IMyEntity TestPath(Vector3D worldDestination, IMyCubeBlock navigationBlock, bool IgnoreAsteroids, out Vector3?pointOfObstruction, IMyCubeGrid DestGrid)
{
worldDestination.throwIfNull_argument("destination");
worldDestination.throwIfNull_argument("navigationBlock");
Interrupt = false;
this.NavigationBlock = navigationBlock;
this.IgnoreAsteroids = IgnoreAsteroids;
this.DestGrid = DestGrid;
myLogger.debugLog("Test path to (world absolute) " + worldDestination, "TestPath()");
//myLogger.debugLog("destination (local) = " + worldDestination.getLocal(), "TestPath()");
//myLogger.debugLog("destination (nav block) = " + worldDestination.getBlock(navigationBlock), "TestPath()");
Vector3D Displacement = worldDestination - navigationBlock.GetPosition();
myLogger.debugLog("Displacement = " + Displacement, "TestPath()");
// entities in large AABB
BoundingBoxD AtDest = myCubeGrid.WorldAABB.Translate(Displacement);
ICollection <IMyEntity> offenders = EntitiesInLargeAABB(myCubeGrid.WorldAABB, AtDest);
if (offenders.Count == 0)
{
myLogger.debugLog("AABB is empty", "TestPath()", Logger.severity.DEBUG);
pointOfObstruction = null;
return(null);
}
myLogger.debugLog("collected entities to test: " + offenders.Count, "TestPath()");
// sort offenders by distance
offenders = SortByDistance(offenders);
// set destination
GridShapeProfiler myGridShape = GridShapeProfiler.getFor(myCubeGrid);
//myLogger.debugLog("destination = " + worldDestination.getWorldAbsolute() + ", navigationBlock = " + navigationBlock.GetPosition(), "TestPath()");
myGridShape.SetDestination(RelativeVector3F.createFromWorldAbsolute(worldDestination, myCubeGrid), navigationBlock);
myPath = myGridShape.myPath;
myLogger.debugLog("got path from " + myPath.P0 + " to " + myPath.P1 + " with radius " + myPath.Radius, "TestPath()");
// test path
return(TestEntities(offenders, myPath, myGridShape, out pointOfObstruction, this.DestGrid));
}
/// <summary>
/// Finds a distance that will always be greater than the distance required to stop.
/// </summary>
/// <remarks>
/// <para>the approximation that is used by this method is: stopping distance = speed * speed / max acceleration</para>
/// <para>max acceleration is the smaller of available force / mass (exact) and speed / 2 (approximation)</para>
/// <para>where there is sufficient thrust, this could be simplified to stopping distance = 2 * speed</para>
/// </remarks>
/// <returns>A distance larger than the distance required to stop</returns>
public float getStoppingDistance()
{
RelativeVector3F velocity = RelativeVector3F.createFromWorld(myGrid.Physics.LinearVelocity, myGrid);
float maxStopDistance = 0;
foreach (Base6Directions.Direction direction in Base6Directions.EnumDirections) //Enum.GetValues(typeof(Base6Directions.Direction)))
{
float velocityInDirection = velocity.getLocal().Dot(Base6Directions.GetVector(direction));
if (velocityInDirection < -0.1) // direction is opposite of velocityGrid
{
float acceleration = Math.Min(Math.Abs(thrustProfile[direction] / myGrid.Physics.Mass), Math.Abs(velocityInDirection / 2));
float stoppingDistance = velocityInDirection * velocityInDirection / acceleration;
maxStopDistance = Math.Max(stoppingDistance, maxStopDistance);
}
}
return(maxStopDistance);
}
public PathCapsule(RelativeVector3F P0, RelativeVector3F P1, float RadiusSquared, float gridSize)
{
//BufferSize = 5;
//BufferSquared = BufferSize * BufferSize;
//float Radius = (float)(Math.Pow(RadiusSquared, 0.5));
//BufferedRadius = Radius + BufferSize;
//BufferedRadiusSquared = BufferedRadius * BufferedRadius;
this.gridSize = gridSize;
this.RadiusSquared = RadiusSquared;
this.Radius = (float)(Math.Pow(RadiusSquared, 0.5));
//this.P0 = P0;
//this.P1 = P1;
//this.line_local = new Line(P0.getLocal(), P1.getLocal(), false);
this.line_world = new Line(P0.getWorldAbsolute(), P1.getWorldAbsolute());
myLogger.debugLog("new path(world abs) from " + P0.getWorldAbsolute() + " to " + P1.getWorldAbsolute(), "Path()");
myLogger.debugLog("new path(local) from " + P0.getLocal() + " to " + P1.getLocal(), "Path()");
}
/// <param name="offenders">entities to test</param>
/// <param name="myGridShape">iff null, skip rejection test</param>
private IMyEntity TestEntities(ICollection <IMyEntity> offenders, Capsule path, GridShapeProfiler myGridShape, out Vector3?pointOfObstruction, IMyCubeGrid GridDestination)
{
foreach (IMyEntity entity in offenders)
{
CheckInterrupt();
myLogger.debugLog("testing offender: " + entity.getBestName() + " at " + entity.GetPosition(), "TestEntities()");
IMyCubeGrid asGrid = entity as IMyCubeGrid;
if (asGrid != null)
{
if (asGrid == GridDestination)
{
myLogger.debugLog("grid is destination: " + asGrid.DisplayName, "TestEntities()");
continue;
}
if (!path.IntersectsAABB(entity))
{
myLogger.debugLog("no AABB intersection: " + asGrid.DisplayName, "TestEntities()");
continue;
}
myLogger.debugLog("searching blocks of " + entity.getBestName(), "TestEntities()");
uint cellCount = 0, cellRejectedCount = 0;
// foreach block
float GridSize = asGrid.GridSize;
List <IMySlimBlock> allSlims = new List <IMySlimBlock>();
asGrid.GetBlocks_Safe(allSlims);
foreach (IMySlimBlock slim in allSlims)
{
bool blockIntersects = false;
Vector3 cellPosWorld = new Vector3();
//myLogger.debugLog("slim = " + slim.getBestName() + ", fat = " + slim.FatBlock + ", cell = " + slim.Position, "TestEntities()");
//if (slim.FatBlock != null)
//{
// myLogger.debugLog("fatblock min = " + slim.FatBlock.Min + ", fatblock max = " + slim.FatBlock.Max, "TestEntities()");
// //myLogger.debugLog("fatblock AABB min = " + slim.FatBlock.LocalAABB.Min + ", fatblock AABB max = " + slim.FatBlock.LocalAABB.Max, "TestEntities()");
//}
slim.ForEachCell((cell) => {
CheckInterrupt();
cellPosWorld = asGrid.GridIntegerToWorld(cell);
cellCount++;
//myLogger.debugLog("slim = " + slim.getBestName() + ", cell = " + cell + ", world position = " + cellPosWorld, "TestEntities()");
// intersects capsule
if (!path.Intersects(cellPosWorld, GridSize))
{
return(false);
}
// rejection
cellRejectedCount++;
if (myGridShape == null || myGridShape.rejectionIntersects(RelativeVector3F.createFromWorldAbsolute(cellPosWorld, myCubeGrid), myCubeGrid.GridSize))
{
myLogger.debugLog("obstructing grid = " + asGrid.DisplayName + ", cell = " + cellPosWorld + ", block = " + slim.getBestName(), "TestEntities()", Logger.severity.DEBUG);
blockIntersects = true;
return(true);
}
//myLogger.debugLog("rejection: no collision, cell = " + cellPosWorld + ", block = " + slim.getBestName(), "TestEntities()", Logger.severity.DEBUG);
return(false);
});
if (blockIntersects)
{
//myLogger.debugLog("closest point on line: {" + path.get_Line().From + ", " + path.get_Line().To + "} to " + cellPosWorld + " is " + path.get_Line().ClosestPoint(cellPosWorld), "TestEntities()");
pointOfObstruction = path.get_Line().ClosestPoint(cellPosWorld);
return(entity);
}
}
myLogger.debugLog("no obstruction for grid " + asGrid.DisplayName + ", tested " + cellCount + " against capsule and " + cellRejectedCount + " against rejection", "TestPath()");
continue;
}
// not a grid
if (IgnoreAsteroids && entity is IMyVoxelMap)
{
myLogger.debugLog("Ignoring asteroid: " + entity.getBestName(), "TestEntities()");
continue;
}
myLogger.debugLog("not a grid, testing bounds", "TestEntities()");
if (!path.IntersectsAABB(entity))
{
continue;
}
if (!path.IntersectsVolume(entity))
{
continue;
}
myLogger.debugLog("no more tests for non-grids are implemented", "TestEntities()", Logger.severity.DEBUG);
//myLogger.debugLog("closest point on line: {" + path.get_Line().From + ", " + path.get_Line().To + "} to " + entity.GetCentre() + " is " + path.get_Line().ClosestPoint(entity.GetCentre()), "TestEntities()");
pointOfObstruction = path.get_Line().ClosestPoint(entity.GetCentre());
return(entity);
}
myLogger.debugLog("no obstruction was found", "TestPath()", Logger.severity.DEBUG);
pointOfObstruction = null;
return(null);
}
/// <summary>
/// Test required elev/azim against min/max
/// Test line segment between turret and target for other entities
/// </summary>
private bool canLase(IMyEntity target)
{
Vector3D targetPos = target.GetPosition();
// Test elev/azim
Vector3 relativeToBlock = RelativeVector3F.createFromWorld(targetPos - turretPosition, myCubeBlock.CubeGrid).getBlock(myCubeBlock);
float azimuth, elevation;
Vector3.GetAzimuthAndElevation(Vector3.Normalize(relativeToBlock), out azimuth, out elevation);
//myLogger.debugLog("for target = " + target.getBestName() + ", at " + target.GetPosition() + ", elevation = " + elevation + ", azimuth = " + azimuth, "canLase()");
if (azimuth < minAzimuth || azimuth > maxAzimuth || elevation < minElevation || elevation > maxElevation)
{
return(false);
}
// if we are waiting on default targeting, ObstructingGrids will not be up-to-date
if (CurrentState == State.WAIT_DTAT)
{
// default targeting may acquire an unowned block on a friendly grid
IMyCubeGrid targetGrid = target as IMyCubeGrid;
if (targetGrid == null)
{
IMyCubeBlock targetAsBlock = target as IMyCubeBlock;
if (targetAsBlock != null)
{
targetGrid = targetAsBlock.CubeGrid;
}
}
List <IMyEntity> entitiesInRange = null;
BoundingSphereD range = new BoundingSphereD(turretPosition, myTurretBase.Range + 200);
MainLock.UsingShared(() => { entitiesInRange = MyAPIGateway.Entities.GetEntitiesInSphere(ref range); });
foreach (IMyEntity entity in entitiesInRange)
{
IMyCubeGrid asGrid = entity as IMyCubeGrid;
//if (asGrid != null)
// myLogger.debugLog("checking entity: " + entity.getBestName(), "canLase()");
if (asGrid != null)
{
// default targeting may acquire an unowned block on a friendly grid
if (targetGrid != null && targetGrid == asGrid)
{
continue;
}
if (!myCubeBlock.canConsiderHostile(asGrid) && IsObstructing(asGrid, targetPos))
{
myLogger.debugLog("for target " + target.getBestName() + ", path from " + turretPosition + " to " + targetPos + ", obstructing entity = " + entity.getBestName(), "canLase()");
return(false);
}
}
}
}
else
{
foreach (IMyCubeGrid grid in ObstructingGrids)
{
//if (grid != null)
// myLogger.debugLog("checking entity: " + grid.getBestName(), "canLase()");
if (IsObstructing(grid, targetPos))
{
myLogger.debugLog("for target " + target.getBestName() + ", path from " + turretPosition + " to " + targetPos + ", obstructing entity = " + grid.getBestName(), "canLase()");
return(false);
}
}
}
return(true);
}
/// <summary>
/// Rejection test for intersection with the profiled grid.
/// </summary>
/// <param name="position">position of potential obstruction</param>
/// <returns>true if the rejection collides with one or more of the grid's rejections</returns>
public bool rejectionIntersects(RelativeVector3F position, float GridSize)
{
return(rejectionIntersects(position.getLocal(), GridSize));
}
