arc.cpp

Go to the documentation of this file.

#include <fstream>
using namespace std;

#include <arcsdef.h>

#include <arc.h>

double arc::zero = 1e-12;

arc::arc()
  : radius(0.0), phi0(0.0), phi1(0.0)
{
}

boolc arc::valid() const
{
  
  if ((p0==p1)==true)
  {
    cout << "error:  " << *this << " has p0 and p1 equal.";
    cout << endl;
    return false;
  }

  if (isStraightLine())
    return true;

/*
  if ( ((p0-p1).dot() <= radius*radius*4.0) == false )
  {
    cout << *this << " failed radius condition." << endl;
    assert(false);
    return false;
  }
*/
  
  return true;
}


boolc arc::isAntiClockwise2() const
{
  assert(isStraightLine()==false);

  double dphi = phi1-phi0;
  if (dphi<0.0)
    dphi += 2.0*PI;

  if (dphi<=PI)
    return true;

  return false;
}

void arc::calculateCenter() 
{
  assert(valid());
  assert(isStraightLine()==false);

  pt2 p2((p0+p1)*0.5);
  pt2 q(p1-p0);
  if (radius<0.0)
    q *= -1.0; 
  pt2 Dq(-q.y,q.x);
  Dq.normalize();
  doublec w = q.distance() * 0.5;
//cout << "w=" << w << endl;
  doublec h = sqrt(radius*radius-w*w);
//cout << "h=" << h << endl;



  center = p2 + Dq*h;

/*
cout << SHOW(p0) << " " << SHOW(p1) << " " << SHOW(center) <<  endl;
cout << SHOW(h) << " " << SHOW(q) << " " << SHOW(Dq) << endl;
*/
}

ostream & operator << (ostream & os, arc const & a)
{
  return os << (string)a;
}

arc::operator string() const
{
  stringstream ss;
  ss << p0 << " " << p1 << " " << radius; 
  return ss.str();
}


void arc::constructRadiusTwoPoints
(
  doublec radius_,
  pt2c & p0_,
  pt2c & p1_
)
{
  radius = radius_;
  p0 = p0_;
  p1 = p1_;

  if (isStraightLine()==true)
    return;

  calculateCenter();

  pt2 g0(p0-center);
  //g0.normalize();
  phi0 = atan2(g0.y,g0.x);
  angleAdjusted(phi0);
  pt2 g1(p1-center);
  //g1.normalize();
  phi1 = atan2(g1.y,g1.x);
  angleAdjusted(phi1);

/*
cout << SHOW(g0) << " " << SHOW(phi0*180./PI) << endl;
cout << SHOW(g1) << " " << SHOW(phi1*180./PI) << endl;
cout << SHOW(phi1-phi0) << " " << SHOW(length()) << endl;
*/
  
}

void arc::constructPhi0TwoPoints
(
  doublec phi0_,
  pt2c & p0_,
  pt2c & p1_
)
{
  p0 = p0_;
  p1 = p1_;
  phi0 = phi0_;
/*
cout << endl << endl;

cout << SHOW(phi0*180.0/PI) << endl; 
cout << SHOW(p0) << endl;
cout << SHOW(p1) << endl;
*/

  pt2c f(cos(phi0),sin(phi0));
  pt2c dp(p0-p1);

  doublec fdotdp(f.dot(dp));
  if ( abs(fdotdp)<zero)
  {
    radius=0.0;
    return;
  }
  

if ( fdotdp< 0.0 )
{
#ifndef NDEBUG
cout << "error:  wrong initial angle" << endl;
#endif
  constructPhi0TwoPoints(phi0+PI,p0,p1);
  return;
}

  radius = dp.dot() / fdotdp * 0.5;
//cout << SHOW(radius) << endl;

#ifndef NDEBUG
if (radius<0.0)
  cout << "error: the radius should be positive." << endl;
#endif
  //center = p0 - f * abs(radius);
  center = p0 - f * radius;





/*
  pt2 c0 = p0 - f * radius;
  center = c0;


cout << SHOW(c0) << endl;
cout << SHOW(c0 + f * radius) << endl;
cout << SHOW(c0 - f * radius) << endl;
cout << SHOW( (p0-c0).distance() ) << endl;
cout << SHOW( (p1-c0).distance() ) << endl;
*/


  pt2 p4(p1-center);
  p4 *= 1.0 / radius;
  phi1 = atan2(p4.y,p4.x);

  if (isAntiClockwise2()==false)
  {

    if (radius<0)
    {
#ifndef NDEBUG
cout << "error:  radius already -ve, this message should not appear" << endl;
#endif
    }
    else
      radius *= -1;
  }

/*

cout << "Verify by finding p2 from phi1." << endl;
  pt2 p5(cos(phi1),sin(phi1));
  p5 *= radius;
  p5 += center;


cout << SHOW(p4) << endl;
cout << SHOW(p4.dot()) << endl;
cout << SHOW(phi1*180.0/PI) << endl;
cout << SHOW(p5) << endl;
*/


}

 
arc::arc
(
  doublec radius_,
  pt2c & p0_,
  pt2c & p1_
)
{
  constructRadiusTwoPoints(radius_,p0_,p1_);
}

doublec arc::length() const
{
  assert(valid()==true);

  if (isStraightLine())
    return (p1-p0).distance();


  // Interpret the arc as convex between points p0 and p1.
  //  Find the acute angle between the points p0 and p1.
  double dphi = abs(phi1-phi0);
  if (dphi>PI)
    dphi = 2.0*PI - dphi;

  return abs(dphi*radius);
}


void arc::arcreader
(
  vector<arc> & v, 
  string const & filename
)
{
  v.clear();

  ifstream targ(filename.c_str());

  assert(targ.good()==true);
  if (targ.good()==false)
    return;
  
  vector< pt2 > pts;

  pt2 p;
  uint k;
  targ >> k;
  for ( uint i=0; i<k; ++i)
  {
    targ >> p;
    pts.push_back(p);
  }

  uint a;
  uint b;
  double radius;

  targ >> k;

  for ( uint i=0; i<k; ++i)
  {
    targ >> a;
    targ >> b;
    targ >> radius;

    assert(a<pts.size());
    assert(b<pts.size());

    v.push_back( arc(radius,pts[a],pts[b]) );
  }
}


void arc::print() const
{
  cout << SHOW(radius) << endl;
  cout << SHOW(p0) << endl;
  cout << SHOW(p1) << endl;
  
  cout << SHOW(center) << endl;
  cout << SHOW(phi0*180.0/PI) << endl;
  cout << SHOW(phi1*180.0/PI) << endl;
  cout << SHOW(isAntiClockwise()) << endl;
  cout << SHOW(isAntiClockwise2()) << endl;
  cout << SHOW(center+pt2(cos(phi0),sin(phi0))*abs(radius) ) << endl;
  cout << SHOW(center+pt2(cos(phi1),sin(phi1))*abs(radius) ) << endl;
  cout << endl;
}


/*
Note that this test first has to find out its
 orientation


*/
void arc::distance( double & val, pt2c & p ) const
{
  pt2 p3(p-center);

  if (isAntiClockwise())
  {
    {
      pt2 z(center-p0);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
//cout << "greater than p0 arm." << endl;
        // Take the distance from the endpoint.
        val = (p-p0).distance();
        return;
      }
    }
    {
      pt2 z(p1-center);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
//cout << "greater than p1 arm." << endl;
        // Take the distance from the endpoint.
        val = (p-p1).distance();
        return;
      }
    }
  }
  else
  {
    {
      pt2 z(center-p1);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
        val = (p-p1).distance();
        return;
      }
    }
    {
      pt2 z(p0-center);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
        val = (p-p0).distance();
        return;
      }
    }
  }

  val = abs(p3.distance() - radius);
  return;
  
} 

void arc::distanceSquared( double & val, pt2c & p ) const
{
  pt2 p3(p-center);

  if (isAntiClockwise())
  {
    {
      pt2 z(center-p0);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
        //val = (p-p0).distanceSquared();
        val = (p-p0).dot();
        return;
      }
    }
    {
      pt2 z(p1-center);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
        //val = (p-p1).distanceSquared();
        val = (p-p1).dot();
        return;
      }
    }
  }
  else
  {
    {
      pt2 z(center-p1);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
        //val = (p-p1).distanceSquared();
        val = (p-p1).dot();
        return;
      }
    }
    {
      pt2 z(p0-center);
      z.rotate90();
      if (z.dot(p3)>0.0)
      {
        //val = (p-p0).distanceSquared();
        val = (p-p0).dot();
        return;
      }
    }
  }

  //val = abs(p3.distanceSquared() - radius*radius);
  val = abs(p3.dot() - radius*radius);
} 

void arc::angleAdjusted(double & angle) const
{ 
  if (angle<0.0) angle += PI*2.0; 
  if (angle>PI*2.0) angle -= PI*2.0;
  
  assert(angle>=0.0);
  assert(angle<=PI*2.0); 
}