a_coord.cxx

Go to the documentation of this file.

/*
Copyright 2011-12 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_coord.h"
#include <fstream>
#include <sstream>
#include <string.h>
// .PORTABILITY : ansi C++
 
//***************************************************************************
const double a_coord::pi = 3.141592653589793238;
//member functions
//---------------------------------------------------------------------------
const std::string a_coord::help()
{
        std::ostringstream o;
        o << "*********" << std::endl;
        o << "a_coord:" << std::endl;
        o << "*********" << std::endl;
        o << "This is a local coordinate system class" << std::endl;
        o << std::endl;
        o << "Create a local coordinate system:" << std::endl;
        o << " a_coord c" << std::endl;
        o << "name _name: give a name to the coordinate system" << std::endl;
        o << "name: get the name of the coordinate system" << std::endl;
        o << "reset: reset to the world coordinate system" << std::endl;
        o << "translate _x _y _z: translate the coordinate system (translation in user coordinate)" << std::endl;
        o << "translateW _x _y _z: translate the coordinate system (translation in world coordinates)" << std::endl;
        o << "scale _x _y _z: scale the coordinate system" << std::endl;
        o << "scale _f: scale the coordinate system" << std::endl;
        o << "rad: set angle unit to radian (default)" << std::endl;
        o << "deg: set angle unit to degrees" << std::endl;
        o << "mode: get the angle mode" << std::endl;
        o << "mode _val: set the angle mode" << std::endl;
        o << "rotateX _val: rotate around local x axis" << std::endl;
        o << "rotateY _val: rotate around local y axis" << std::endl;
        o << "rotateZ _val: rotate around local z axis" << std::endl;
        o << "rotateXW _val: rotate around world x axis" << std::endl;
        o << "rotateYW _val: rotate around world y axis" << std::endl;
        o << "rotateZW _val: rotate around world z axis" << std::endl;
        o << "orient _origin _x _y: place the coordinate system with origin in _origin, x axis along _x and y axis in the plane defined by _x and _y" << std::endl;
        o << "w2l _a_point: convert world to local coordinates" << std::endl;
        o << "l2w _a_point: convert local to world coordinates" << std::endl;
        o <<  std::endl;
        o << a_coord_base::help();
        return o.str();
}
//-a_coord------------------------------------------------------------------
a_coord::a_coord() : a_coord_base(), radian_(true), name_("no name")
{
        for (int i=0; i<4; i++) (*this)(i,i) = 1;
}
//-a_coord------------------------------------------------------------------
a_coord::a_coord(const std::string& aname) : a_coord_base(), radian_(true), name_("no name")
{
        for (int i=0; i<4; i++) (*this)(i,i) = 1;
}
//-a_coord------------------------------------------------------------------
a_coord::a_coord(const a_coord& c) : a_coord_base(c), radian_(c.mode()), name_(c.name())
{
}
//-~a_coord-----------------------------------------------------------------
a_coord::~a_coord()
{
//      a_mat_sq::~a_mat_sq();
}
//---------------------------------------------------------------------------
void a_coord::reset()
{
        this->settoI();
}
//---------------------------------------------------------------------------
void a_coord::mat(a_mat_sq& r) const
{
        if (r.dim()!=4) 
                throw a_mat::range_error();
        for (int i=0; i<4; i++)
        {
                for (int j=0; j<4; j++)
                        r(i,j) = (*this)(i,j);
        }
}
//---------------------------------------------------------------------------
void a_coord::mat(const a_mat_sq& r)
{
        if (r.dim()!=4) 
                throw a_mat::range_error();
        for (UI i=0; i<4; i++)
        {
                for (UI j=0; j<4; j++)
                        (*this)(i,j) = r(i,j);
        }
}
//---------------------------------------------------------------------------
void a_coord::R(a_mat_sq& r) const
{
        if (r.dim()!=3) 
                throw a_mat::range_error();
        r = this->sub_matrix(3,3);
}
//---------------------------------------------------------------------------
void a_coord::R(const a_mat_sq& r)
{
        if (r.dim()!=3) 
                throw a_mat::range_error();
        for (int i=0; i<3; i++)
        {
                for (int j=0; j<3; j++)
                        (*this)(i,j) = r(i,j);
        }
}
//---------------------------------------------------------------------------
void a_coord::T(a_mat_c& t) const
{
        if (t.maxi()!=3) 
                throw a_mat::range_error();
        for (int i=0; i<3; i++)
                t(i) = (*this)(i,3);
}
//---------------------------------------------------------------------------
void a_coord::T(const a_mat_c& t)
{
        if (t.maxi()!=3) 
                throw a_mat::range_error();
        for (int i=0; i<3; i++)
                (*this)(i,3) = t(i);
}
//-translate-----------------------------------------------------------------
void a_coord::translate(double dx, double dy, double dz)
{
        (*this)(0,3) -= dx;
        (*this)(1,3) -= dy;
        (*this)(2,3) -= dz;
}
//-translate-----------------------------------------------------------------
void a_coord::translateW(double dx, double dy, double dz)
{
        double t[] = {dx,dy,dz};
        for (int i=0; i<3; i++)
        {
                for (int j=0; j<3; j++)
                        (*this)(i,3) -= t[j]*(*this)(i,j);
        }
}
//-scale---------------------------------------------------------------------
void a_coord::scale(double sx, double sy, double sz)
{
        for (int i=0; i<3; i++) (*this)(i,0) /= sx;
        for (int i=0; i<3; i++) (*this)(i,1) /= sy;
        for (int i=0; i<3; i++) (*this)(i,2) /= sz;
}
//-scale---------------------------------------------------------------------
void a_coord::scale(double s)
{
        a_coord::scale(s,s,s);
}
//-rotateX-------------------------------------------------------------------
void a_coord::rotateX(double t)
{
        if (!radian_) t /= 180/a_coord::pi;
        a_mat_sq r(4);
        r(1,1) =  cos(t);
        r(1,2) =  sin(t);
        r(2,1) = -sin(t);
        r(2,2) =  cos(t);
        r(0,0) = 1;
        r(3,3) = 1;
        this->mat(r*(*this));
}
//-rotateY-------------------------------------------------------------------
void a_coord::rotateY(double t)
{
        if (!radian_) t /= 180/a_coord::pi;
        a_mat_sq r(4);
        r(0,0) =  cos(t);
        r(0,2) = -sin(t);
        r(2,0) =  sin(t);
        r(2,2) =  cos(t);
        r(1,1) = 1;
        r(3,3) = 1;
        this->mat(r*(*this));
}
//-rotateZ-------------------------------------------------------------------
void a_coord::rotateZ(double t)
{
        if (!radian_) t /= 180/a_coord::pi;
        a_mat_sq r(4);
        r(0,0) =  cos(t);
        r(0,1) =  sin(t);
        r(1,0) = -sin(t);
        r(1,1) =  cos(t);
        r(2,2) = 1;
        r(3,3) = 1;
        this->mat(r*(*this));
}
//-rotateX-------------------------------------------------------------------
void a_coord::rotateXW(double t)
{
        if (!radian_) t /= 180/a_coord::pi;
        a_mat_sq r(4);
        r(1,1) =  cos(t);
        r(1,2) =  sin(t);
        r(2,1) = -sin(t);
        r(2,2) =  cos(t);
        r(0,0) = 1;
        r(3,3) = 1;
        this->mat((*this)*r);
}
//-rotateY-------------------------------------------------------------------
void a_coord::rotateYW(double t)
{
        if (!radian_) t /= 180/a_coord::pi;
        a_mat_sq r(4);
        r(0,0) =  cos(t);
        r(0,2) = -sin(t);
        r(2,0) =  sin(t);
        r(2,2) =  cos(t);
        r(1,1) = 1;
        r(3,3) = 1;
        this->mat((*this)*r);
}
//-rotateZ-------------------------------------------------------------------
void a_coord::rotateZW(double t)
{
        if (!radian_) t /= 180/a_coord::pi;
        a_mat_sq r(4);
        r(0,0) =  cos(t);
        r(0,1) =  sin(t);
        r(1,0) = -sin(t);
        r(1,1) =  cos(t);
        r(2,2) = 1;
        r(3,3) = 1;
        this->mat((*this)*r);
}
//-orient--------------------------------------------------------------------
void a_coord::orient(const a_mat_c& o0, const a_mat_c& x0, const a_mat_c& py0)
{
        a_mat_c o(3);
        o = o0;
        a_mat_c x(3);
        x = x0;
        a_mat_c py(3);
        py = py0;
        if ((o.maxi()==3)&&(o.maxi()==3)&&(o.maxi()==3))
        {
                translate(o(0),o(1),o(2));
                x  -= o;
                py -= o;
                x.normalise();
                a_mat_c z(3);
                z = cross(x,py);
                z.normalise();
                a_mat_c y(3);
                y = cross(z,x);
                for (UI i = 0; i<3; i++) (*this)(0,i) = x(i);
                for (UI i = 0; i<3; i++) (*this)(1,i) = y(i);
                for (UI i = 0; i<3; i++) (*this)(2,i) = z(i);
        }
        else throw a_mat::compatibility_error();
}
//-orient--------------------------------------------------------------------
void a_coord::orient(const a_point& o, const a_point& x0, const a_point& py0)
{
        a_point x = x0;
        a_point py = py0;
        this->translate(o);
        x -= o;
        x.normalise();
        py -= o;
        a_point z = cross(x,py);
        z.normalise();
        a_point y = cross(z,x);
        for (UI i = 0; i<3; i++) (*this)(0,i) = x[i];
        for (UI i = 0; i<3; i++) (*this)(1,i) = y[i];
        for (UI i = 0; i<3; i++) (*this)(2,i) = z[i];
}
//***************************************************************************
//---------------------------------------------------------------------------
void a_coord::read(std::istream &in)
{
        //skip blank lines      
        std::string  s = "";
        while (strcmp(s.c_str(),"")==0)
        {
                if (!in) return;
                std::getline(in,s);
        }
        //skip spaces
        std::string::size_type ii=s.find_first_not_of(" ");
        std::string::size_type ie=s.find_last_not_of(" ");
        name_ = s.substr(ii,ie);
 
        this->a_mat::read(in);
}
//---------------------------------------------------------------------------
void a_coord::write(std::ostream &o) const
{
        o << this->name() << std::endl;
        this->a_mat::write(o);
}
//---------------------------------------------------------------------------
a_coord a_coord::invert() const
{
        a_coord out;
        std::string name = this->name()+"-1";
        out.name(name);
        //translation = R^-1*T
        for (UI i=0;i<3;i++)
        {
                for (UI j=0;j<3;j++)
                        out(i,3)  -= (*this)(j,i)*(*this)(j,3);
        }
        //transpose the rotation matrix
        for (UI i=0;i<3;i++)
        {
                for (UI j=0;j<3;j++)
                        out(i,j)=(*this)(j,i);
        }
        return out;
}
//---------------------------------------------------------------------------
a_point a_coord::w2l(const a_point& pt) const
{
        a_mat_c pt0(4);
        pt0(0) = pt.x();
        pt0(1) = pt.y();
        pt0(2) = pt.z();
        pt0(3) = 1.;
        a_mat_c ptn = (*this)*pt0;
        ptn /= ptn(3);
        a_point out(ptn(0),ptn(1),ptn(2));
        return out;
}
//---------------------------------------------------------------------------
a_point a_coord::l2w(const a_point& pt) const
{
        a_mat_c pt0(4);
        pt0(0) = pt.x();
        pt0(1) = pt.y();
        pt0(2) = pt.z();
        pt0(3) = 1.;
        a_coord t = this->invert();
        a_mat_c ptn = t*pt0;
        ptn /= ptn(3);
        a_point out(ptn(0),ptn(1),ptn(2));
        return out;
}
//***************************************************************************
//-operator*-----------------------------------------------------------------
a_mat_c operator*(const a_coord & c, const a_mat_c & pt)
{
        if (pt.maxi() == 3)      //non-homogeneous coordinates
        {
                a_mat_c r(3);
                a_mat_c r0(4);
                for (UI i = 0; i<3; i++) r0(i) = pt(i);
                r0(3) = 1;
                double v;
                for (UI i = 0; i < 4; i++)
                {
                        v = 0;
                        for (UI j = 0; j < 4; j++)
                                v += c(i,j)*r0(j);
                        if (i != 3)
                                r(i) = v;
                        else
                                for (UI j = 0; j < 3; j++) r(j) /= v;
                }
                return r;
        }
        else if (pt.maxi() == 4) //homogeneous coordinates
        {
                a_mat_c r(4);
                double v;
                for (UI i = 0; i < 4; i++)
                {
                        v = 0;
                        for (UI j = 0; j < 4; j++)
                                v += c(i,j)*pt(j);
                        if (i != 3)
                                r(i) = v;
                        else
                        {
                                for (UI j = 0; j < 3; j++) r(j) /= v;
                                r(3) = 1;
                        }
                }
                return r;
        }
        else throw a_mat::compatibility_error();
}