a_twist.cxx

Go to the documentation of this file.

/*
Copyright 2009-2014 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 <math.h>
#include "a_debug.h"
#include "a_twist.h"
 
void a_twist::set(const a_point& p, a_point d, double t, double r)
{
        //d cannot be null, it is a direction
        if (d.norm()==0)
                return;
        d.normalise();
        p1_ = d*r;//rotation is the same
        a_point d0 =d*t;
        double lp = p.norm();
        if (lp==0.)//point of application is at origin
        {
                p2_ = d0;
                return;
        }
        //compute transformation of origin
        //(is also opposite of transformation of p around origin)
        //angle between p and d
        //component of p along d (not affected by rotation)
        a_point pl = (p*d)*d;
        //component of p perpendicular to d (affected by rotation)
        a_point pp = p-pl;
        //length of pp
        double spp=pp.norm();
        //unit vector // to pp
        a_point u = pp/spp;
        //unit vector normal to p and pp
        a_point v = cross(d,u);
        //new (rotated) pp
        a_point ppn=(u*cos(r)+v*sin(r))*spp;
        //translation of p
        a_point tp = pp-ppn;
        //translation is opposite
        p2_ = tp+d0;
//VE(p2_)
        return; 
}
//---------------------------------------------------------------------------
void a_twist::set(double x, double y, double z, double dx, double dy, double dz, double t, double r)
{
        a_point p(x,y,z);
        a_point d(dx,dy,dz);
        this->set(p,d,t,r);
}
//---------------------------------------------------------------------------
// d:(dx, dy, dz) is a screw applied at point p:(x, y, z) (length is not important)
// t is displacement along d
// r is a rotation around d applied at p (radian)
// p_1 is the equivalent rotation centered at the origin: applied first
// p_2 is the equivalent displacement: applied second
//---------------------------------------------------------------------------
a_twist::a_twist(double x, double y, double z, double dx, double dy, double dz, double t, double r)
{
        a_point p(x,y,z);
        a_point d(dx,dy,dz);
        this->set(p,d,t,r);
}
//---------------------------------------------------------------------------
a_twist::a_twist()
{
        a_point p(0.,0.,0.);
        a_point d(1.,0.,0.);
        this->set(p,d,0.,0.);
}
//---------------------------------------------------------------------------
a_twist::a_twist(const a_point& p, a_point d, double t, double r)
{
        this->set(p,d,t,r);
}
//---------------------------------------------------------------------------
a_point a_twist::operator* (a_point p) const
{
        a_point p1 = p1_;
        a_point p2 = p2_;
        double a = p1.norm(); //angle of rotation
        //if no rotation, it is just a translation
        if (a==0)
                return p+p2;
        a_point ar = p1_/a; //axis of rotation
        a_point pl = ar*(ar*p);//part of p not affected by rotation
        a_point pr = p-pl;//part of p affected by rotation
        double lpr = pr.norm();//radius to be rotated
        //if point on the axis of rotation, just a translation
        if (lpr==0.)
                return p+p2;
        a_point prx = pr/lpr;//x axis perp. to axis of rot.
        a_point pry = cross(ar,pr).normalise();//y axis perp. to axis of rot.
        prx *= lpr*cos(a);
        pry *= lpr*sin(a);
        a_point prn = prx+pry;//radius rotated
        return p2+pl+prn;
}
//---------------------------------------------------------------------------
const std::string a_twist::help()
{
        std::ostringstream o;
        o << "*********" << std::endl;
        o << "a_twist:" << std::endl;
        o << "*********" << std::endl;
        o << "This class is used to model a twist: rotation + translation" << std::endl;
        o << "Commands:" << std::endl;
        o << "--------" << std::endl;
        o << "a_twist T: creates the identity twist" << std::endl;
        o << "a_twist T 'p' 'd' 't' 'r': creates a twist applied at 'p', translating 't' and rotating of angle 'r' along direction 'd'" << std::endl;
        o << "a_twist T 'x' 'y' 'z' 'dx' 'dy' 'dz' ('r'): creates a twist applied at 'x,y,z', translating 'dx,dy,dz' while, possibly, rotating of angle 'r'" << std::endl;
        o << "" << std::endl;
        o << "set 'p' 'd' 't' 'r': set twist applied at 'p', translating 't' and rotating of angle 'r' along direction 'd'" << std::endl;
        o << "set 'x' 'y' 'z' 'dx' 'dy' 'dz' ('r'): set twist applied at 'x,y,z', translating 'dx,dy,dz' while, possibly, rotating of angle 'r'" << std::endl;
        o << "reset: reset to an identity twist" << std::endl;
        o << "dx: gets translation along x" << std::endl;
        o << "dy: gets translation along y" << std::endl;
        o << "dz: gets translation along z" << std::endl;
        o << "d: gets translation norm" << std::endl;
        o << "rx: gets rotation about x" << std::endl;
        o << "ry: gets rotation about y" << std::endl;
        o << "rz: gets rotation about z" << std::endl;
        o << "r: gets rotation norm" << std::endl;
        o << "quaternion: gets quaternion of transformation (a_quaternion)" << std::endl;
        o << "matrix: gets matrix of transformation (a_mat_sq)" << std::endl;
        o << "translate 'p': translates of 'p'" << std::endl;
        o << "translate 'x' 'y' 'z': idem" << std::endl;
        o << "rotate 'p' 'dir' 'v': rotate of an angle 'v' around an axe of rotation passing by 'p' and with direction 'dir'" << std::endl;
        o << "'twist' * 'p': gets a new point, transforming point 'p' using the twist 'twist'" << std::endl;
        return o.str();
}
//---------------------------------------------------------------------------
void a_twist::reset()
{
        p1_.set(0.,0.,0.);
        p2_.set(0.,0.,0.);
}
//---------------------------------------------------------------------------
void a_twist::translate(const a_point& d)
{
        p2_ += d;
}
//---------------------------------------------------------------------------
a_quaternion a_twist::quaternion() const
{
        a_point n = this->p1();
        double r = n.norm();
        n.normalise();
        double c = cos(r/2.);
        double s = sin(r/2.);
        a_quaternion q(c,n.x()*s,n.y()*s,n.z()*s);
        return q;
}
//---------------------------------------------------------------------------
void a_twist::quaternion(a_quaternion q)
{
        a_point n(q.i(),q.j(),q.k());
        n.normalise();
        double a=q.r();
        if (a<-1.) a=-1.;
        else if (a>1.) a=1.;
        double r = 2*acos(a);
        p1_ = r*n;      
}
//---------------------------------------------------------------------------
a_mat_sq a_twist::matrix() const
{
//method not yet checked !!
        a_point n = this->p1();
        double r = n.norm();
        double c = cos(r);
        double s = sin(r);
        double cc= 1-c;
        double x = n.x();
        double y = n.y();
        double z = n.z();
        a_mat_sq m(3);
        m(0,0) = x*x*cc+c;      m(0,1) = y*x*cc-z*s;    m(0,2) = z*x*cc+y*s;
        m(1,0) = x*y*cc+z*s;    m(1,1) = y*y*cc+c;      m(1,2) = z*y*cc-x*s;
        m(2,0) = x*z*cc-y*s;    m(2,1) = y*z*cc+x*s;    m(2,2) = z*z*cc+c;
VE(m)
        return m;
}
//---------------------------------------------------------------------------
void a_twist::matrix(const a_mat_sq& mat)
{
//method not yet checked !!
        double r = acos((mat(0,0)+mat(1,1)+mat(2,2)-1)/2.);
        double x = mat(2,1)-mat(1,2);
        double y = mat(0,2)-mat(2,0);
        double z = mat(1,0)-mat(0,1); 
        a_point p(x,y,z);
        p.normalise();
        p1_ = p;
}
//---------------------------------------------------------------------------
void a_twist::rotate(const a_point& pt, const a_point& dir, double r)
{
        //technique: add the rotation + be sure than pt does not move
        //1. add rotations
        a_quaternion q1 = this->quaternion();
        a_point o(0.,0.,0.);
        a_twist t(o,dir,0.,r);
        a_quaternion q2 = t.quaternion();
        a_quaternion q3 = q2*q1;
        this->quaternion(q3);
        //2. calculate new translation
        a_point dp = pt-p2_; //effect of first rotation on pt
        a_quaternion q4(0.,dp.x(),dp.y(),dp.z());
        a_quaternion q2i = q2;
        q2i.invert();
        a_quaternion q5 = q2*q4*q2i;
        p2_.x(pt.x()-q5.i());
        p2_.y(pt.y()-q5.j());
        p2_.z(pt.z()-q5.k());
}
//***************************************************************************
//---------------------------------------------------------------------------
a_point operator* (a_point p, const a_twist& t)
{
        return t*p;
}