macierz manipulator::rq(int i)
{
    double kat, pomocnik;
    macierz wynik;
    kat = p[i].zwroc_kat();
    pomocnik = cos(M_PI * kat / 180);
    wynik.wstaw(0, 0, pomocnik);
    wynik.wstaw(1, 1, pomocnik);
    pomocnik = (-1)*sin(M_PI * kat / 180);
    wynik.wstaw(0, 1, pomocnik);
    pomocnik = sin(M_PI * kat / 180);
    wynik.wstaw(1, 0, pomocnik);
    return wynik;
}

wektor manipulator::translacja(int i)
{
    wektor wynik;
    double p_x, p_y;
    p_x = p[i].zwroc_dlugosc();
    //cout << p_x << "--<<-- p_x !!!\n";
    p_y = 0;
    wynik.wstaw(p_x, p_y);
    return wynik;
}