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);
}
