Segment.cpp

Go to the documentation of this file.

#include <Segment.h>


Segment::Segment(){}
Segment::Segment(const V_3D& start_point,const V_3D& end_point)
{x0=start_point;x1=end_point;}
Segment::Segment(const Segment& s){x0=s.x0;x1=s.x1;}

Segment::~Segment(){destroy();}

int Segment::set(const V_3D _x0,const V_3D _x1){x0=_x0;x1=_x1;return 0;}
const V_3D& Segment::get_start() const{return x0;}
const V_3D& Segment::get_last() const{return x1;}

int Segment::flip(){V_3D temp=x0;x0=x1;x1=temp;return 0;}
int Segment::destroy(){x0.set(0.0,0.0,0.0);x1.set(0.0,0.0,0.0);return 0;}

V_3D Segment::unit_vector() const
{
  V_3D u=x1-x0;
  u = u.normalized();
  return u;
}

double Segment::length() const
{return (x1-x0).norm();}

V_3D Segment::closest_point(const V_3D& x) const
{
  V_3D u=unit_vector();
  double proj=0.0;
  proj = (x-x0).dot(u);
  if(proj<0)
    return x0;
  else if(proj>length())
    return x1;
  else
    return x0+proj*u;
}

double Segment::distance_to_point(const V_3D& x) const
{return (x-closest_point(x)).norm();}



Segment Segment::closest_to_segment(const Segment& s) const
{

  // 1. Search the closest position between the two lines
  //    = line perpendicular to both segments
  //****************************************************//


  // useful vectors
  V_3D u1,u2;
  V_3D A12;

  u1 = get_last()-get_start();
  u2 = s.get_last()-s.get_start();
  A12 = s.get_start()-get_start();
  
  double det=0.0;
  det = powf(u1.dot(u2),2.0) - u1.dot(u1)*u2.dot(u2);

  // check if the lines are parallel
  double epsilon=0.00001;
  if(fabs(det)<epsilon)
    {printf("Error in closest_point_in_segment, lines are almost parallel\n");exit(-1);}

  // distance of the intersection
  double s1=0.0,s2=0.0;
  s1 = 1/det*(-(A12.dot(u1))*(u2.dot(u2)) + (A12.dot(u2))*(u1.dot(u2)) );
  s2 = 1/det*(-(A12.dot(u1))*(u2.dot(u1)) + (A12.dot(u2))*(u1.dot(u1)) );

  V_3D A1=get_start(),A2=s.get_start();
  V_3D P1;P1 = A1 + s1*u1;
  V_3D P2;P2 = A2 + s2*u2;

  // check if the points (P1,P2) are inside the two segments
  double dot_p1=0.0,dot_p2=0.0;
  dot_p1 = (P1-A1).dot(u1);
  dot_p2 = (P2-A2).dot(u2);

  Segment seg;
  int is_valid_p1=0,is_valid_p2=0;
  if(dot_p1>=0 && dot_p1<=u1.dot(u1))
    is_valid_p1=1;
  if(dot_p2>=0 && dot_p2<=u2.dot(u2))
    is_valid_p2=1;

  if(is_valid_p1==1 && is_valid_p2==1)
    seg.set(P1,P2);
  else
    {
      // else the closest points are at the extremities
      // 9 possibilities in this cases:
      V_3D y0[3]={A1,A1+u1,P1};
      V_3D y1[3]={A2,A2+u2,P2};
      double min_dist=99999.99;
      double current_dist=0.0;
      int min_k1=-1,min_k2=-1;
      for(int k1=0;k1<3;k1++)
        for(int k2=0;k2<3;k2++)
          {
            current_dist=(y0[k1]-y1[k2]).norm();
            if(current_dist<min_dist)
              if(k1!=2 || (k1==2 && is_valid_p1==1))
                if(k2!=2 || (k2==2 && is_valid_p2==1))
                  {min_dist=current_dist;min_k1=k1;min_k2=k2;}
          }

      // 9 cases
           if(min_k1==0 && min_k2==0) {seg.set(A1,A2);}
      else if(min_k1==0 && min_k2==1) {seg.set(A1,A2+u2);}
      else if(min_k1==0 && min_k2==2) {seg.set(A1,P2);}

      else if(min_k1==1 && min_k2==0) {seg.set(A1+u1,A2);}
      else if(min_k1==1 && min_k2==1) {seg.set(A1+u1,A2+u2);}
      else if(min_k1==1 && min_k2==2) {seg.set(A1+u1,P2);}

      else if(min_k1==2 && min_k2==0) {seg.set(P1,A2);}
      else if(min_k1==2 && min_k2==1) {seg.set(P1,A2+u2);}
      else if(min_k1==2 && min_k2==2) {seg.set(P1,P2);}

    }
  
  return seg;


}

V_3D Segment::operator()(int index) const
{
  if(index!=0 && index!=1){printf("Error in Segment(%d), index must be 0 or 1\n",index);}
  if(index==0) return x0;
  return x1;
}
V_3D& Segment::operator()(int index)
{
  if(index!=0 && index!=1){printf("Error in Segment(%d), index must be 0 or 1\n",index);}
  if(index==0) return x0;
  return x1;
}
V_3D Segment::operator[](int index) const
{
  if(index!=0 && index!=1){printf("Error in Segment[%d], index must be 0 or 1\n",index);}
  if(index==0) return x0;
  return x1;
}
V_3D Segment::operator[](int index)
{
  if(index!=0 && index!=1){printf("Error in Segment[%d], index must be 0 or 1\n",index);}
  if(index==0) return x0;
  return x1;
}

double Segment::distance_to_segment(const Segment& s) const
{Segment closest = closest_to_segment(s);return closest.length();}

ostream& operator << (ostream& flux, const Segment& s)
{flux<<"["<<s.get_start()<<" ; "<<s.get_last()<<"]";return flux;}

V_3D Segment::plane_intersection(const V_3D& n,const V_3D& _x0,int *type) const
{

  double epsilon=0.0001;
  //first check if the segment is parallel to the plane
  V_3D AB=x1-x0;
  if(fabs(AB.dot(n))<epsilon)
    {
      //plane is \\ to the segment
      //now test if the plane pass through the line
      if(n.dot(_x0-x0)<epsilon)//pass through
        {
          *type=3;
          return V_3D(-1,-1,-1);
        }
      else//no intersection
        {
          *type=0;
          return V_3D(-1,-1,-1);
        }
    }
  
  //We are sure that the plane is not \\ to the segment
  double t=n.dot(_x0-x0)/AB.dot(n);

  V_3D intersection=x0+t*AB;



  //check if the intersection is inside the segment
  if(t>=0 && t<=1)
    {
      //check if its close to a vertex or not
      if( (intersection-x0).norm()<epsilon || (intersection-x1).norm()<epsilon)
        *type=2;
      else
        *type=1;
      return intersection;
    }
  
  //else outside the segment => no intersection
  *type=0;
  return intersection;
    
}