示例#1
0
        /// <summary>
        /// Try to face in the target direction. Must execute on game thread.
        /// </summary>
        public void FaceTowards(DirectionWorld target)
        {
            Log.DebugLog("Not server!", Logger.severity.FATAL, condition: !MyAPIGateway.Multiplayer.IsServer);
            Debug.Assert(Threading.ThreadTracker.IsGameThread, "not game thread");

            if (!m_claimedElevation && !m_claimedAzimuth)
            {
                // nothing to do
                return;
            }

            FaceResult bestResult;

            if (!CalcFaceTowards(target, out bestResult))
            {
                Stop();
                return;
            }

            Log.TraceLog("Face: " + target + ", elevation: " + bestResult.DeltaElevation + ", azimuth: " + bestResult.DeltaAzimuth);

            if (m_claimedElevation)
            {
                SetVelocity(StatorEl, bestResult.DeltaElevation);
            }
            if (m_claimedAzimuth)
            {
                SetVelocity(StatorAz, bestResult.DeltaAzimuth);
            }
        }
示例#2
0
        private bool CalcFaceTowards_NotClaimed(ref FaceResult bestResult, DirectionWorld target, DirectionWorld faceDirection, float azimuthDelta)
        {
            if (Math.Abs(azimuthDelta) > m_requiredAccuracyRadians)
            {
                return(false);
            }

            float elevationDelta, elevationAccuracy;

            if (CalcElevation(target, faceDirection, azimuthDelta, out elevationDelta, out elevationAccuracy))
            {
                float accSq = azimuthDelta * azimuthDelta + elevationAccuracy * elevationAccuracy;
                if (accSq > m_requiredAccuracyRadians * m_requiredAccuracyRadians)
                {
                    return(false);
                }
                float sumDeltaMag = Math.Abs(elevationDelta);
                Log.TraceLog("Best: " + bestResult + ", current: " + new FaceResult()
                {
                    AccuracySquared = accSq, SumDeltaMag = sumDeltaMag, DeltaElevation = elevationDelta, DeltaAzimuth = 0f
                });
                if (bestResult.ReplaceBy(accSq, sumDeltaMag))
                {
                    bestResult.AccuracySquared = accSq;
                    bestResult.SumDeltaMag     = sumDeltaMag;
                    bestResult.DeltaAzimuth    = 0f;
                    bestResult.DeltaElevation  = elevationDelta;
                }
                return(true);
            }
            return(false);
        }
示例#3
0
        private bool CalcElevation(DirectionWorld targetWorldSpace, DirectionWorld faceDirection, float azimuthDelta, out float elevationDelta, out float elevationAccuracy)
        {
            if (StatorOk(StatorAz) && m_claimedAzimuth)
            {
                ApplyAzimuthChange(ref faceDirection, azimuthDelta);
            }
            Vector3 current = faceDirection.ToBlock(StatorEl);
            Vector3 target  = targetWorldSpace.ToBlock(StatorEl);

            Log.TraceLog(nameof(targetWorldSpace) + ": " + targetWorldSpace + ", " + nameof(faceDirection) + ": " + faceDirection + ", " + nameof(target) + ": " + target + ", " + nameof(current) + ": " + current);

            float firstDelta, secondDelta;

            CalcDelta(current, target, out firstDelta, out secondDelta);

            if (m_claimedElevation)
            {
                // elevation has been claimed, check limits
                if (WithinLimits(StatorEl, firstDelta))
                {
                    Log.TraceLog("First elevation delta is reachable: " + firstDelta);
                    elevationDelta    = firstDelta;
                    elevationAccuracy = 0f;
                    return(true);
                }
                if (WithinLimits(StatorEl, secondDelta))
                {
                    Log.TraceLog("Second elevation delta is reachable: " + secondDelta);
                    elevationDelta    = secondDelta;
                    elevationAccuracy = 0f;
                    return(true);
                }
                BestEffort(StatorEl, firstDelta, secondDelta, out elevationDelta, out elevationAccuracy);
                if (elevationAccuracy < m_requiredAccuracyRadians)
                {
                    Log.TraceLog("Best effort: " + elevationDelta);
                    return(true);
                }
            }
            else
            {
                // elevation not claimed, check that the current elevation is close enough
                elevationAccuracy = Math.Abs(firstDelta);
                if (elevationAccuracy < m_requiredAccuracyRadians)
                {
                    Log.TraceLog("Elevation within tolerance: " + firstDelta);
                    elevationDelta = 0f;                     // not claimed, no rotation
                    return(true);
                }
            }

            Log.TraceLog("Neither elevation delta acceptable");
            elevationDelta    = float.NaN;
            elevationAccuracy = float.PositiveInfinity;
            return(false);
        }
示例#4
0
 private bool CalcFaceTowards(DirectionWorld targetWorldSpace, out FaceResult bestResult)
 {
     if (StatorOk(StatorAz))
     {
         return(CalcFaceTowards_AzimuthOk(targetWorldSpace, out bestResult));
     }
     else
     {
         return(CalcFaceTowards_NoAzimuth(targetWorldSpace, out bestResult));
     }
 }
示例#5
0
        /// <summary>
        /// Determine if the motor turret can face the target direction. Thread-safe.
        /// </summary>
        /// <param name="target">The direction to face.</param>
        /// <returns>True iff the turret can face the target direction.</returns>
        public bool CanFaceTowards(DirectionWorld target)
        {
            if (!StatorOk(StatorEl))
            {
                return(false);
            }

            FaceResult bestResult;

            return(CalcFaceTowards(target, out bestResult));
        }
示例#6
0
        /// <summary>
        /// Rotate current direction by azimuth stator's delta.
        /// </summary>
        /// <param name="facing">The current direction.</param>
        /// <param name="azDelta">The change in angle of azimuth stator.</param>
        private void ApplyAzimuthChange(ref DirectionWorld facing, float azDelta)
        {
            Log.TraceLog(nameof(facing) + " is " + facing);
            Vector3 axis = StatorAz.WorldMatrix.Down;

            Log.TraceLog(nameof(axis) + " is " + axis);
            Log.TraceLog(nameof(azDelta) + " is " + azDelta);
            Quaternion rotation; Quaternion.CreateFromAxisAngle(ref axis, azDelta, out rotation);

            Vector3.Transform(ref facing.vector, ref rotation, out facing.vector);
            Log.TraceLog(nameof(facing) + " is " + facing);
        }
示例#7
0
        private bool CalcFaceTowards_Claimed(ref FaceResult bestResult, DirectionWorld target, DirectionWorld faceDirection, float azimuthDelta)
        {
            float azimuthAccuracy;

            if (WithinLimits(StatorAz, azimuthDelta))
            {
                azimuthAccuracy = 0f;
            }
            else
            {
                float clamped = ClampToLimits(StatorAz, azimuthDelta);
                azimuthAccuracy = Math.Abs(azimuthDelta - clamped);
                if (azimuthAccuracy > m_requiredAccuracyRadians)
                {
                    return(false);
                }
                azimuthDelta = clamped;
            }

            float elevationDelta, elevationAccuracy;

            if (CalcElevation(target, faceDirection, azimuthDelta, out elevationDelta, out elevationAccuracy))
            {
                float accSq       = azimuthAccuracy * azimuthAccuracy + elevationAccuracy * elevationAccuracy;
                float sumDeltaMag = Math.Abs(azimuthDelta) + Math.Abs(elevationDelta);
                Log.TraceLog("Best: " + bestResult + ", current: " + new FaceResult()
                {
                    AccuracySquared = accSq, SumDeltaMag = sumDeltaMag, DeltaElevation = elevationDelta, DeltaAzimuth = azimuthDelta
                });
                if (bestResult.ReplaceBy(accSq, sumDeltaMag))
                {
                    bestResult.AccuracySquared = accSq;
                    bestResult.SumDeltaMag     = sumDeltaMag;
                    bestResult.DeltaAzimuth    = azimuthDelta;
                    bestResult.DeltaElevation  = elevationDelta;
                }
                return(true);
            }
            return(false);
        }
示例#8
0
        private bool CalcFaceTowards_NoAzimuth(DirectionWorld targetWorldSpace, out FaceResult bestResult)
        {
            bestResult = FaceResult.Default;

            foreach (var direction in FaceBlock.FaceDirections())
            {
                DirectionWorld faceDirection = new DirectionWorld()
                {
                    vector = FaceBlock.WorldMatrix.GetDirectionVector(direction)
                };
                CalcFaceTowards_NotClaimed(ref bestResult, targetWorldSpace, faceDirection, MathHelper.TwoPi);
            }

            if (bestResult.AccuracySquared != float.PositiveInfinity)
            {
                Log.TraceLog("Best: " + bestResult);
                return(true);
            }

            Log.TraceLog("Cannot rotate to target");
            return(false);
        }
示例#9
0
        private bool CalcFaceTowards_AzimuthOk(DirectionWorld targetWorldSpace, out FaceResult bestResult)
        {
            bestResult = FaceResult.Default;
            Vector3 target = targetWorldSpace.ToBlock(StatorAz);

            foreach (var direction in FaceBlock.FaceDirections())
            {
                DirectionWorld faceDirection = new DirectionWorld()
                {
                    vector = FaceBlock.WorldMatrix.GetDirectionVector(direction)
                };
                Vector3 current = faceDirection.ToBlock(StatorAz);

                float firstDelta, secondDelta;
                CalcDelta(current, target, out firstDelta, out secondDelta);
                if (m_claimedAzimuth)
                {
                    // azimuth has been claimed, check limits
                    if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, firstDelta))
                    {
                        Log.TraceLog("First azimuth delta is reachable: " + firstDelta);
                    }
                    else if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, secondDelta))
                    {
                        Log.TraceLog("Second azimuth delta is reachable: " + secondDelta);
                    }

                    if (bestResult.AccuracySquared > 0.1f)
                    {
                        // try flipped
                        float clamped = ClampToLimits(StatorAz, firstDelta);
                        if (clamped > 0f)
                        {
                            firstDelta  = clamped - MathHelper.Pi;
                            secondDelta = clamped + MathHelper.Pi;
                        }
                        else
                        {
                            firstDelta  = clamped + MathHelper.Pi;
                            secondDelta = clamped - MathHelper.Pi;
                        }

                        if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, firstDelta))
                        {
                            Log.TraceLog("First flipped azimuth delta is reachable: " + firstDelta);
                        }
                        else if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, secondDelta))
                        {
                            Log.TraceLog("Second flipped azimuth delta is reachable: " + secondDelta);
                        }
                    }
                }
                else if (CalcFaceTowards_NotClaimed(ref bestResult, targetWorldSpace, faceDirection, firstDelta))                 // azimuth has not been claimed, check that current azimuth is close enough
                {
                    Log.TraceLog("Azimuth is within tolerance: " + firstDelta);
                }
                else
                {
                    Log.TraceLog("Azimuth is outside tolerance: " + firstDelta);
                }
            }

            if (bestResult.AccuracySquared != float.PositiveInfinity)
            {
                Log.TraceLog("Best: " + bestResult);
                return(true);
            }

            Log.TraceLog("Cannot rotate to target");
            return(false);
        }