a_segment.cxx

Go to the documentation of this file.

/*
         Copyright 2000-17 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_segment.h"
#include <vector>
#include <algorithm>
#include <math.h>
#include <sstream>
 
#include "a_debug.h"
 
//---------------------------------------------------------------------------
const std::string a_segment::help()
{
        std::ostringstream o;
        o << "**********" << std::endl;
        o << "a_segment:" << std::endl;
        o << "**********" << std::endl;
        o << "This is a 'segment' class." << std::endl;
        o << std::endl;
        o << "Create a segment:" << std::endl;
        o << " a_point a 0 0 0" << std::endl;
        o << " a_point b 1 0 0" << std::endl;
        o << " a_segment s1 a b" << std::endl;
        o << " set s2 [a_segment [new_a_point 0 0 0] [new_a_point 0 1 0] ]" << std::endl;
        o << std::endl;
        o << "Commands:" << std::endl;
        o << "p1, p2, p1 _pt, p2 _pt: returns or set apices." << std::endl;
        o << "c: returns centre."<< std::endl;
        o << "vec: returns p2-p1."<< std::endl;
        o << "dir: returns direction (normalised)."<< std::endl;
        o << "translate _x _y _z: move segment."<< std::endl;
        o << "rotate _xaxis _yaxis _zaxis: rotate segment; 3 points define new axis."<< std::endl;
        o << "length: returns length of segment." << std::endl;
        o << "project _pt: project a point on the segment."<< std::endl;
        o << "closest _pt: returns pt closest to _pt belonging to segment."<< std::endl;
        o << "closestl _pt: returns pt closest to _pt on line defined by segment."<< std::endl;
        o << "dist _pt: closest distance to _pt."<< std::endl;
        o << "dist _seg: closest distance to _seg."<< std::endl;
        o << "distl _pt: closest distance between _pt and line defined by segment."<< std::endl;
        o << "distl _seg: closest distance between lines defined by segments."<< std::endl;
        o << "intersect _seg _m1* _m2*: intersection between to segments; returns a segment between the closest point between the two lines (l1,l2) defined by the segment; also returns the relative position (0-1 if inside segments) of its apices on l1 and l2." << std::endl;
        o << "shortest _seg _m1* _m2*: shortest distance between two lines defined by segments; returns a segment between the closest point between the two segments; also returns the relative position of its apices on the input segments."<< std::endl;
        o << "print: print segment parameters (coordinates of apices)."<< std::endl;
        o << std::endl;
        o << a_geom_base::help();
        return o.str();
}
//---------------------------------------------------------------------------
a_segment& a_segment::rotate(const a_point& x_axis, const a_point& y_axis, const a_point& z_axis)
{
        p1_.rotate(x_axis,y_axis,z_axis); 
        p2_.rotate(x_axis,y_axis,z_axis); 
        return *this;
}
//---------------------------------------------------------------------------
bool a_segment::operator==(const a_segment& s)
{
        return ((p1_ == s.p1())&&(p2_ == s.p2()));
}
//---------------------------------------------------------------------------
a_segment & a_segment::operator=(const a_segment& s)
{
        p1_ = s.p1();
        p2_ = s.p2();
        return *this;
}
//---------------------------------------------------------------------------
a_mat_sq a_segment::inertia() const
{
        a_mat_sq a(3);
        a_point d = this->vec();
        a_point c = this->c();
        double l = this->length();
        for (short i=0; i<3; i++)
        {
                double li = d[i];
                double ci = c[i];
                for (short j=0; j<=i; j++)
                {
                        double lj = d[j];
                        double cj = c[j];
                        double v = l*(li*lj/12.+ci*cj);
                        a(i,j) = v; 
                        a(j,i) = v; 
                }
        }
        return a;
}
//---------------------------------------------------------------------------
double a_segment::project(const a_point v) const
{
        a_point dir = p2_-p1_;
        a_point u = v-p1_;
        return (u*dir)/(dir*dir);
}
//---------------------------------------------------------------------------
a_point a_segment::closestl(const a_point& p) const
{
        a_point dir = p2_-p1_;
        double t = (p-p1_)*dir/dir.sumsq();
        return p1_+t*dir;
}
//---------------------------------------------------------------------------
a_point a_segment::closest(const a_point& p) const
{
        a_point dir = p2_-p1_;
        double t = (p-p1_)*dir/dir.sumsq();
        if (t<0)
                return p1_;
        else if (t>1)
                return p2_;
        else
                return p1_+t*dir;
}
//---------------------------------------------------------------------------
double a_segment::distl(const a_point& p) const
{
        return p.dist(this->closestl(p));
}
//---------------------------------------------------------------------------
double a_segment::dist(const a_point& p) const
{
        return p.dist(this->closest(p));
}
//---------------------------------------------------------------------------
a_segment a_segment::intersect(const a_segment& s, double& m1, double& m2) const
{
        const double eps = 1e-10;
        a_point dor = p1_-s.p1();
        a_point d1 = this->vec();
        if (d1.sumsq() < eps) throw a_segment::null_segment_error();
        a_point d2 = s.vec();
        if (d2.sumsq() < eps) throw a_segment::null_segment_error();
        double a = d1*d1;
        double b = d2*d1;
        double c = d2*d2;
        double d = dor*d1;
        double e = dor*d2;
        double D = a*c-b*b;
        if (fabs(D) < eps) //lines are parallel
        {
                m1 = 0;
                m2 = (b>c ? d/b : e/c);   
        }
        else
        {
                m1 = (b*e-c*d)/D;
                m2 = (a*e-b*d)/D;
        }
        a_point r1 = p1_+m1*d1;
        a_point r2 = s.p1()+m2*d2;
        return a_segment(r1,r2);
}
//---------------------------------------------------------------------------
class pack
{
        public:
                pack(int ref, double dist) : ref_(ref), dist_(dist) {}
                int ref_;
                double dist_;
};
bool operator<(const pack& p1, const pack& p2) {return p1.dist_<p2.dist_;};
//---------------------------------------------------------------------------
a_segment a_segment::shortest(const a_segment& s, double& m1, double& m2) const
{
        const double eps = 1e-10;
        a_point dor = p1_-s.p1();
        a_point d1 = p2_-p1_;
        if (d1.sumsq() < eps) throw a_segment::null_segment_error();
        a_point d2 = s.p2()-s.p1();
        if (d2.sumsq() < eps) throw a_segment::null_segment_error();
        double sd1 = d1*d1;
        double sd1d2 = d2*d1;
        double sd2 = d2*d2;
        double sd1dor = dor*d1;
        double sd2dor = dor*d2;
        double D = (sd1*sd2-sd1d2*sd1d2);//surface of the triangle of the 2 directions
        if (D < eps) // the lines are almost parallel
        {
                m1 = 0;
                m2 = sd2dor/sd2;
        }
        else // get the closest points on the infinite lines
        {
                m1 = (sd1d2*sd2dor-sd2*sd1dor) / D;
                m2 = (sd1*sd2dor-sd1d2*sd1dor) / D;
        }
        if ((m1<0)||(m1>1)||(m2<0)||(m2>1))
        { 
                std::vector<pack> li;
                pack p1(1,this->dist(s.p1()));
                li.push_back(p1);
                pack p2(2,this->dist(s.p2()));
                li.push_back(p2);
                pack p3(3,s.dist(p1_));
                li.push_back(p3);
                pack p4(4,s.dist(p2_));
                li.push_back(p4);
 
                std::sort(li.begin(),li.end());
                int ref = li[0].ref_;
                a_point pn;
                switch (ref)
                {
                        case 1 :
                                pn = this->closest(s.p1());
                                m1 = ((pn-p1_)*d1)/sd1;
                                m2 = 0;
                                return a_segment(pn,s.p1());
                        case 2 :
                                pn = this->closest(s.p2());
                                m1 = ((pn-p1_)*d1)/sd1;
                                m2 = 1;
                                return a_segment(pn,s.p2());
                        case 3 :
                                pn = s.closest(p1_);
                                m1 = 0;
                                m2 = ((pn-s.p1())*d2)/sd2;
                                return a_segment(p1_,pn);
                        case 4 :
                                pn = s.closest(p2_);
                                m1 = 1;
                                m2 = ((pn-s.p1())*d2)/sd2;
                                return a_segment(p2_,pn);
                }
        }
        a_point r1 = p1_+m1*d1;
        a_point r2 = s.p1()+m2*d2;
        return a_segment(r1,r2);
}
/*
         a_segment a_segment::shortest(const a_segment& s, double& m1, double& m2) const
         {
         const double eps = 1e-10;
         a_point dor = p1_-s.p1();
         a_point d1 = p2_-p1_;
         if (d1.sumsq() < eps) throw a_segment::null_segment_error();
         a_point d2 = s.p2()-s.p1();
         if (d2.sumsq() < eps) throw a_segment::null_segment_error();
         bool inv = false;
         if (d1.sumsq() < d2.sumsq())
         {
         a_point di = d1;
         d1 = d2;
         d2 = di;
         dor = -dor;
         inv = true;
         }
         std::cerr << "d1 " << d1 << std::endl;
         std::cerr << "d2 " << d2 << std::endl;
         std::cerr << "dor " << dor << std::endl;
 
         double a = d1*d1;
         double b = d2*d1;
         double c = d2*d2;
         double d = dor*d1;
         double e = dor*d2;
         double D = a*c-b*b;
         double    sN, sD = D;      // sc = sN / sD, default sD = D >= 0
         double    tN, tD = D;      // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < eps) // the lines are almost parallel
{
sN = 0.0;
tN = e;
tD = c;
std::cerr << "13 tN " << tN << "   tD " << tD << std::endl;
std::cerr << "sD " << sD << std::endl;
}
else // get the closest points on the infinite lines
{                
std::cerr << "14" << std::endl;
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0) // sc < 0 => the s=0 edge is visible
{       
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD) // sc > 1 => the s=1 edge is visible
{  
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0) // tc < 0 => the t=0 edge is visible
{           
std::cerr << "15" << std::endl;
tN = 0.0;
// recompute sc for this edge
if (-d < 0)
sN = 0.0;
else if (-d > a)
sN = sD;
else 
{
sN = -d;
sD = a;
}
}
else if (tN > tD) // tc > 1 => the t=1 edge is visible
{      
        std::cerr << "16" << std::endl;
        tN = tD;
        // recompute sc for this edge
        if ((-d + b) < 0)
                sN = 0;
        else if ((-d + b) > a)
                sN = sD;
        else 
        {
                sN = (-d + b);
                sD = a;
                std::cerr << "a " << a << std::endl;
        }
}
std::cerr << "sD " << sD << "   tD " << tD << std::endl;
std::cerr << "sN " << sN << "   tN " << tN << std::endl;
std::cerr << "*this " << *this << std::endl;
std::cerr << "s " << s << std::endl;
if ((sD==0)||(tD==0))
{
        std::cerr << "error" << std::endl;
        std::exit(-1);
}
// finally do the division to get sc and tc
m1 = sN / sD;
m2 = tN / tD;
 
std::cerr << "18" << std::endl;
if (inv)
{
        a_point r1 = s.p1()+m1*d1;
        a_point r2 = p1_+m2*d2;
        double m = m1;
        m1 = m2;
        m2 = m;
        return a_segment(r2,r1);
}
else
{
        a_point r1 = p1_+m1*d1;
        a_point r2 = s.p1()+m2*d2;
        return a_segment(r1,r2);
}
}*/
//---------------------------------------------------------------------------
double a_segment::distl(const a_segment& s) const
{
        double m1,m2;
        a_segment i = this->intersect(s,m1,m2);
        return i.length();
}
//---------------------------------------------------------------------------
double a_segment::dist(const a_segment& s) const
{
        double m1,m2;
        a_segment i = this->shortest(s,m1,m2);
        return i.length();
}
//***************************************************************************
//---------------------------------------------------------------------------
void a_segment::read(std::istream &in)
{
        in >> p1_;
        in >> p2_;
}
//---------------------------------------------------------------------------
void a_segment::write(std::ostream &o) const
{
        o << this->p1() << " ";
        o << this->p2() << " ";
}