a_pos.cxx

Go to the documentation of this file.

/*
Copyright 2000-2019 Pierre SMARS (smars@yuntech.edu.tw)
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 2 of the License, or
(at your option) any later version.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "a_pos.h"
#include <sstream>
//---------------------------------------------------------------------------
const std::string a_pos::help()
{
        std::ostringstream o;
        o << "*****" << std::endl;
        o << "a_pos" << std::endl;
        o << "*****" << std::endl;
        o << "This is a pose class (position + orientation)" << std::endl;
        o << "Commands:" << std::endl;
        o << "--------" << std::endl;
        o << "translate x y z" << std::endl;
        o << "translate a_point" << std::endl;
        o << "rotateX angle (degree)" << std::endl;
        o << "rotateY angle (degree)" << std::endl;
        o << "rotateZ angle (degree)" << std::endl;
        o << "rotate a_quaternion" << std::endl;
        o << "orient a_point (new z axis)" << std::endl;
        return o.str();
}
//---------------------------------------------------------------------------
a_pos::a_pos(const a_mat& m) : a_mat(4,4)
{
        for (int i = 0; i<4; i++)
        {
                for (int j = 0; j<4; j++)
                        (*this)(i,j)= m(i,j); 
        }
}
//---------------------------------------------------------------------------
a_pos & a_pos::translate(double x, double y, double z)
{
        double v[] = {x,y,z};
        for (int i = 0; i<3; i++) 
                (*this)(i,3) -= v[i];
        return (*this);
}
//---------------------------------------------------------------------------
a_pos & a_pos::translate(a_point t)
{
        this->translate(t.x(),t.y(),t.z());
        return (*this);
}
//---------------------------------------------------------------------------
a_pos & a_pos::rotateX(double v)
{
        a_pos m;
        m(1,1) = m(2,2) = cos(v);
        m(2,1) = -sin(v);
        m(1,2) =  sin(v);
        a_mat m2 = m*(*this);
        (*this) = m2;
        return (*this);
}
//---------------------------------------------------------------------------
a_pos & a_pos::rotateY(double v)
{
        a_pos m;
        m(0,0) = m(2,2) = cos(v);
        m(2,0) =  sin(v);
        m(0,2) = -sin(v);
        a_mat m2 = m*(*this);
        (*this) = m2;
        return (*this);
}
//---------------------------------------------------------------------------
a_pos & a_pos::rotateZ(double v)
{
        a_pos m;
        m(0,0) = m(1,1) = cos(v);
        m(1,0) = -sin(v);
        m(0,1) =  sin(v);
        a_mat m2 = m*(*this);
        (*this) = m2;
        return (*this);
}
//---------------------------------------------------------------------------
a_pos & a_pos::orient(a_point zn)
{
        zn.normalise();
        //new z in the direction of reference
        a_point xn,yn;
        a_point x(1,0,0);
        a_point y(0,1,0);
        a_point z(0,0,1);
        if (zn.x()>zn.y()) //zn is closer to x than y
        {
                xn = cross(y,zn).normalise();
                yn = cross(zn,xn).normalise();
        }
        else //zn is closer to y than x
        {
                yn = cross(zn,x).normalise();
                xn = cross(yn,zn).normalise();
        }
        a_pos m;
        m(0,0) = xn.x();
        m(0,1) = xn.y();
        m(0,2) = xn.z();
        m(1,0) = yn.x();
        m(1,1) = yn.y();
        m(1,2) = yn.z();
        m(2,0) = zn.x();
        m(2,1) = zn.y();
        m(2,2) = zn.z();
        (*this) = m;
        return (*this);
}
//---------------------------------------------------------------------------
a_pos & a_pos::rotate(const a_quaternion & q)
{
        double x = q.r();
        double y = q.i();
        double z = q.j();
        double w = q.k();
        a_pos m;
        m(0,0) = 1.-2.*y*y-2.*z*z;
        m(0,1) = 2.*x*y-2.*z*w;
        m(0,2) = 2.*x*z+2.*y*w;
        m(1,0) = 2.*x*y+2.*z*w;
        m(1,1) = 1.-2.*x*x-2.*z*z;
        m(1,2) = 2.*y*z-2.*x*w;
        m(2,0) = 2.*x*z-2.*y*w;
        m(2,1) = 2.*y*z+2.*x*w;
        m(2,2) = 1.-2.*x*x-2.*y*y;
        (*this) = m;
        return (*this);
}