a_wrench.cxx

Go to the documentation of this file.

/*
Copyright 2010-2016 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>           // math routines
 
#include "a_wrench.h"
#include "a_debug.h"
 
const double verysmall=1.e-8;
//---------------------------------------------------------------------------
// f:(fx, fy, fz) is a force applied in point p:(x, y, z)
// m is a moment around f applied in p 
// p_1 is the equivalent force applied at the origin
// p_2 is the equivalent moment applied at the origin
//---------------------------------------------------------------------------
const std::string a_wrench::help()
{
        std::ostringstream o;
        o << "*********" << std::endl;
        o << "a_wrench:" << std::endl;
        o << "*********" << std::endl;
        o << "This class is used to model a wrench: force + moment" << std::endl;
        o << "Commands:" << std::endl;
        o << "--------" << std::endl;
        o << "a_wrench W 'p' 'f' ('m'): create a wrench by applying force 'f' at point 'p', possibly with a moment having the same direction of 'f'" << std::endl;
        o << "a_wrench W 'x' 'y' 'z' 'fx' 'fy' 'fz' ('m'): idem above" << std::endl;
        o << "a_wrench W 'p1' 'p2' 'p3' 'w1' 'w2': create a wrench at p1 such that m=0 at p1, torque//normal plane (p1,p2,p3)=0 at p2 & p3, w1 applied between p1 and p2, w2 between p2 and p3" << std::endl;
        o << "" << std::endl;
        o << "set 'p' 'f' ('m'): apply force 'f' at point 'p', possibly with a moment having the same direction of 'f'" << std::endl;
        o << "set 'x' 'y' 'z' 'fx' 'fy' 'fz' ('m'): idem above" << std::endl;
        o << "print: print the six values of the force and moment" << std::endl;
        o << "F: returns the force (vector)" << std::endl;
        o << "fx: returns x component of the force" << std::endl;
        o << "fy: returns y component of the force" << std::endl;
        o << "fz: returns z component of the force" << std::endl;
        o << "f: returns norm of the force" << std::endl;
        o << "M: returns the moment (vector)" << std::endl;
        o << "mx: returns x component of the moment" << std::endl;
        o << "my: returns y component of the moment" << std::endl;
        o << "mz: returns z component of the moment" << std::endl;
        o << "m: returns norm of the moment" << std::endl;
        o << "clear: clear the wrench" << std::endl;
        o << "origin 'a_point': change the origin of the coordinate system" << std::endl;
        o << "d0: point where wrench reduces to a pure force, return origin if f=0" << std::endl;
        o << "torque 'p': gets torque at point 'p' (a_point)" << std::endl;
        o << "torque 'p' 'n': gets torque at 'p', in direction 'n'" << std::endl;
        o << "pierce 'a_plane': find the point of interesction of the wrench with the plane" << std::endl;
        o << "" << std::endl;
        o << "If 'w1' and 'w2' are two wrenches" << std::endl;
        o << "w1 + w1: returns the sum of the two wrenches" << std::endl;
        o << "w1 - w1: returns the difference of the two wrenches" << std::endl;
        o << "-w1: sreturns the inverse of 'w1'" << std::endl;
        return o.str();
}
//---------------------------------------------------------------------------
void a_wrench::set(const a_point& p, a_point f, double m)
{
        p1_ = f;
        a_point m0 = f.normalise()*m;
        p2_ = cross(p,p1_)+m0;
}
//---------------------------------------------------------------------------
void a_wrench::set(double x, double y, double z, double fx, double fy, double fz, double m)
{
        a_point p(x,y,z);
        a_point f(fx,fy,fz);
        this->set(p,f,m);
}
//---------------------------------------------------------------------------
a_wrench::a_wrench(const a_point & p, a_point f, double m)
{
        this->set(p,f,m);
}
//---------------------------------------------------------------------------
a_wrench::a_wrench(double x, double y, double z, double fx, double fy, double fz, double m)
{
        a_point p(x,y,z);
        a_point f(fx,fy,fz);
        this->set(p,f,m);
}
//---------------------------------------------------------------------------
a_wrench::a_wrench(const a_point& p1, const a_point& p2, const a_point& p3, 
                const a_wrench& f1, const a_wrench& f2)
{
        a_point d2 = p2-p1;
        a_point d3 = p3-p1;
        a_point u = d3.normalise();
        a_point n = cross(u,d2).normalise();
        a_point v = cross(n,u).normalise();
        a_wrench ft = f1;
        ft += f2;
        double t2n = f1.torque(p2,n);
//      double t2u = f1.torque(p2,u);
//      double t2v = f1.torque(p2,v);
        double t3n = ft.torque(p3,n);
//      double t3u = ft.torque(p3,u);
//      a_point tt2 = f1.torque(p2);
//      a_point tt3 = ft.torque(p3);
        double du = dist(p1,p3);
        double fv = t3n/du;
        double u2 = d2*u;
        double v2 = d2*v;
        double fu = -(t2n-u2*fv)/v2;
        p1_ = u*fu+v*fv;
        p2_ = cross(p1,p1_);
}
//---------------------------------------------------------------------------
void a_wrench::origin(a_point p)
{
        //force does not change
        //new moment
        p2_ -= cross(p,p1_);
}
//---------------------------------------------------------------------------
a_point a_wrench::pierce(const a_plane& p) const
{
        a_point x1 = this->d0();
        a_point x2 = x1+p1_;
        a_segment s(x1,x2);
        a_point x = p.intersect(s);
        return x;
}
//---------------------------------------------------------------------------
a_point a_wrench::d0() const
{
        //f=0 does not matter where is point of application
        if (this->f()==0.)
                return a_point(0.,0.,0.);
        a_point f = p1_;
        double m = this->m();
        a_point dm = p2_;
        dm.normalise();
        //component of force along torque axis
        a_point fl = (dm*f)*dm;
        //component of force perpendicular to torque, can be used to reduce it
        a_point fp = f-fl;
        double fps = fp.norm();
        //force is parallel to torque (and necessarily passes by origin)
        if (fps < verysmall)
                return a_point(0.,0.,0.);
        fp.normalise();
        return cross(fp,dm)*(m/fps);
}
//---------------------------------------------------------------------------
a_point a_wrench::torque(const a_point& p) const
{
        return cross(p1_,p)+p2_;
}
//---------------------------------------------------------------------------
double a_wrench::torque(const a_point& p, const a_point& n) const
{
        a_point n2 = n;
        n2.normalise();
        return (this->torque(p))*n2;
}