a_pos2.cxx

Go to the documentation of this file.

/*
Copyright 2000-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_pos2.h"
//---------------------------------------------------------------------------
a_pos2::a_pos2(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); 
        }
}
//---------------------------------------------------------------------------
void a_pos2::compute_rot()
{
        // rz*ry*rx
        double a[] = {cos(ax_), sin(ax_), cos(ay_), sin(ay_), cos(az_), sin(az_)};
        (*this)(0,0) = a[2]*a[4];
        (*this)(0,1) = a[1]*a[3]*a[4] + a[0]*a[5];
        (*this)(0,2) = -a[0]*a[3]*a[4] + a[1]*a[5];
        (*this)(1,0) = -a[2]*a[5];
        (*this)(1,1) = -a[1]*a[3]*a[5] + a[0]*a[4];
        (*this)(1,2) = a[0]*a[3]*a[5] + a[1]*a[4];
        (*this)(2,0) = a[3];
        (*this)(2,1) = -a[1]*a[2];
        (*this)(2,2) = a[0]*a[2];
}
//---------------------------------------------------------------------------
bool angle_egal(double a, double b)
{
        double c = cos(a)-cos(b);
        double s = sin(a)-sin(b);
        return c*c+s*s < 0.0001;
}
//---------------------------------------------------------------------------
void a_pos2::compute_rot_inv()
{
        const double pi = 3.141592653589793238;
        double f1 = asin((*this)(2,0));
 
        double o1 = asin(-(*this)(2,1)/cos(f1));
        double o2 = -o1;
        double o3 = acos((*this)(2,2)/cos(f1));
        double o4 = -o3;
 
        if (angle_egal(o2,o3)||angle_egal(o2,o4))
                o1 = o2;
 
        double k1 = acos((*this)(0,0)/cos(f1));
        double k2 = -k1;
        double k3 = asin(-(*this)(1,0)/cos(f1));
        double k4 = -k3;
 
        if (angle_egal(k2,k3)||angle_egal(k2,k4))
                k1 = k2;
 
        ax_ = o1;
        ay_ = f1;
        az_ = k1;
}
//---------------------------------------------------------------------------
void a_pos2::compute_or()
{
        (*this)(0,3) = ox_;
        (*this)(1,3) = oy_;
        (*this)(2,3) = oz_;
        (*this)(3,3) = 1;
}
//---------------------------------------------------------------------------
void a_pos2::compute_or_inv()
{
        ox_ = (*this)(0,3);
        oy_ = (*this)(1,3);
        oz_ = (*this)(2,3);
}
//---------------------------------------------------------------------------
std::istream& operator>> (std::istream& i, a_pos2& m)
{
        int i0,j0;
        i >> i0;
        i >> j0;
        if ((i0 != m.maxi_)||(j0 != m.maxj_)) throw a_mat::range_error();
    for (UI j=0; j < m.maxi_; j++)
        {
            for (UI k=0; k < m.maxj_; k++)
                        i >> m(j,k);
        }
        m.compute_or_inv();
        m.compute_rot_inv();
        return i;
}