Пример #1
0
        /// <summary>
        /// Calculates the force necessary to move the grid.
        /// </summary>
        /// <param name="block">To get world position from.</param>
        /// <param name="destPoint">The world position of the destination</param>
        /// <param name="destVelocity">The speed of the destination</param>
        /// <param name="landing">Puts an emphasis on not overshooting the target.</param>
        public void CalcMove(PseudoBlock block, Vector3 destPoint, Vector3 destVelocity, bool landing = false)
        {
            CheckGrid();

            // using world vectors

            if (NavSet.Settings_Current.CollisionAvoidance)
            {
                myPathfinder.TestPath(destPoint, landing);
                if (!myPathfinder.CanMove)
                {
                    myLogger.debugLog("Pathfinder not allowing movement", "CalcMove()");
                    return;
                }
            }

            Vector3 destDisp = destPoint - block.WorldPosition;
            Vector3 velocity = Block.CubeGrid.Physics.LinearVelocity;

            // switch to using local vectors

            Matrix positionToLocal = Block.CubeBlock.WorldMatrixNormalizedInv;
            Matrix directionToLocal = positionToLocal.GetOrientation();

            destDisp = Vector3.Transform(destDisp, directionToLocal);
            destVelocity = Vector3.Transform(destVelocity, directionToLocal);
            velocity = Vector3.Transform(velocity, directionToLocal);

            float distance = destDisp.Length();
            NavSet.Settings_Task_NavWay.Distance = distance;

            Vector3 targetVelocity = MaximumVelocity(destDisp) * 0.5f;

            // project targetVelocity onto destination direction (take shortest path)
            Vector3 destDir = destDisp / distance;
            targetVelocity = Vector3.Dot(targetVelocity, destDir) * destDir;

            // apply relative speed limit
            float relSpeedLimit = NavSet.Settings_Current.SpeedMaxRelative;
            if (landing)
            {
                float landingSpeed = distance * 0.5f;
                if (relSpeedLimit > landingSpeed)
                    relSpeedLimit = landingSpeed;
            }
            if (relSpeedLimit < float.MaxValue)
            {
                float tarSpeedSq_1 = targetVelocity.LengthSquared();
                if (tarSpeedSq_1 > relSpeedLimit * relSpeedLimit)
                {
                    targetVelocity *= relSpeedLimit / (float)Math.Sqrt(tarSpeedSq_1);
                    myLogger.debugLog("imposing relative speed limit: " + relSpeedLimit + ", targetVelocity: " + targetVelocity, "CalcMove()");
                }
            }

            targetVelocity += destVelocity;

            // apply speed limit
            float tarSpeedSq = targetVelocity.LengthSquared();
            float speedRequest = NavSet.Settings_Current.SpeedTarget;
            if (tarSpeedSq > speedRequest * speedRequest)
                targetVelocity *= speedRequest / (float)Math.Sqrt(tarSpeedSq);

            Vector3 accel = targetVelocity - velocity;

            moveForceRatio = ToForceRatio(accel);

            // dampeners
            bool enableDampeners = false;
            for (int i = 0; i < 3; i++)
            {
                // if target velocity is close to 0, use dampeners

                float targetDim = targetVelocity.GetDim(i);
                if (targetDim < 0.1f && targetDim > -0.1f)
                {
                    //myLogger.debugLog("close to 0, i: " + i + ", targetDim: " + targetDim, "CalcMove()");
                    moveForceRatio.SetDim(i, 0);
                    enableDampeners = true;
                    continue;
                }

                // if there is not enough force available for braking, use dampeners

                float forceRatio = moveForceRatio.GetDim(i);
                if (forceRatio < 1f && forceRatio > -1f)
                    continue;

                float velDim = velocity.GetDim(i);
                if (velDim < 1f && velDim > -1f)
                    continue;

                if (Math.Sign(forceRatio) * Math.Sign(velDim) < 0)
                {
                    myLogger.debugLog("damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
                    moveForceRatio.SetDim(i, 0);
                    enableDampeners = true;
                }
                else
                    myLogger.debugLog("not damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
            }

            //if (enableDampeners)
            //	Logger.debugNotify("Damping", 16);

            Block.SetDamping(enableDampeners);

            myLogger.debugLog("destDisp: " + destDisp
                + ", destDir: " + destDir
                + ", destVelocity: " + destVelocity
                //+ ", relaVelocity: " + relaVelocity
                + ", targetVelocity: " + targetVelocity
                //+ ", diffVel: " + diffVel
                + ", accel: " + accel
                + ", moveForceRatio: " + moveForceRatio, "CalcMove()");
        }
Пример #2
0
        /// <summary>
        /// Calculates the force necessary to rotate the grid.
        /// </summary>
        /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
        /// <param name="Direction">The direction to face the localMatrix in.</param>
        /// <param name="angularVelocity">The local angular velocity of the controlling block.</param>
        private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity)
        {
            CheckGrid();

            myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR);

            angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());

            //myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()");
            float gyroForce = myGyro.TotalGyroForce();

            const ulong UpdateFrequency = ShipController_Autopilot.UpdateFrequency;
            if (rotateForceRatio != Vector3.Zero)
                if (Globals.UpdateCount - updated_prevAngleVel == UpdateFrequency) // needs to be == because displacment is not divided by frequency
                {
                    Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce);

                    //myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");

                    myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
                }
                else
                    myLogger.debugLog("prevAngleVel is old: " + (Globals.UpdateCount - updated_prevAngleVel), "CalcRotate()", Logger.severity.DEBUG);

            localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
            Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);

            localMatrix = localMatrix.GetOrientation();
            inverted = inverted.GetOrientation();

            //myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()");
            //myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()");
            //myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()");
            //myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()");

            Vector3 localDirect = Direction.ToLocal();
            Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);

            rotBlockDirect.Normalize();

            float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

            Vector3 rotaRight = localMatrix.Right;
            Vector3 rotaUp = localMatrix.Up;

            Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
            Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

            Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

            if (UpDirect != null)
            {
                Vector3 upLocal = UpDirect.ToLocal();
                Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
                float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);

                Vector3 rotaBackward = localMatrix.Backward;
                Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

                myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (-roll * NFR_backward), "CalcRotate()");

                displacement += roll * NFR_backward;
            }

            NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();

            if (NavSet.Settings_Current.CollisionAvoidance)
            {
                myPathfinder.TestRotate(displacement);
                if (!myPathfinder.CanRotate)
                {
                    Logger.debugNotify("Cannot Rotate", 50);
                    myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
                    return;
                }
            }

            //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");

            if (myGyro.torqueAccelRatio == 0)
            {
                // do a test
                myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
                rotateForceRatio = new Vector3(0, 1f, 0);
                return;
            }

            Vector3 targetVelocity = MaxAngleVelocity(displacement);

            // Adjust for moving target by measuring changes in displacement. Part of the change in displacement is attributable to ship rotation.
            const float dispToVel = (float)Globals.UpdatesPerSecond / (float)ShipController_Autopilot.UpdateFrequency;

            Vector3 addVelocity = angularVelocity - (prevAngleDisp - displacement) * dispToVel;
            if (addVelocity.LengthSquared() < 0.1f)
            {
                myLogger.debugLog("Adjust for moving, adding to target velocity: " + addVelocity, "CalcRotate()");
                targetVelocity += addVelocity;
            }
            else
                myLogger.debugLog("Not adjusting for moving, assuming target changed: " + addVelocity, "CalcRotate()");
            prevAngleDisp = displacement;

            Vector3 diffVel = targetVelocity - angularVelocity;

            rotateForceRatio = diffVel / (myGyro.torqueAccelRatio * gyroForce);

            myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", diffVel: " + diffVel, "CalcRotate()");
            //myLogger.debugLog("diffVel: " + diffVel + ", torque: " + (myGyro.torqueAccelRatio * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()");

            // dampeners
            for (int i = 0; i < 3; i++)
            {
                // if targetVelocity is close to 0, use dampeners

                float target = targetVelocity.GetDim(i);
                if (target > -0.01f && target < 0.01f)
                {
                    myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()");
                    rotateForceRatio.SetDim(i, 0f);
                    continue;
                }

                float velDim = angularVelocity.GetDim(i);
                if (velDim < 0.01f && velDim > -0.01f)
                    continue;

                // where rotateForceRatio opposes angularVelocity, use dampeners

                float dim = rotateForceRatio.GetDim(i);
                if (Math.Sign(dim) * Math.Sign(angularVelocity.GetDim(i)) < 0)
                    //{
                    //	myLogger.debugLog("force ratio(" + dim + ") opposes velocity(" + angularVelocity.GetDim(i) + "), index: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()");
                    rotateForceRatio.SetDim(i, 0f);
                //}
                //else
                //	myLogger.debugLog("force ratio is aligned with velocity: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()");
            }
        }
Пример #3
0
        /// <summary>
        /// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect.
        /// </summary>
        /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
        /// <param name="Direction">The direction to face the localMatrix in.</param>
        private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity)
        {
            m_logger.debugLog(Direction == null, "Direction == null", Logger.severity.ERROR);

            m_gyro.Update();
            float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor);
            if (minimumMoment <= 0f)
            {
                // == 0f, not calculated yet. < 0f, we have math failure
                StopRotate();
                m_logger.debugLog(minimumMoment < 0f, "minimumMoment < 0f", Logger.severity.FATAL);
                return;
            }

            localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
            Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);

            localMatrix = localMatrix.GetOrientation();
            inverted = inverted.GetOrientation();

            Vector3 localDirect = Direction.ToLocalNormalized();
            Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);

            float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

            Vector3 rotaRight = localMatrix.Right;
            Vector3 rotaUp = localMatrix.Up;

            Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
            Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

            Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

            if (UpDirect != null)
            {
                Vector3 upLocal = UpDirect.ToLocalNormalized();
                Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
                upRotBlock.Z = 0f;
                upRotBlock.Normalize();
                float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f));

                Vector3 rotaBackward = localMatrix.Backward;
                Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

                //m_logger.debugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()");

                displacement += roll * NFR_backward;
            }

            m_lastMoveAttempt = Globals.UpdateCount;
            Pathfinder.TestRotate(displacement);

            switch (Pathfinder.m_rotateState)
            {
                case Autopilot.Pathfinder.Pathfinder.PathState.Not_Running:
                    m_logger.debugLog("Pathfinder not run yet: " + Pathfinder.m_rotateState);
                    m_lastMove = Globals.UpdateCount;
                    return;
                case Autopilot.Pathfinder.Pathfinder.PathState.No_Obstruction:
                    break;
                default:
                    m_logger.debugLog("Pathfinder not allowing rotation: " + Pathfinder.m_rotateState);
                    return;
            }

            float distanceAngle = displacement.Length();
            if (distanceAngle < m_bestAngle || float.IsNaN(m_navSet.Settings_Current.DistanceAngle))
            {
                m_bestAngle = distanceAngle;
                m_lastMove = Globals.UpdateCount;
            }
            m_navSet.Settings_Task_NavWay.DistanceAngle = distanceAngle;

            //myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()");

            m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null);

            // adjustment to face a moving entity
            if (targetEntity != null)
            {
                Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - LinearVelocity;
                float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());

                //myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()");

                float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down);
                float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right);
                float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
                float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);

                m_logger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw));

                m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw;
            }
            //m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()");

            // angular velocity is reversed
            Vector3 angularVelocity = AngularVelocity.ToBlock(Block.CubeBlock);// ((DirectionWorld)(-Block.Physics.AngularVelocity)).ToBlock(Block.CubeBlock);
            m_rotateForceRatio = (m_rotateTargetVelocity + angularVelocity) / (minimumMoment * m_gyro.GyroForce);

            m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + (m_rotateTargetVelocity + angularVelocity));
            m_logger.debugLog("minimumMoment: " + minimumMoment + ", force: " + m_gyro.GyroForce + ", rotateForceRatio: " + m_rotateForceRatio);

            // dampeners
            for (int index = 0; index < 3; index++)
            {
                // if targetVelocity is close to 0, use dampeners

                float target = m_rotateTargetVelocity.GetDim(index);
                if (target > -0.01f && target < 0.01f)
                {
                    //m_logger.debugLog("target near 0 for " + i + ", " + target, "in_CalcRotate()");
                    m_rotateTargetVelocity.SetDim(index, 0f);
                    m_rotateForceRatio.SetDim(index, 0f);
                    continue;
                }

            }
        }
Пример #4
0
        private void CalcMove(ref Vector3 velocity)
        {
            DirectionBlock gravBlock = Thrust.WorldGravity.ToBlock(Block.CubeBlock);
            m_moveForceRatio = ToForceRatio(m_moveAccel - gravBlock.vector);

            // dampeners
            bool enableDampeners = false;
            m_thrustHigh = false;

            for (int index = 0; index < 3; index++)
            {
                //const float minForceRatio = 0.1f;
                //const float zeroForceRatio = 0.01f;

                float velDim = velocity.GetDim(index);

                // dampeners are useful for precise stopping but they do not always work properly
                if (velDim < 1f && velDim > -1f)
                {
                    float targetVelDim = m_moveAccel.GetDim(index) + velDim;

                    if (targetVelDim < 0.01f && targetVelDim > -0.01f)
                    {
                        m_logger.debugLog("for dim: " + index + ", target velocity near zero: " + targetVelDim);
                        m_moveForceRatio.SetDim(index, 0f);
                        enableDampeners = true;
                        continue;
                    }
                }

                float forceRatio = m_moveForceRatio.GetDim(index);

                if (forceRatio < 1f && forceRatio > -1f)
                {
                    //if (forceRatio > zeroForceRatio && forceRatio < minForceRatio)
                    //	moveForceRatio.SetDim(i, minForceRatio);
                    //else if (forceRatio < -zeroForceRatio && forceRatio > -minForceRatio)
                    //	moveForceRatio.SetDim(i, -minForceRatio);
                    continue;
                }

                m_thrustHigh = true;

                // force ratio is > 1 || < -1. If it is useful, use dampeners

                if (velDim < 1f && velDim > -1f)
                    continue;

                if (Math.Sign(forceRatio) * Math.Sign(velDim) < 0)
                {
                    m_logger.debugLog("damping, i: " + index + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim));
                    m_moveForceRatio.SetDim(index, 0);
                    enableDampeners = true;
                }
                //else
                //	myLogger.debugLog("not damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
            }

            SetDamping(enableDampeners);
        }
Пример #5
0
		/// <summary>
		/// Calculates the force necessary to rotate the grid.
		/// </summary>
		/// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
		/// <param name="Direction">The direction to face the localMatrix in.</param>
		/// <param name="angularVelocity">The local angular velocity of the controlling block.</param>
		private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity, IMyEntity targetEntity)
		{
			myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR);

			angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());

			//myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()");
			float gyroForce = myGyro.TotalGyroForce();

			float secondsSinceLast = (float)(DateTime.UtcNow - updated_prevAngleVel).TotalSeconds;
			updated_prevAngleVel = DateTime.UtcNow;

			if (rotateForceRatio != Vector3.Zero)
			{
				if (secondsSinceLast <= MaxUpdateSeconds)
				{
					Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce * secondsSinceLast);

					//myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");

					myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
				}
				else
					myLogger.debugLog("prevAngleVel is old: " + secondsSinceLast, "CalcRotate()", Logger.severity.DEBUG);
			}

			localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
			Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);

			localMatrix = localMatrix.GetOrientation();
			inverted = inverted.GetOrientation();

			//myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()");
			//myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()");
			//myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()");
			//myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()");

			Vector3 localDirect = Direction.ToLocal();
			Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);

			rotBlockDirect.Normalize();

			float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);

			Vector3 rotaRight = localMatrix.Right;
			Vector3 rotaUp = localMatrix.Up;

			Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
			Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));

			Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;

			if (UpDirect != null)
			{
				Vector3 upLocal = UpDirect.ToLocal();
				Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
				float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);

				Vector3 rotaBackward = localMatrix.Backward;
				Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));

				myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (roll * NFR_backward), "CalcRotate()");

				displacement += roll * NFR_backward;
			}

			NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();

			if (NavSet.Settings_Current.CollisionAvoidance)
			{
				myPathfinder.TestRotate(displacement);
				if (!myPathfinder.CanRotate)
				{
					// if cannot rotate and not calculating move, move away from obstruction
					if (myPathfinder.RotateObstruction != null && Globals.UpdateCount >= m_notCalcMove)
					{
						Vector3 position  = Block.CubeBlock.GetPosition();
						Vector3 away = position - myPathfinder.RotateObstruction.GetCentre();
						away.Normalize();
						myLogger.debugLog("Cannot rotate and not calculating move, creating GOLIS to move away from obstruction", "CalcRotate()", Logger.severity.INFO);
						new GOLIS(this, NavSet, position + away * (10f + NavSet.Settings_Current.DestinationRadius), true);
					}
					Logger.debugNotify("Cannot Rotate", 50);
					myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
					return;
				}
			}

			//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");

			if (myGyro.torqueAccelRatio == 0)
			{
				// do a test
				myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
				rotateForceRatio = new Vector3(0, 1f, 0);
				return;
			}

			Vector3 targetVelocity = MaxAngleVelocity(displacement, secondsSinceLast);

			if (targetEntity != null)
			{
				Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - Block.Physics.LinearVelocity;
				float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());
				//Vector3 tangentialVelocity = Vector3.Reject(relativeLinearVelocity, targetEntity.GetCentre() - Block.CubeBlock.GetPosition());
				//Vector3 localTangVel = Vector3.Transform(tangentialVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());

				//myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "CalcRotate()");

				float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Up);
				float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Left);
				float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
				float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);

				myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch, "CalcRotate()");

				targetVelocity += new Vector3(angl_pitch, angl_yaw, 0f);
			}

			Vector3 accel = (targetVelocity - angularVelocity) / secondsSinceLast;

			rotateForceRatio = accel / (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce);

			myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + accel, "CalcRotate()");
			myLogger.debugLog("accel: " + accel + ", torque: " + (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()");

			// dampeners
			for (int i = 0; i < 3; i++)
			{
				// if targetVelocity is close to 0, use dampeners

				float target = targetVelocity.GetDim(i);
				if (target > -0.01f && target < 0.01f)
				{
					//myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()");
					rotateForceRatio.SetDim(i, 0f);
					continue;
				}
			}
		}
Пример #6
0
		/// <summary>
		/// Calculates the force necessary to move the grid.
		/// </summary>
		/// <param name="block">To get world position from.</param>
		/// <param name="destPoint">The world position of the destination</param>
		/// <param name="destVelocity">The speed of the destination</param>
		/// <param name="landing">Puts an emphasis on not overshooting the target.</param>
		public void CalcMove(PseudoBlock block, Vector3 destPoint, Vector3 destVelocity, bool landing = false)
		{
			CheckGrid();

			// using world vectors

			if (NavSet.Settings_Current.CollisionAvoidance)
			{
				myPathfinder.TestPath(destPoint, landing);
				if (!myPathfinder.CanMove)
				{
					myLogger.debugLog("Pathfinder not allowing movement", "CalcMove()");
					return;
				}
			}

			myThrust.Update();

			Vector3 destDisp = destPoint - block.WorldPosition;
			Vector3 velocity = Block.CubeGrid.Physics.LinearVelocity;

			// switch to using local vectors

			Matrix positionToLocal = Block.CubeBlock.WorldMatrixNormalizedInv;
			Matrix directionToLocal = positionToLocal.GetOrientation();

			destDisp = Vector3.Transform(destDisp, directionToLocal);
			destVelocity = Vector3.Transform(destVelocity, directionToLocal);
			velocity = Vector3.Transform(velocity, directionToLocal);

			float distance = destDisp.Length();
			NavSet.Settings_Task_NavWay.Distance = distance;

			m_targetVelocity = MaximumVelocity(destDisp) * 0.5f;

			// project targetVelocity onto destination direction (take shortest path)
			Vector3 destDir = destDisp / distance;
			m_targetVelocity = Vector3.Dot(m_targetVelocity, destDir) * destDir;

			// apply relative speed limit
			float relSpeedLimit = NavSet.Settings_Current.SpeedMaxRelative;
			if (landing)
			{
				float landingSpeed = distance * 0.5f;
				if (relSpeedLimit > landingSpeed)
					relSpeedLimit = landingSpeed;
			}
			if (relSpeedLimit < float.MaxValue)
			{
				float tarSpeedSq_1 = m_targetVelocity.LengthSquared();
				if (tarSpeedSq_1 > relSpeedLimit * relSpeedLimit)
				{
					m_targetVelocity *= relSpeedLimit / (float)Math.Sqrt(tarSpeedSq_1);
					myLogger.debugLog("imposing relative speed limit: " + relSpeedLimit + ", targetVelocity: " + m_targetVelocity, "CalcMove()");
				}
			}

			m_targetVelocity += destVelocity;

			// apply speed limit
			float tarSpeedSq = m_targetVelocity.LengthSquared();
			float speedRequest = NavSet.Settings_Current.SpeedTarget;
			if (tarSpeedSq > speedRequest * speedRequest)
				m_targetVelocity *= speedRequest / (float)Math.Sqrt(tarSpeedSq);

			m_moveAccel = m_targetVelocity - velocity - myThrust.m_localGravity;

			moveForceRatio = ToForceRatio(m_moveAccel);

			// dampeners
			m_moveEnableDampeners = false;
			bool checkNearZero = m_targetVelocity.LengthSquared() < 1f || m_moveAccel.LengthSquared() > 1f;

			for (int i = 0; i < 3; i++)
			{
				float targetDim = m_targetVelocity.GetDim(i);

				if (checkNearZero)
				{
					// if target velocity is close to 0, use dampeners

					if (targetDim < 0.1f && targetDim > -0.1f)
					{
						myLogger.debugLog("close to 0, i: " + i + ", targetDim: " + targetDim, "CalcMove()");
						moveForceRatio.SetDim(i, 0);
						m_moveEnableDampeners = true;
						continue;
					}
				}

				float forceRatio = moveForceRatio.GetDim(i);
				if (forceRatio < 1f && forceRatio > -1f)
				{
					// minimum force ratio is needed because SE is being strange about gravity
					if (forceRatio < MinForceRatio && forceRatio > -MinForceRatio && myThrust.m_worldGravity.LengthSquared() > 0.1f)
					{
						if (targetDim > 0.1f)
							moveForceRatio.SetDim(i, MinForceRatio);
						else
							moveForceRatio.SetDim(i, -MinForceRatio);
					}
					continue;
				}

				// force ratio is > 1 || < -1. If it is useful, use dampeners

				float velDim = velocity.GetDim(i);
				if (velDim < 1f && velDim > -1f)
					continue;

				if (Math.Sign(forceRatio) * Math.Sign(velDim) < 0)
				{
					myLogger.debugLog("damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
					moveForceRatio.SetDim(i, 0);
					m_moveEnableDampeners = true;
				}
				else
					myLogger.debugLog("not damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
			}

			Block.SetDamping(m_moveEnableDampeners);

			if (destDisp.LengthSquared() > 1f)
				m_notCalcMove = Globals.UpdateCount + CalcMoveIdle;

			myLogger.debugLog("destDisp: " + destDisp
				//+ ", destDir: " + destDir
				+ ", destVelocity: " + destVelocity
				//+ ", relaVelocity: " + relaVelocity
				+ ", targetVelocity: " + m_targetVelocity
				+ ", velocity: " + velocity
				//+ ", diffVel: " + diffVel
				+ ", m_moveAccel: " + m_moveAccel
				+ ", moveForceRatio: " + moveForceRatio, "CalcMove()");
		}