posregcmu.cpp

#include "posregcmu.h"
#include <math.h>
#include "mymath.h"

PosRegCmu::PosRegCmu()
{
}


PosRegCmu::~PosRegCmu()
{
}

int* PosRegCmu::CMUnited(double v, double aktPos[], double nextPos[], double nextPos2[])
        {
            double thetaD, theta, alfa, d, fi, delta, t, r;
            const int c = 5;
            double x1, x2, y1, y2;
            int rychlosti[4];// = new int[4];

            x1 = nextPos[0] - aktPos[0];
            y1 = nextPos[1] - aktPos[1];

            x2 = nextPos2[0] - nextPos[0];
            y2 = nextPos2[1] - nextPos[1];

            fi = aktPos[2];

            d = sqrt(pow(nextPos[0] - aktPos[0], 2) + (pow(nextPos[1] - aktPos[1], 2)));


            alfa = acos((x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) * sqrt(x2 * x2 + y2 * y2)));
            if ((x1 > 0) && (y1 < 0)) alfa = 2 * M_PI - alfa;



            theta = acos(x1 / (sqrt(x1 * x1 + y1 * y1)));
            if ((x1 > 0) && (y1 < 0)) theta = 2 * M_PI - theta;

            if ((x1 < 0) && (y1 < 0)) theta = 2 * M_PI - theta;

            thetaD = theta + min(alfa, atan(c / d));
            delta = thetaD - fi;
            t = pow(cos(delta), 2) * signum(cos(delta));
            r = pow(sin(delta), 2) * signum(sin(delta));





            //konvertim na byte, potencialni zdroj pruseru

            double a = (1 * (t - r));
            double b = (1 * (t + r));

            if ((a > 0.5) & (b < -0.5)) v = 50;
            if ((a < 0.5) & (b > -0.5)) v = 50;
            a *= v;
            b *= v;


            rychlosti[0] = (int)abs(v * (t - r)); // leve kolo
            rychlosti[1] = (int)abs(v * (t + r)); // prave kolo
            if ((v * (t - r)) < 0) rychlosti[2] = 1;
            if ((v * (t + r)) < 0) rychlosti[3] = 1;


//          sw.WriteLine(aktPos[0].ToString() + ";" + aktPos[1].ToString() + ";" + aktPos[2].ToString() + ";" + a.ToString() + ";" + b.ToString());


            return rychlosti;
        }

---