arcsconnected.cpp

Go to the documentation of this file.

#include <fstream>
using namespace std;

#include <arcsconnected.h>


// TOTALLY UNTESTED <TODO>
void arcsconnected::constructPhi0
( 
  doublec phi0, 
  vector< pt2 > const & pts 
)
{
  assert(pts.size()==N+1);
  assert(pts.size()>=2);
  if (pts.size()<2)
    return;

  arc a;
  a.constructPhi0TwoPoints(phi0,pts[0],pts[1]);
  double r0 = a.radius;

  constructR0(r0,pts);
}

void arcsconnected::constructR0
(
  doublec r0, 
  doublec * pts
)
{
  pt2 * v = & constructR0Vec[0];
  pt2 * const vend = v + N + 1;
  doublec *p = pts;
  for ( ; v!=vend; )
  {
    (*v).x = *p;
    ++p;
    (*v).y = *p;
    ++p;
    ++v;
  }

  constructR0(r0,constructR0Vec);

  //assert(false) // UNTESTED ROUTINE<TODO>
}

void arcsconnected::constructR0
( 
  doublec r0, 
  vector< pt2 > const & pts 
)
{
  assert(pts.size()==N+1);
  assert(pts.size()>=2);
  if (pts.size()<2)
    return;

  vi[0].constructRadiusTwoPoints(r0,pts[0],pts[1]);

  double theta;
  for (uint i=1; i<N; ++i)
  {
    pt2c & p0(pts[i]);
    pt2c & p1(pts[i+1]);
    pt2c & c0(vi[i-1].center);

    theta = vi[i-1].phi1;

//cout << SHOW(theta*radtodeg) << endl;

    // Is the new point above the tangent?
    // This is a halfspace test.
    if ( (p0-c0).dot(p1-p0) > 0.0 )
    {
      theta += PI;
      if (theta >= PI*2.0)
        theta -= PI*2.0;
    }

//cout << SHOW(theta*radtodeg) << endl;
      
    vi[i].constructPhi0TwoPoints(theta,pts[i],pts[i+1]);
  }
}


arcsconnected::arcsconnected(uintc nstates, uintc _N)
  : N(_N), cb(nstates,_N)
{
  vi.resize(N);
  constructR0Vec.resize(N+1);
}


void arcsconnected::arcwriter( stringc filename ) const
{
  ofstream targ(filename.c_str(),ios::trunc);

  assert(targ.good()==true);
  if (targ.good()==false)
    return;

  targ << N+1 << endl;
  for (uint i=0; i<N; ++i)
    targ << vi[i].p0 << endl;
  targ << vi[N-1].p1 << endl;

  targ << N << endl;
  for (uint i=0; i<N ; ++i)
    targ << i << " " << i+1 << " " << vi[i].radius << endl;

}

void arcsconnected::sumdisttoarc
( 
  double & sum,
  vector< pt2 > const & qi 
) const 
{
  uintc W = qi.size();

  sum=0.0;
  double val;
  double val2;
  for (uint i=0; i<N; ++i)
  {
    arc const & a(vi[i]);
    a.distanceSquared(val,qi[0]);

    for (uint k=0; k<W; ++k)
    {
      a.distanceSquared(val2,qi[k]);
      if (val2<val)
        val=val2;
    }

    sum += val;
  }
}