Esempio n. 1
0
		public void Update_torqueAccelRatio(Vector3 command, Vector3 ratio)
		{
			if (Globals.UpdateCount >= m_nextCheck_attachedGrids)
				CheckAttachedGrids();

			float mass = myGrid.Physics.Mass;
			if (!dirty_torqueAccelRatio)
			{
				dirty_torqueAccelRatio = mass > m_mass;
				myLogger.debugLog(dirty_torqueAccelRatio, "mass increased from " + m_mass + " to " + mass, "Update_torqueAccelRatio()");
			}
			else
				myLogger.debugLog("torqueAccelRatio is dirty", "Update_torqueAccelRatio()");
			m_mass = mass;

			for (int i = 0; i < 3; i++)
			{
				// where ratio is from damping, ignore it
				if (Math.Abs(command.GetDim(i)) < 0.01f)
					continue;

				float dim = ratio.GetDim(i);
				if (dim.IsValid() && (dirty_torqueAccelRatio || dim > torqueAccelRatio))
				{
					if (dim > 0f)
					{
						myLogger.debugLog("torqueAccelRatio changed from " + torqueAccelRatio + " to " + dim, "Update_torqueAccelRatio()", Logger.severity.DEBUG);
						torqueAccelRatio = dim;
						dirty_torqueAccelRatio = false;
					}
					else // caused by bumping into things
						myLogger.debugLog("dim <= 0 : " + dim, "Update_torqueAccelRatio()", Logger.severity.DEBUG);
				}
			}
		}
Esempio n. 2
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()");
        }
Esempio n. 3
0
        /// <summary>
        /// Calulates the maximum angular velocity to stop at the destination.
        /// </summary>
        /// <param name="disp">Displacement to the destination.</param>
        /// <returns>The maximum angular velocity to stop at the destination.</returns>
        private Vector3 MaxAngleVelocity(Vector3 disp)
        {
            Vector3 result = Vector3.Zero;

            // S.E. provides damping for angular motion, we will ignore this
            float accel = -myGyro.torqueAccelRatio * myGyro.TotalGyroForce();

            //myLogger.debugLog("torqueAccelRatio: " + myGyro.torqueAccelRatio + ", TotalGyroForce: " + myGyro.TotalGyroForce() + ", accel: " + accel, "MaxAngleVelocity()");

            //myLogger.debugLog("speed cap: " + speedCap, "MaxAngleVelocity()");

            for (int i = 0; i < 3; i++)
            {
                float dim = disp.GetDim(i);
                if (dim > 0)
                    //result.SetDim(i, Math.Min(MaxAngleSpeed(accel, dim), speedCap));
                    result.SetDim(i, MaxAngleSpeed(accel, dim));
                else if (dim < 0)
                    //result.SetDim(i, -Math.Min(MaxAngleSpeed(accel, -dim), speedCap));
                    result.SetDim(i, -MaxAngleSpeed(accel, -dim));
            }

            return result;
        }
Esempio n. 4
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()");
            }
        }
Esempio n. 5
0
        /// <summary>
        /// Calulates the maximum angular velocity to stop at the destination.
        /// </summary>
        /// <param name="disp">Displacement to the destination.</param>
        /// <returns>The maximum angular velocity to stop at the destination.</returns>
        private Vector3 MaxAngleVelocity(Vector3 disp, float minimumMoment, bool fast)
        {
            Vector3 result = Vector3.Zero;

            // S.E. provides damping for angular motion, we will ignore this
            float accel = -minimumMoment * m_gyro.GyroForce;

            for (int index = 0; index < 3; index++)
            {
                float dim = disp.GetDim(index);
                if (dim > 0)
                    result.SetDim(index, MaxAngleSpeed(accel, dim, fast));
                else if (dim < 0)
                    result.SetDim(index, -MaxAngleSpeed(accel, -dim, fast));
            }

            return result;
        }
Esempio n. 6
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);
        }
Esempio n. 7
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()");
		}