void ShipDimensions(IMyShipController orientationBlock)//BoundingBox bb, double BlockMetricConversion)
{
if (thisProgram.Me.CubeGrid.GridSizeEnum.ToString().ToLower().Contains("small"))
{
gridsize = SMALL_BLOCK_LENGTH;
}
else
{
gridsize = LARGE_BLOCK_LENGTH;
}
_obbf = new OrientedBoundingBoxFaces(orientationBlock);
Vector3D[] points = new Vector3D[4];
_obbf.GetFaceCorners(OrientedBoundingBoxFaces.LookupFront, points); // 5 = front
// front output order is BL, BR, TL, TR
_width = (points[0] - points[1]).Length();
_height = (points[0] - points[2]).Length();
_obbf.GetFaceCorners(0, points);
// face 0=right output order is BL, TL, BR, TR ???
_length = (points[0] - points[2]).Length();
_length_blocks = (float)(_length / gridsize);
_width_blocks = (float)(_width / gridsize);
_height_blocks = (float)(_height / gridsize);
/*
* _length_blocks = bb.Size.GetDim(2) + 1;
* _width_blocks = bb.Size.GetDim(0) + 1;
* _height_blocks = bb.Size.GetDim(1) + 1;
* _block2metric = BlockMetricConversion;
* _length = Math.Round(_length_blocks * BlockMetricConversion, 2);
* _width = Math.Round(_width_blocks * BlockMetricConversion, 2);
* _height = Math.Round(_height_blocks * BlockMetricConversion, 2);
*/
}
public OrientedBoundingBoxFaces _obbf;// = OrientedBoundingBoxFaces(Me);
void calculateGridBBPosition(IMyTerminalBlock sourceBlock = null)
{
if (sourceBlock == null)
{
sourceBlock = shipOrientationBlock;
}
_obbf = new OrientedBoundingBoxFaces(sourceBlock);
}
string SensorInit(IMyTerminalBlock orientatioBlock, bool bSleep = false)
{
sensorsList.Clear();
sensorInfos.Clear();
List <IMyTerminalBlock> ltb = GetBlocksContains <IMySensorBlock>(sSensorUse);
OrientedBoundingBoxFaces orientedBoundingBox = new OrientedBoundingBoxFaces(shipOrientationBlock);
Vector3D vFTL;
Vector3D vFBL;
Vector3D vFTR;
Vector3D vFBR;
Vector3D vBTL;
Vector3D vBBL;
Vector3D vBTR;
Vector3D vBBR;
Vector3D[] points = new Vector3D[4];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupFront, points); // front output order is BL, BR, TL, TR
vFBL = points[0];
vFBR = points[1];
vFTL = points[2];
vFTR = points[3];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupBack, points); // face 4=back output order is BL, BR, TL, TR
vBBL = points[0];
vBBR = points[1];
vBTL = points[2];
vBTR = points[3];
foreach (var sb1 in ltb)
{
sensorsList.Add(sb1 as IMySensorBlock);
SensorInfo si = new SensorInfo();
si.EntityId = sb1.EntityId;
Vector3D vPos = sb1.GetPosition();
double distanceFront = PlanarDistance(vPos, vFBL, vFBR, vFTR);
// Echo("DistanceFront=" + distanceFront.ToString("0.00"));
// Vector3D vFront = vPos + sb1.WorldMatrix.Forward * distanceFront;
// debugGPSOutput("FRONT", vFront);
double distanceBack = PlanarDistance(vPos, vBBL, vBBR, vBTR);
// Echo("DistanceBack=" + distanceBack.ToString("0.00"));
// Vector3D vBack = vPos + sb1.WorldMatrix.Forward * distanceBack; // distance is negative for 'back'
// debugGPSOutput("BACK", vBack);
double distanceLeft = PlanarDistance(vPos, vFBL, vFTL, vBBL);
double distanceRight = PlanarDistance(vPos, vFBR, vFTR, vBBR);
double distanceUp = PlanarDistance(vPos, vFTL, vFTR, vBTL);
double distanceDown = PlanarDistance(vPos, vFBL, vFBR, vBBR);
si.DistanceFront = distanceFront;
si.DistanceBack = distanceBack;
si.DistanceLeft = distanceLeft;
si.DistanceRight = distanceRight;
si.DistanceUp = distanceUp;
si.DistanceDown = distanceDown;
sensorInfos.Add(si);
}
if (bSleep)
{
SensorsSleepAll();
}
return("S" + sensorsList.Count.ToString("00"));
}
void SensorSetToShip(IMyTerminalBlock tb1, float fLeft, float fRight, float fUp, float fDown, float fFront, float fBack)
{
// need to use world matrix to get orientation correctly
IMySensorBlock sb1 = tb1 as IMySensorBlock;
// Echo("SensorSetShip()");
int i1 = 0;
for (; i1 < sensorInfos.Count; i1++)
{
if (sensorInfos[i1].EntityId == sb1.EntityId)
{
break;
}
}
if (i1 < sensorInfos.Count)
{
// Echo("Using cached location information");
// we found cached info
float fSet = 0;
if (fLeft < 0)
{
fSet = -fLeft;
}
else
{
fSet = (float)Math.Abs(fLeft + Math.Abs(sensorInfos[i1].DistanceLeft));
}
sb1.LeftExtend = Math.Max(fSet, 1.0f);
if (fRight < 0)
{
fSet = -fRight;
}
else
{
fSet = (float)Math.Abs(fRight + Math.Abs(sensorInfos[i1].DistanceRight));
}
sb1.RightExtend = Math.Max(fSet, 1.0f);
if (fUp < 0)
{
fSet = -fUp;
}
else
{
fSet = (float)Math.Abs(fUp + Math.Abs(sensorInfos[i1].DistanceUp));
}
sb1.TopExtend = Math.Max(fSet, 1.0f);
if (fDown < 0)
{
fSet = -fDown;
}
else
{
fSet = (float)Math.Abs(fDown + Math.Abs(sensorInfos[i1].DistanceDown));
}
sb1.BottomExtend = Math.Max(fSet, 1.0f);
if (fFront < 0)
{
fSet = -fFront;
}
else
{
fSet = (float)Math.Abs(fFront + Math.Abs(sensorInfos[i1].DistanceFront));
}
sb1.FrontExtend = Math.Max(fSet, 1.0f);
if (fBack < 0)
{
fSet = -fBack;
}
else
{
fSet = (float)Math.Abs(fBack + Math.Abs(sensorInfos[i1].DistanceBack));
}
sb1.BackExtend = Math.Max(fSet, 1.0f);
}
else
{
OrientedBoundingBoxFaces orientedBoundingBox = new OrientedBoundingBoxFaces(shipOrientationBlock);
Vector3D vFTL;
Vector3D vFBL;
Vector3D vFTR;
Vector3D vFBR;
Vector3D vBTL;
Vector3D vBBL;
Vector3D vBTR;
Vector3D vBBR;
Vector3D[] points = new Vector3D[4];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupFront, points); // front output order is BL, BR, TL, TR
vFBL = points[0];
vFBR = points[1];
vFTL = points[2];
vFTR = points[3];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupBack, points); // face 4=back output order is BL, BR, TL, TR
vBBL = points[0];
vBBR = points[1];
vBTL = points[2];
vBTR = points[3];
debugGPSOutput("FBL", vFBL);
debugGPSOutput("FBR", vFBR);
debugGPSOutput("FTL", vFTL);
debugGPSOutput("FTR", vFTR);
debugGPSOutput("BBL", vBBL);
debugGPSOutput("BBR", vBBR);
debugGPSOutput("BTL", vBTL);
debugGPSOutput("BTR", vBTR);
if (sb1 == null)
{
return;
}
Echo(sb1.CustomName);
Vector3D vPos = sb1.GetPosition();
double distanceFront = PlanarDistance(vPos, vFBL, vFBR, vFTR);
Echo("DistanceFront=" + distanceFront.ToString("0.00"));
Vector3D vFront = vPos + sb1.WorldMatrix.Forward * distanceFront;
debugGPSOutput("FRONT", vFront);
double distanceBack = PlanarDistance(vPos, vBBL, vBBR, vBTR);
Echo("DistanceBack=" + distanceBack.ToString("0.00"));
Vector3D vBack = vPos + sb1.WorldMatrix.Forward * distanceBack; // distance is negative for 'back'
debugGPSOutput("BACK", vBack);
double distanceLeft = PlanarDistance(vPos, vFBL, vFTL, vBBL);
double distanceRight = PlanarDistance(vPos, vFBR, vFTR, vBBR);
double distanceUp = PlanarDistance(vPos, vFTL, vFTR, vBTL);
double distanceDown = PlanarDistance(vPos, vFBL, vFBR, vBBR);
float fSet = 0;
fSet = (float)Math.Abs(fLeft + Math.Abs(distanceLeft));
sb1.LeftExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fRight + Math.Abs(distanceRight));
sb1.RightExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fUp + Math.Abs(distanceUp));
sb1.TopExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fDown + Math.Abs(distanceDown));
sb1.BottomExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fFront + Math.Abs(distanceFront));
sb1.FrontExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fBack + Math.Abs(distanceBack));
sb1.BackExtend = Math.Max(fSet, 1.0f);
}
/*
* //x=width, y=height, z=back/forth. (fw=+z) (right=-y)
*
* float fScale = 2.5f;
* if (tb1.CubeGrid.GridSizeEnum == MyCubeSize.Small)
* {
* Echo("Small Grid Ship");
* fScale = 0.5f;
* }
* else Echo("Large Grid Ship");
* float fXOffset = sb1.Position.X * fScale; // small grid only?
* float fYOffset = sb1.Position.Y * fScale;
* float fZOffset = sb1.Position.Z * fScale;
* Echo("SB.x.y.z=" + fXOffset.ToString("0.0") + ":" + fYOffset.ToString("0.0") + ":" + fZOffset.ToString("0.0"));
*
* // Echo("MIN=" + Me.CubeGrid.Min.ToString() + "\nMAX:" + Me.CubeGrid.Max.ToString());
* // TODO: need to use grid orientation to main orientation block
* // BUG: Assumes orientation is SAME as main orientation block
* float fSet;
* fSet = (float)(shipDim.WidthInMeters() / 2 - fXOffset + fLeft);
* sb1.LeftExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.WidthInMeters() / 2 + fXOffset + fRight);
* sb1.RightExtend = Math.Max(fSet, 1.0f);
*
* fSet = (float)(shipDim.HeightInMeters() / 2 - fYOffset + fUp);
* sb1.TopExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.HeightInMeters() / 2 + fYOffset + fDown);
* sb1.BottomExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.LengthInMeters() + fZOffset + fFront);
* sb1.FrontExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.LengthInMeters() - fZOffset + fBack);
* sb1.BackExtend = Math.Max(fSet, 1.0f);
*/
sb1.Enabled = true;
}
OrientedBoundingBoxFaces _obbf;// = OrientedBoundingBoxFaces(Me);
void calculateGridBBPosition()
{
_obbf = new OrientedBoundingBoxFaces(gpsCenter);
}
float fMinSensorSetting = 0.1f; //was 1.0f before 1.193.100
public void SensorSetToShip(IMySensorBlock sb1, float fLeft, float fRight, float fUp, float fDown, float fFront, float fBack)
{
if (sb1 == null)
{
_program.ErrorLog("SetSensorToShip: Null sensor");
return;
}
// _program.ErrorLog("SensorSetShip("+sb1.CustomName+")");
int iSensor = 0;
for (; iSensor < sensorInfos.Count; iSensor++)
{
if (sensorInfos[iSensor].EntityId == sb1.EntityId)
{
break;
}
}
if (iSensor < sensorInfos.Count)
{
// Echo("Using cached location information");
// we found cached info
float fSet = 0;
if (fLeft < 0)
{
fSet = -fLeft;
}
else
{
fSet = (float)Math.Abs(fLeft + Math.Abs(sensorInfos[iSensor].DistanceLeft));
}
sb1.LeftExtend = Math.Max(fSet, fMinSensorSetting);
if (fRight < 0)
{
fSet = -fRight;
}
else
{
fSet = (float)Math.Abs(fRight + Math.Abs(sensorInfos[iSensor].DistanceRight));
}
sb1.RightExtend = Math.Max(fSet, fMinSensorSetting);
if (fUp < 0)
{
fSet = -fUp;
}
else
{
fSet = (float)Math.Abs(fUp + Math.Abs(sensorInfos[iSensor].DistanceUp));
}
sb1.TopExtend = Math.Max(fSet, fMinSensorSetting);
if (fDown < 0)
{
fSet = -fDown;
}
else
{
fSet = (float)Math.Abs(fDown + Math.Abs(sensorInfos[iSensor].DistanceDown));
}
sb1.BottomExtend = Math.Max(fSet, fMinSensorSetting);
if (fFront < 0)
{
fSet = -fFront;
}
else
{
fSet = (float)Math.Abs(fFront + Math.Abs(sensorInfos[iSensor].DistanceFront));
}
sb1.FrontExtend = Math.Max(fSet, fMinSensorSetting);
if (fBack < 0)
{
fSet = -fBack;
}
else
{
fSet = (float)Math.Abs(fBack + Math.Abs(sensorInfos[iSensor].DistanceBack));
}
sb1.BackExtend = Math.Max(fSet, fMinSensorSetting);
}
else
{
OrientedBoundingBoxFaces orientedBoundingBox = new OrientedBoundingBoxFaces(shipController);
Vector3D vFTL;
Vector3D vFBL;
Vector3D vFTR;
Vector3D vFBR;
Vector3D vBTL;
Vector3D vBBL;
Vector3D vBTR;
Vector3D vBBR;
Vector3D[] points = new Vector3D[4];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupFront, points); // front output order is BL, BR, TL, TR
vFBL = points[0];
vFBR = points[1];
vFTL = points[2];
vFTR = points[3];
// _program.ErrorLog("vFBL=" + vFBL.ToString());
// _program.ErrorLog("vFBR=" + vFBR.ToString());
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupBack, points); // face 4=back output order is BL, BR, TL, TR
vBBL = points[0];
vBBR = points[1];
vBTL = points[2];
vBTR = points[3];
// debugGPSOutput("FBL", vFBL);
// debugGPSOutput("FBR", vFBR);
// debugGPSOutput("FTL", vFTL);
// debugGPSOutput("FTR", vFTR);
// debugGPSOutput("BBL", vBBL);
// debugGPSOutput("BBR", vBBR);
// debugGPSOutput("BTL", vBTL);
// debugGPSOutput("BTR", vBTR);
// Echo(sb1.CustomName);
Vector3D vPos = sb1.GetPosition();
double distanceFront = PlanarDistance(vPos, vFBL, vFBR, vFTR);
// _program.ErrorLog("DistanceFront=" + distanceFront.ToString("0.00"));
Vector3D vFront = vPos + sb1.WorldMatrix.Forward * distanceFront;
// debugGPSOutput("FRONT", vFront);
double distanceBack = PlanarDistance(vPos, vBBL, vBBR, vBTR);
// _program.ErrorLog("DistanceBack=" + distanceBack.ToString("0.00"));
Vector3D vBack = vPos + sb1.WorldMatrix.Forward * distanceBack; // distance is negative for 'back'
// debugGPSOutput("BACK", vBack);
double distanceLeft = PlanarDistance(vPos, vFBL, vFTL, vBBL);
double distanceRight = PlanarDistance(vPos, vFBR, vFTR, vBBR);
double distanceUp = PlanarDistance(vPos, vFTL, vFTR, vBTL);
double distanceDown = PlanarDistance(vPos, vFBL, vFBR, vBBR);
float fSet = 0;
fSet = (float)Math.Abs(fLeft + Math.Abs(distanceLeft));
sb1.LeftExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fRight + Math.Abs(distanceRight));
sb1.RightExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fUp + Math.Abs(distanceUp));
sb1.TopExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fDown + Math.Abs(distanceDown));
sb1.BottomExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fFront + Math.Abs(distanceFront));
sb1.FrontExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fBack + Math.Abs(distanceBack));
sb1.BackExtend = Math.Max(fSet, fMinSensorSetting);
}
sb1.Enabled = true;
}
/// <summary>
/// Does travel movement with collision detection and avoidance. On arrival, changes state to arrivalState. If collision, changes to colDetectState
/// </summary>
/// <param name="vTargetLocation">Location of target</param>
/// <param name="arrivalDistance">minimum distance for 'arrival'</param>
/// <param name="arrivalState">state to use when 'arrived'</param>
/// <param name="colDetectState">state to use when 'collision'</param>
/// <param name="bAsteroidTarget">if True, target location is in/near an asteroid. don't collision detect with it</param>
public void doTravelMovement(Vector3D vTargetLocation, float arrivalDistance, int arrivalState, int colDetectState, bool bAsteroidTarget = false)
{
bool bArrived = false;
if (dTMDebug)
{
thisProgram.Echo("dTM:" + thisProgram.wicoControl.IState + "->" + arrivalState + "-C>" + colDetectState + " A:" + arrivalDistance);
// thisProgram.Echo("dTM:" + current_state + "->" + arrivalState + "-C>" + colDetectState + " A:" + arrivalDistance);
thisProgram.Echo("W=" + btmWheels.ToString() + " S=" + btmSled.ToString() + " R=" + btmRotor.ToString());
}
// Vector3D vTargetLocation = vHome;// shipOrientationBlock.GetPosition();
// shipOrientationBlock.CubeGrid.
if (tmShipController == null)
{
// first (for THIS target) time init
InitDoTravelMovement(vTargetLocation, thisProgram.wicoControl.fMaxWorldMps, thisProgram.wicoBlockMaster.GetMainController());
}
if (tmCameraElapsedMs >= 0)
{
tmCameraElapsedMs += thisProgram.Runtime.TimeSinceLastRun.TotalMilliseconds;
}
if (tmScanElapsedMs >= 0)
{
tmScanElapsedMs += thisProgram.Runtime.TimeSinceLastRun.TotalMilliseconds;
}
Vector3D vVec = vTargetLocation - tmShipController.CenterOfMass;
double distance = vVec.Length();
// TODO: Adjust targetlocation for gravity and min altitude.
// if (NAVGravityMinElevation > 0 && dGravity > 0)
if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0 && dGravity > 0)
{
// Need some trig here to calculate angle/distance to ground and target above ground
// Have: our altitude. Angle to target. Distance to target.
// iff target is 'below' us, then raise effective target to maintain min alt
// iff target is 'above' us.. then raise up to it..
// cheat for now.
if (distance < (arrivalDistance + thisProgram.wicoBlockMaster.DesiredMinTravelElevation))
{
bArrived = true;
}
}
if (dTMDebug)
{
thisProgram.Echo("dTM:distance=" + niceDoubleMeters(distance) + " (" + arrivalDistance.ToString() + ")");
thisProgram.Echo("dTM:velocity=" + velocityShip.ToString("0.00"));
thisProgram.Echo("dTM:tmMaxSpeed=" + tmMaxSpeed.ToString("0.00"));
}
if (distance < arrivalDistance)
{
bArrived = true;
}
if (bArrived)
{
ResetMotion(); // start the stopping
thisProgram.wicoControl.SetState(arrivalState); // current_state = arrivalState; // we have arrived
ResetTravelMovement(); // reset our brain so we re-calculate for the next time we're called
// TODO: Turn brakes ON (if not done in ResetMotion())
thisProgram.wicoControl.WantFast(); // bWantFast = true; // process this quickly
return;
}
// debugGPSOutput("TargetLocation", vTargetLocation);
List <IMySensorBlock> aSensors = null;
double stoppingDistance = 0;
if (!(btmWheels || btmRotor))
{
// if (dTMDebug) Echo("CalcStopD()");
stoppingDistance = thisProgram.wicoThrusters.calculateStoppingDistance(thrustTmBackwardList, velocityShip, 0);
}
// TODO: calculate stopping D for wheels
// Echo("dtmStoppingD=" + niceDoubleMeters(stoppingDistance));
if (thisProgram.wicoSensors.GetCount() > 0)
{
// float fScanDist = Math.Min(1f, (float)stoppingDistance * 1.5f);
float fScanDist = Math.Min(tmMaxSensorM, (float)stoppingDistance * 1.5f);
if (!dTMUseCameraCollision)
{
thisProgram.wicoSensors.SensorSetToShip(tmSB, 1, 1, 1, 1, fScanDist, 0);
}
else
{
thisProgram.wicoSensors.SensorSetToShip(tmSB, 0, 0, 0, 0, fScanDist, 0);
}
}
// else Echo("No Sensor for Travel movement");
bool bAimed = false;
Vector3D grav = tmShipController.GetNaturalGravity();
if (
btmSled ||
btmRotor
)
{
double yawangle = -999;
yawangle = CalculateYaw(vTargetLocation, tmShipController);
thisProgram.Echo("yawangle=" + yawangle.ToString());
if (btmSled)
{
thisProgram.Echo("Sled");
thisProgram.wicoGyros.DoRotate(yawangle, "Yaw");
}
else if (btmRotor)
{
thisProgram.Echo("Rotor");
thisProgram.wicoNavRotors.DoRotorRotate(yawangle);
}
bAimed = Math.Abs(yawangle) < .05;
}
else if (btmWheels && btmHasGyros)
{
thisProgram.Echo("Wheels W/ Gyro");
double yawangle = -999;
yawangle = CalculateYaw(vTargetLocation, shipOrientationBlock);
thisProgram.Echo("yawangle=" + yawangle.ToString());
bAimed = Math.Abs(yawangle) < .05;
if (!bAimed)
{
thisProgram.wicoWheels.WheelsPowerUp(0, 5);
// WheelsSetFriction(0);
thisProgram.wicoGyros.DoRotate(yawangle, "Yaw");
}
else
{
thisProgram.wicoWheels.WheelsSetFriction(50);
}
}
else if (btmWheels) // & ! btmHasGyro)
{
thisProgram.Echo("Wheels with no gyro...");
double yawangle = thisProgram.CalculateYaw(vTargetLocation, tmShipController);
thisProgram.Echo("yawangle=" + yawangle.ToString());
bAimed = Math.Abs(yawangle) < .05;
if (!bAimed)
{
// TODO: rotate with wheels..
}
else
{
thisProgram.wicoWheels.WheelsSetFriction(50);
}
}
else
{
if (grav.Length() > 0)
{ // in gravity. try to stay aligned to gravity, but change yaw to aim at location.
bool bGravAligned = GyroMain("", grav, shipOrientationBlock);
// if (bGravAligned)
{
double yawangle = CalculateYaw(vTargetLocation, shipOrientationBlock);
thisProgram.wicoGyros.DoRotate(yawangle, "Yaw");
bAimed = Math.Abs(yawangle) < .05;
}
}
else
{
bAimed = GyroMain("forward", vVec, shipOrientationBlock);
}
}
tmShipController.DampenersOverride = true;
if ((distance - stoppingDistance) < arrivalDistance)
{ // we are within stopping distance, so start slowing
thisProgram.wicoGyros.SetMinAngle(0.0005f); // minAngleRad = 0.005f;// aim tighter (next time)
// StatusLog("\"Arriving at target. Slowing", textPanelReport);
thisProgram.Echo("Waiting for stop");
if (!bAimed)
{
thisProgram.wicoControl.WantFast(); // bWantFast = true;
}
ResetMotion();
return;
}
if (bAimed)
{
thisProgram.wicoControl.WantMedium(); // bWantMedium = true;
// we are aimed at location
thisProgram.Echo("Aimed");
thisProgram.wicoGyros.gyrosOff();
if (btmWheels)
{
thisProgram.wicoWheels.WheelsSetFriction(50);
}
if (
dTMUseSensorCollision &&
(tmScanElapsedMs > dSensorSettleWaitMS || tmScanElapsedMs < 0)
)
{
tmScanElapsedMs = 0;
aSensors = thisProgram.wicoSensors.SensorsGetActive();
if (aSensors.Count > 0)
{
var entities = new List <MyDetectedEntityInfo>();
string s = "";
for (int i1 = 0; i1 < aSensors.Count; i1++) // we only use one sensor
{
aSensors[i1].DetectedEntities(entities);
int j1 = 0;
bool bValidCollision = false;
if (entities.Count > 0)
{
bValidCollision = true;
}
for (; j1 < entities.Count; j1++)
{
s = "\nSensor TRIGGER!";
s += "\nName: " + entities[j1].Name;
s += "\nType: " + entities[j1].Type;
s += "\nRelationship: " + entities[j1].Relationship;
s += "\n";
if (dTMDebug)
{
thisProgram.Echo(s);
// StatusLog(s, textLongStatus);
}
if (entities[j1].Type == MyDetectedEntityType.Planet)
{
bValidCollision = false;
}
if (entities[j1].Type == MyDetectedEntityType.LargeGrid ||
entities[j1].Type == MyDetectedEntityType.SmallGrid
)
{
if (entities[j1].BoundingBox.Contains(vTargetLocation) != ContainmentType.Disjoint)
{
if (dTMDebug)
{
thisProgram.Echo("Ignoring collision because we want to be INSIDE");
}
// if the target is inside the BB of the target, ignore the collision
bValidCollision = false;
}
}
if (bValidCollision)
{
break;
}
}
if (bValidCollision)
{
// something in way.
// save what we detected
lastDetectedInfo = entities[j1];
ResetTravelMovement();
thisProgram.wicoControl.SetState(colDetectState); // current_state = colDetectState; // set the collision detetected state
bCollisionWasSensor = true;
thisProgram.wicoControl.WantFast(); // bWantFast = true; // process next state quickly
ResetMotion(); // start stopping
return;
}
}
}
else
{
lastDetectedInfo = new MyDetectedEntityInfo(); // since we found nothing, clear it.
}
}
double scanDistance = stoppingDistance * 2;
// double scanDistance = stoppingDistance * 1.05;
// if (bAsteroidTarget) scanDistance *= 2;
// if (btmRotor || btmSled)
{
if (scanDistance < 100)
{
if (distance < 500)
{
scanDistance = distance;
}
else
{
scanDistance = 500;
}
}
scanDistance = Math.Min(distance, scanDistance);
}
// if (dTMDebug)
if (dTMUseCameraCollision)
{
// Echo("Scanning distance=" + niceDoubleMeters(scanDistance));
}
if (
dTMUseCameraCollision &&
(tmCameraElapsedMs > tmCameraWaitMs || tmCameraElapsedMs < 0) && // it is time to scan..
distance > tmMaxSensorM // if we are in sensor range, we don't need to scan with cameras
// && !bAsteroidTarget
)
{
OrientedBoundingBoxFaces orientedBoundingBox = new OrientedBoundingBoxFaces(shipOrientationBlock);
Vector3D[] points = new Vector3D[4];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupFront, points); // front output order is BL, BR, TL, TR
// assumes moving forward...
// May 29, 2018 do a BB forward scan instead of just center..
bool bDidScan = false;
Vector3D vTarget;
switch (dtmRayCastQuadrant)
{
case 0:
if (thisProgram.wicoCameras.CameraForwardScan(scanDistance))
{
bDidScan = true;
}
break;
case 1:
vTarget = points[2] + tmShipController.WorldMatrix.Forward * distance;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
case 2:
vTarget = points[3] + tmShipController.WorldMatrix.Forward * distance;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
case 3:
vTarget = points[0] + tmShipController.WorldMatrix.Forward * distance;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
case 4:
vTarget = points[1] + tmShipController.WorldMatrix.Forward * distance;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
case 5:
// check center again. always full length
if (thisProgram.wicoCameras.CameraForwardScan(scanDistance))
{
bDidScan = true;
}
break;
case 6:
vTarget = points[2] + tmShipController.WorldMatrix.Forward * distance / 2;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
case 7:
vTarget = points[3] + tmShipController.WorldMatrix.Forward * distance / 2;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
case 8:
vTarget = points[0] + tmShipController.WorldMatrix.Forward * distance / 2;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
case 9:
vTarget = points[1] + tmShipController.WorldMatrix.Forward * distance / 2;
if (thisProgram.wicoCameras.CameraForwardScan(vTarget))
{
bDidScan = true;
}
break;
}
if (bDidScan)
{
dtmRayCastQuadrant++;
if (dtmRayCastQuadrant > 9)
{
dtmRayCastQuadrant = 0;
}
tmCameraElapsedMs = 0;
// the routine sets lastDetetedInfo itself if scan succeeds
if (!lastDetectedInfo.IsEmpty())
{
bool bValidCollision = true;
// assume it MIGHT be asteroid and check
// if (bAsteroidTarget)
{
if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid)
{
if (lastDetectedInfo.BoundingBox.Contains(vTargetLocation) != ContainmentType.Disjoint)
{ // if the target is inside the BB of the target, ignore the collision
bValidCollision = false;
// check to see if we are close enough to surface of asteroid
double astDistance = ((Vector3D)lastDetectedInfo.HitPosition - shipOrientationBlock.GetPosition()).Length();
if ((astDistance - stoppingDistance) < arrivalDistance)
{
ResetMotion();
thisProgram.wicoControl.SetState(arrivalState);// current_state = arrivalState;
ResetTravelMovement();
// don't need 'fast'...
return;
}
}
}
else if (lastDetectedInfo.Type == MyDetectedEntityType.Planet)
{
// ignore
bValidCollision = false;
}
else
{
}
}
if (dTMDebug)
{
// Echo(s);
thisProgram.Echo("raycast hit:" + lastDetectedInfo.Type.ToString());
StatusLog("Camera Trigger collision", textPanelReport);
}
if (bValidCollision)
{
// sInitResults += "Camera collision: " + scanDistance + "\n" + lastDetectedInfo.Name + ":" + lastDetectedInfo.Type + "\n";
// something in way.
ResetTravelMovement(); // reset our brain for next call
thisProgram.wicoControl.SetState(colDetectState); // current_state = colDetectState; // set the detetected state
bCollisionWasSensor = false;
thisProgram.wicoControl.WantFast(); // bWantFast = true; // process next state quickly
ResetMotion(); // start stopping
return;
}
}
else
{
if (dTMDebug)
{
// Echo(s);
// StatusLog("Camera Scan Clear", textPanelReport);
}
}
}
else
{
if (dTMDebug)
{
// Echo(s);
// StatusLog("No Scan Available", textPanelReport);
}
}
}
else
{
thisProgram.Echo("Raycast delay");
}
if (dTMDebug)
{
thisProgram.Echo("dtmFar=" + niceDoubleMeters(dtmFar));
}
if (dTMDebug)
{
thisProgram.Echo("dtmApproach=" + niceDoubleMeters(dtmApproach));
}
if (dTMDebug)
{
thisProgram.Echo("dtmPrecision=" + niceDoubleMeters(dtmPrecision));
}
if (distance > dtmFar && !btmApproach)
{
// we are 'far' from target location. use fastest movement
// if(dTMDebug)
thisProgram.Echo("dtmFar. Target Vel=" + dtmFarSpeed.ToString("N0"));
// StatusLog("\"Far\" from target\n Target Speed=" + dtmFarSpeed.ToString("N0") + "m/s", textPanelReport);
TmDoForward(dtmFarSpeed, 100f);
}
else if (distance > dtmApproach && !btmPrecision)
{
// we are on 'approach' to target location. use a good speed
// if(dTMDebug)
thisProgram.Echo("Approach. Target Vel=" + dtmApproachSpeed.ToString("N0"));
// StatusLog("\"Approach\" distance from target\n Target Speed=" + dtmApproachSpeed.ToString("N0") + "m/s", textPanelReport);
btmApproach = true;
TmDoForward(dtmApproachSpeed, 100f);
}
else if (distance > dtmPrecision && !btmClose)
{
// we are getting nearto our target. use a slower speed
// if(dTMDebug)
thisProgram.Echo("Precision. Target Vel=" + dtmPrecisionSpeed.ToString("N0"));
// StatusLog("\"Precision\" distance from target\n Target Speed=" + dtmPrecisionSpeed.ToString("N0") + "m/s", textPanelReport);
if (!btmPrecision)
{
thisProgram.wicoGyros.SetMinAngle(0.005f); // minAngleRad = 0.005f;// aim tighter (next time)
}
btmPrecision = true;
TmDoForward(dtmPrecisionSpeed, 100f);
}
else
{
// we are very close to our target. use a very small speed
// if(dTMDebug)
thisProgram.Echo("Close. Target Speed=" + dtmCloseSpeed.ToString("N0") + "m/s");
// StatusLog("\"Close\" distance from target\n Target Speed=" + dtmCloseSpeed.ToString("N0") + "m/s", textPanelReport);
if (!btmClose)
{
thisProgram.wicoGyros.SetMinAngle(0.005f); //minAngleRad = 0.005f;// aim tighter (next time)
}
btmClose = true;
TmDoForward(dtmCloseSpeed, 100f);
}
}
else
{
// StatusLog("Aiming at target", textPanelReport);
if (dTMDebug)
{
thisProgram.Echo("Aiming");
}
thisProgram.wicoControl.WantFast();// bWantFast = true;
tmShipController.DampenersOverride = true;
if (velocityShip < 5)
{
// we are probably doing precision maneuvers. Turn on all thrusters to avoid floating past target
thisProgram.wicoThrusters.powerDownThrusters();
}
else
{
thisProgram.wicoThrusters.powerDownThrusters(thrustTmBackwardList, WicoThrusters.thrustAll, true); // coast
}
// sleepAllSensors();
}
}
