コード例 #1
0
            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);
                 */
            }
コード例 #2
0
        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"));
        }
コード例 #3
0
        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;
        }
コード例 #4
0
            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;
            }
コード例 #5
0
            /// <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();
                }
            }