Exemplo n.º 1
0
    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);
            }
        }
    }
Exemplo n.º 2
0
    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;
    }