public ThrusterGroup SolvePartialThrust(Vector3D g, Vector3D targetThrust) { var resultantForce = -g + targetThrust; var t1 = FindThrusterGroupsInDirection(resultantForce); //var mat = new double[,] { // { t1.finalThrusterGroups[0].WorldThrustDirection.X, t1.finalThrusterGroups[1].WorldThrustDirection.X, t1.finalThrusterGroups[2].WorldThrustDirection.X }, // { t1.finalThrusterGroups[0].WorldThrustDirection.Y, t1.finalThrusterGroups[1].WorldThrustDirection.Y, t1.finalThrusterGroups[2].WorldThrustDirection.Y }, // { t1.finalThrusterGroups[0].WorldThrustDirection.Z, t1.finalThrusterGroups[1].WorldThrustDirection.Z, t1.finalThrusterGroups[2].WorldThrustDirection.Z }, //}; t1.matrixM[0, 0] = t1.finalThrusterGroups[0].WorldThrustDirection.X; t1.matrixM[0, 1] = t1.finalThrusterGroups[1].WorldThrustDirection.X; t1.matrixM[0, 2] = t1.finalThrusterGroups[2].WorldThrustDirection.X; t1.matrixM[1, 0] = t1.finalThrusterGroups[0].WorldThrustDirection.Y; t1.matrixM[1, 1] = t1.finalThrusterGroups[1].WorldThrustDirection.Y; t1.matrixM[1, 2] = t1.finalThrusterGroups[2].WorldThrustDirection.Y; t1.matrixM[2, 0] = t1.finalThrusterGroups[0].WorldThrustDirection.Z; t1.matrixM[2, 1] = t1.finalThrusterGroups[1].WorldThrustDirection.Z; t1.matrixM[2, 2] = t1.finalThrusterGroups[2].WorldThrustDirection.Z; t1.ANS[0] = resultantForce.X; t1.ANS[1] = resultantForce.Y; t1.ANS[2] = resultantForce.Z; //t1.ANS WorldDirectionForce.X, WorldDirectionForce.Y, WorldDirectionForce.Z }; PID.ComputeCoefficients(t1.matrixM, t1.ANS); t1.finalThrustForces.X = t1.ANS[0]; t1.finalThrustForces.Y = t1.ANS[1]; t1.finalThrustForces.Z = t1.ANS[2]; return(t1); }
/// <summary> /// Solves the required thrusts to go in targetDirection at max speed. This takes gravity into account. /// </summary> /// <param name="g">Gravity and unknown forces</param> /// <param name="targetDirection">The target direction to go in</param> /// <param name="maxPercentageThrustToUse">The maximum amount of thrust devoted to going in the target direction.</param> /// <returns></returns> public ThrusterGroup SolveMaxThrust(Vector3D minus_g, Vector3D targetDirection, double maxPercentageThrustToUse = 1) { //parent_program.Echo("Starting"); Base6Directions.Direction actual2Di; Base6Directions.Direction actual3Di; double t2c; double t3c; double Lambda; ThrusterGroup t1; ThrusterGroup t2; ThrusterGroup t3; if (maxPercentageThrustToUse > 1) { maxPercentageThrustToUse = 1; } else if (maxPercentageThrustToUse < 0) { maxPercentageThrustToUse = 0; } foreach (var entry in thrusterGroups) { //parent_program.Echo("Loading " + entry.Key.ToString()); t1 = entry.Value; if (t1.LocalThrustDirection == Base6Directions.Direction.Up || t1.LocalThrustDirection == Base6Directions.Direction.Down) { t2 = thrusterGroups[Base6Directions.Direction.Left]; t3 = thrusterGroups[Base6Directions.Direction.Forward]; } else if (t1.LocalThrustDirection == Base6Directions.Direction.Left || t1.LocalThrustDirection == Base6Directions.Direction.Right) { t2 = thrusterGroups[Base6Directions.Direction.Up]; t3 = thrusterGroups[Base6Directions.Direction.Forward]; } else if (t1.LocalThrustDirection == Base6Directions.Direction.Forward || t1.LocalThrustDirection == Base6Directions.Direction.Backward) { t2 = thrusterGroups[Base6Directions.Direction.Up]; t3 = thrusterGroups[Base6Directions.Direction.Left]; } else { parent_program.shipIOHandler.Error( "Encountered unusual thruster direction.\nIf you've gotten this error in particular,\nplease report it to the script owner, Spug."); t2 = thrusterGroups[Base6Directions.Direction.Up]; t3 = thrusterGroups[Base6Directions.Direction.Left]; } //var mat = new double[,] { //{ t2.WorldThrustDirection.X, t3.WorldThrustDirection.X, -targetDirection.X }, //{ t2.WorldThrustDirection.Y, t3.WorldThrustDirection.Y, -targetDirection.Y }, //{ t2.WorldThrustDirection.Z, t3.WorldThrustDirection.Z, -targetDirection.Z }, //}; t1.matrixM[0, 0] = t2.WorldThrustDirection.X; t1.matrixM[0, 1] = t3.WorldThrustDirection.X; t1.matrixM[0, 2] = -targetDirection.X; t1.matrixM[1, 0] = t2.WorldThrustDirection.Y; t1.matrixM[1, 1] = t3.WorldThrustDirection.Y; t1.matrixM[1, 2] = -targetDirection.Y; t1.matrixM[2, 0] = t2.WorldThrustDirection.Z; t1.matrixM[2, 1] = t3.WorldThrustDirection.Z; t1.matrixM[2, 2] = -targetDirection.Z; t1.ANS[0] = -t1.MaxThrust * t1.WorldThrustDirection.X * maxPercentageThrustToUse - minus_g.X; t1.ANS[1] = -t1.MaxThrust * t1.WorldThrustDirection.Y * maxPercentageThrustToUse - minus_g.Y; t1.ANS[2] = -t1.MaxThrust * t1.WorldThrustDirection.Z * maxPercentageThrustToUse - minus_g.Z; PID.ComputeCoefficients(t1.matrixM, t1.ANS); t2c = t1.ANS[0]; t3c = t1.ANS[1]; Lambda = t1.ANS[2]; actual2Di = t2.LocalThrustDirection; actual3Di = t3.LocalThrustDirection; if (t2c < 0) { actual2Di = Base6Directions.GetOppositeDirection(t2.LocalThrustDirection); t2c *= -1; } if (t3c < 0) { actual3Di = Base6Directions.GetOppositeDirection(t3.LocalThrustDirection); t3c *= -1; } //// Publish the results t1.finalThrustForces.X = t1.MaxThrust * maxPercentageThrustToUse; t1.finalThrustForces.Y = t2c; t1.finalThrustForces.Z = t3c; t1.lambdaResult = Lambda; t1.finalThrusterGroups[0] = t1; t1.finalThrusterGroups[1] = thrusterGroups[actual2Di]; t1.finalThrusterGroups[2] = thrusterGroups[actual3Di]; t1.finalThrusterGroups[3] = thrusterGroups[Base6Directions.GetOppositeDirection(t1.LocalThrustDirection)]; t1.finalThrusterGroups[4] = thrusterGroups[Base6Directions.GetOppositeDirection(t2.LocalThrustDirection)]; t1.finalThrusterGroups[5] = thrusterGroups[Base6Directions.GetOppositeDirection(t3.LocalThrustDirection)]; //parent_program.Echo("Completed " + entry.Key.ToString()); } ThrusterGroup bestCandidate = null; double bestCandidateLambda = -999999999; foreach (var entry in thrusterGroups) { if (entry.Value.lambdaResult > bestCandidateLambda) { if (entry.Value.finalThrustForces.Y <= entry.Value.finalThrusterGroups[1].MaxThrust + 1 && entry.Value.finalThrustForces.Z <= entry.Value.finalThrusterGroups[2].MaxThrust + 1) { bestCandidate = entry.Value; bestCandidateLambda = entry.Value.lambdaResult; } } } //parent_program.Echo("Finished"); //parent_program.Echo("Best: " + bestCandidate.LocalThrustDirection.ToString()); return(bestCandidate); }