void CalculateThrusterExample()
{
float maxThrusterForce = 2.0f;
KerbCom.CLP.Problem thrustProblem = new KerbCom.CLP.Problem();
KerbCom.CLP.Constraint directionConstraint, maxThrustConstraint;
var cnames = new Dictionary <KerbCom.CLP.Constraint, string>();
var vnames = new Dictionary <KerbCom.CLP.BoundedVariable, string>();
thrustProblem.constraints.AddRange(new KerbCom.CLP.Constraint[] {
directionConstraint = new KerbCom.CLP.Constraint(),
maxThrustConstraint = new KerbCom.CLP.Constraint(),
});
directionConstraint.RHS = 0.0;
cnames [directionConstraint] = "directionConstraint constraint";
//constraint currently causes error. I want it to say less than 5, it currently says EQUAL TO 5
//aka if they produce less force there won't be optimal solution
maxThrustConstraint.RHS = 20.0;
cnames [maxThrustConstraint] = "maxThrustConstraint constraint";
int thrusterCount = 10;
Vector2[] thrusterDirections = new Vector2[thrusterCount];
Vector2 linearDirection = new Vector2(Mathf.Cos(30 * Mathf.Deg2Rad), Mathf.Sin(30 * Mathf.Deg2Rad));
//Vector2 linearDirection = new Vector2(Mathf.Cos (290 * Mathf.Deg2Rad), Mathf.Sin (290 * Mathf.Deg2Rad));
thrusterDirections [0] = new Vector2(Mathf.Cos(20 * Mathf.Deg2Rad), Mathf.Sin(20 * Mathf.Deg2Rad));
thrusterDirections [1] = new Vector2(Mathf.Cos(70 * Mathf.Deg2Rad), Mathf.Sin(70 * Mathf.Deg2Rad));
thrusterDirections [2] = new Vector2(Mathf.Cos(115 * Mathf.Deg2Rad), Mathf.Sin(115 * Mathf.Deg2Rad));
thrusterDirections [3] = new Vector2(Mathf.Cos(150 * Mathf.Deg2Rad), Mathf.Sin(150 * Mathf.Deg2Rad));
thrusterDirections [4] = new Vector2(Mathf.Cos(160 * Mathf.Deg2Rad), Mathf.Sin(160 * Mathf.Deg2Rad));
thrusterDirections [5] = new Vector2(Mathf.Cos(185 * Mathf.Deg2Rad), Mathf.Sin(185 * Mathf.Deg2Rad));
thrusterDirections [6] = new Vector2(Mathf.Cos(206 * Mathf.Deg2Rad), Mathf.Sin(206 * Mathf.Deg2Rad));
thrusterDirections [7] = new Vector2(Mathf.Cos(230 * Mathf.Deg2Rad), Mathf.Sin(230 * Mathf.Deg2Rad));
thrusterDirections [8] = new Vector2(Mathf.Cos(300 * Mathf.Deg2Rad), Mathf.Sin(300 * Mathf.Deg2Rad));
thrusterDirections [9] = new Vector2(Mathf.Cos(350 * Mathf.Deg2Rad), Mathf.Sin(350 * Mathf.Deg2Rad));
KerbCom.CLP.BoundedVariable[] allVariables = new KerbCom.CLP.BoundedVariable[thrusterCount + 1];
for (int i = 0; i < thrusterCount + 1; ++i)
{
if (i == thrusterCount)
{
allVariables [i] = new KerbCom.CLP.BoundedVariable();
//print ("special set " + i + " thrusterCount: " + thrusterCount);
}
else
{
allVariables [i] = new KerbCom.CLP.BoundedVariable(0, maxThrusterForce);
}
vnames[allVariables [i]] = "var " + i;
}
for (int i = 0; i < thrusterCount; ++i)
{
KerbCom.CLP.BoundedVariable thrustVar = allVariables [i];
float dotProduct = Vector2.Dot(thrusterDirections[i], linearDirection);
thrustProblem.objective[thrustVar] = dotProduct;
maxThrustConstraint.f[thrustVar] = dotProduct;
directionConstraint.f[thrustVar] = thrusterDirections[i].x - (linearDirection.x / linearDirection.y) * thrusterDirections[i].y;
}
//our less than constraint
//allVariables [5] = new KerbCom.CLP.BoundedVariable ();
var thrustVarLast = allVariables [thrusterCount];
maxThrustConstraint.f[thrustVarLast] = 1;
KerbCom.CLP.Solvers.Solver thrustSolver = new KerbCom.CLP.Solvers.MaxLPSolve(thrustProblem, allVariables);
thrustSolver.solve();
print(thrustSolver.dump(vnames, cnames));
int valueNum = 0;
Vector2 resultVec = Vector2.zero;
foreach (double valueD in thrustSolver.values)
{
print("var " + valueNum + ": " + valueD);
if (valueNum < 10)
{
float valueF = (float)valueD;
Vector2 currentVec;
if (valueF > 0)
{
currentVec = new Vector2(valueF * thrusterDirections[valueNum].x, valueF * thrusterDirections[valueNum].y);
Debug.DrawLine(Vector2.zero, currentVec, Color.blue, 300);
resultVec += currentVec;
}
else
{
currentVec = new Vector2(thrusterDirections[valueNum].x, thrusterDirections[valueNum].y);
Debug.DrawLine(Vector2.zero, currentVec, Color.red, 300);
}
}
valueNum++;
}
if (thrustSolver.status != KerbCom.CLP.Solvers.Solver.Status.Optimal)
{
print("solve failed, not optimal");
}
else
{
double maxThrust = thrustSolver.objective_value;
if (maxThrust <= 0)
{
print("impossible direction: " + maxThrust);
Debug.DrawLine(Vector2.zero, linearDirection, Color.cyan, 300);
}
else
{
print("maxThrust: " + maxThrust);
Debug.DrawLine(Vector2.zero, resultVec, Color.green, 300);
}
}
}
void CalculateBoosterForces(Vector2 targetDirection)
{
float maxThrusterForce = 2.0f;
KerbCom.CLP.Problem thrustProblem = new KerbCom.CLP.Problem();
KerbCom.CLP.Constraint directionConstraint, maxThrustConstraint,
torqueContstraint;
var cnames = new Dictionary <KerbCom.CLP.Constraint, string>();
var vnames = new Dictionary <KerbCom.CLP.BoundedVariable, string>();
thrustProblem.constraints.AddRange(new KerbCom.CLP.Constraint[] {
directionConstraint = new KerbCom.CLP.Constraint(),
maxThrustConstraint = new KerbCom.CLP.Constraint(),
torqueContstraint = new KerbCom.CLP.Constraint()
});
directionConstraint.RHS = 0.0;
cnames [directionConstraint] = "directionConstraint constraint";
//less than 5 maxthrust
maxThrustConstraint.RHS = 5.0;
cnames [maxThrustConstraint] = "maxThrustConstraint constraint";
torqueContstraint.RHS = 0.0;
cnames [torqueContstraint] = "torqueContstraint constraint";
KerbCom.CLP.BoundedVariable[] allVariables = new KerbCom.CLP.BoundedVariable[boosters.Length + 1];
for (int i = 0; i < boosters.Length + 1; ++i)
{
if (i == boosters.Length)
{
allVariables [i] = new KerbCom.CLP.BoundedVariable();
//print ("special set " + i + " thrusterCount: " + thrusterCount);
}
else
{
allVariables [i] = new KerbCom.CLP.BoundedVariable(0, maxThrusterForce);
}
vnames[allVariables [i]] = "var " + i;
}
for (int boosterNum = 0; boosterNum < boosters.Length; boosterNum++)
{
BoosterNavigate boosterScript = boosters[boosterNum].GetComponent <BoosterNavigate> ();
Vector2 boosterDirection = boosterScript.GetDirection();
//print ("booster direction " + boosters[boosterNum].name + " " + boosterNum + ": " + boosterDirection);
KerbCom.CLP.BoundedVariable thrustVar = allVariables [boosterNum];
float dotProduct = Vector2.Dot(boosterDirection, targetDirection);
thrustProblem.objective[thrustVar] = dotProduct;
maxThrustConstraint.f[thrustVar] = dotProduct;
//determined by setting (x0/y0)*var0 + (x1/1)*var1 = (lx/ly)
directionConstraint.f[thrustVar] = boosterDirection.x - (targetDirection.x / targetDirection.y) * boosterDirection.y;
//create a torque constraint that minimizes torque to 0
//now that we know what the center of mass is we can determine the lever,
//which is from the center of the booster to the center of mass/
//keep this as a pending action. if we want to minimize
//torque for a direction we will have to do this
//failed because of torque constraint
// Vector3 boosterToCenterMass = new Vector3(
// boosters[boosterNum].transform.position.x - centerOfMassObject.transform.position.x,
// boosters[boosterNum].transform.position.y - centerOfMassObject.transform.position.y,
// 0);
// print (boosters[boosterNum].name + " boosterToCenterMass: " + boosterToCenterMass.x + " " +
// boosterToCenterMass.y + " " + boosterToCenterMass.z);
// Vector3 boosterDirection3d = new Vector3(
// boosterDirection.x,
// boosterDirection.y,
// 0);
// var cross = Vector3.Cross(
// boosterToCenterMass,
// boosterDirection3d);
// print ("cross: " + cross.x + " " + cross.y + " " + cross.z);
//similar to our distance constraint, except here
//we add all the cross product result vectors and
//set that equal to 0
// torqueContstraint.f[thrustVar] = cross.z;
//create a new problem that solves for torque in
//a direction if we can't go the desired direction
//here
//modify ship to have 2 torque boosters
}
var thrustVarLast = allVariables [boosters.Length];
maxThrustConstraint.f[thrustVarLast] = 1;
KerbCom.CLP.Solvers.Solver thrustSolver = new KerbCom.CLP.Solvers.MaxLPSolve(thrustProblem, allVariables);
thrustSolver.solve();
print(thrustSolver.dump(vnames, cnames));
int valueNum = 0;
Vector2 resultVec = Vector2.zero;
foreach (double valueD in thrustSolver.values)
{
print("var " + valueNum + ": " + valueD);
if (valueNum < boosters.Length)
{
float valueF = (float)valueD;
Vector2 currentVec;
var boosterTransform = boosters[valueNum].transform;
BoosterNavigate boosterScript = boosters[valueNum].GetComponent <BoosterNavigate> ();
Vector2 boosterDirection = boosterScript.GetDirection();
if (valueF > 0)
{
currentVec = new Vector2(boosterTransform.position.x + valueF * boosterDirection.x,
boosterTransform.position.y + valueF * boosterDirection.y);
Color colourrr = Color.white;
if (valueNum == 0)
{
colourrr = Color.magenta;
}
else if (valueNum == 1)
{
colourrr = Color.blue;
}
else if (valueNum == 2)
{
colourrr = Color.white;
}
else if (valueNum == 3)
{
colourrr = Color.black;
}
Debug.DrawLine(boosterTransform.position, currentVec, colourrr, 1);
var resTempVec = new Vector2(valueF * boosterDirection.x, valueF * boosterDirection.y);
resultVec += resTempVec;
boosterScript.SetCurrentForce(valueF / 10);
boosterScript.ActivateBooster();
}
else
{
//currentVec = new Vector2(boosterDirection.x, boosterDirection.y);
currentVec = new Vector2(boosterDirection.x + boosterTransform.position.x,
boosterDirection.y + boosterTransform.position.y);
Debug.DrawLine(boosterTransform.position, currentVec, Color.red, 1);
boosterScript.DeActivateBooster();
}
}
valueNum++;
}
bool returnStatus = true;
var hullPiece = GameObject.Find("HullPiece");
if (thrustSolver.status != KerbCom.CLP.Solvers.Solver.Status.Optimal)
{
//print ("solve failed, not optimal");
var finalDirection = new Vector2(targetDirection.x + hullPiece.transform.position.x,
targetDirection.y + hullPiece.transform.position.y);
Debug.DrawLine(hullPiece.transform.position, finalDirection, Color.yellow, 1);
returnStatus = false;
}
else
{
double maxThrust = thrustSolver.objective_value;
if (maxThrust <= 0)
{
//print ("impossible direction: " + maxThrust);
var finalDirection = new Vector2(targetDirection.x + hullPiece.transform.position.x,
targetDirection.y + hullPiece.transform.position.y);
Debug.DrawLine(hullPiece.transform.position, finalDirection, Color.cyan, 1);
returnStatus = false;
}
else
{
//print ("maxThrust: " + maxThrust);
var finalDirection = new Vector2(resultVec.x + hullPiece.transform.position.x,
resultVec.y + hullPiece.transform.position.y);
//Debug.DrawLine (hullPiece.transform.position, finalDirection, Color.cyan, 1);
Debug.DrawLine(hullPiece.transform.position, finalDirection, Color.green, 1);
}
}
if (returnStatus)
{
//update booster forces
// booster1.SetCurrentForce(booster1MaxForce);
// booster2.SetCurrentForce(booster2MaxForce);
// booster1.ActivateBooster();
// booster2.ActivateBooster();
}
else
{
//disable all boosters
}
//return returnStatus;
}