cell.C

Go to the documentation of this file.

#ifdef HAVE_CONFIG_H
#include <tumble-conf.h>
#endif /* HAVE_CONFIG_H */

#include <iostream>

#include "cell.h"

#include "datastore.h"
#include "spline.h"
#include "util.h"

using namespace std;

/* Print out the result of each iteration in newton_find */
//#define DEBUG_NEWTON_FIND

#define define_iterators_any(cls, cell, plural, out) \
  cls::cell##_iterator cls::begin_##plural() { \
    return cell##_iterator(super::begin_##plural()); \
  } \
  cls::cell##_const_iterator cls::begin_##plural() const { \
    return cell##_const_iterator(super::begin_##plural()); \
  } \
  cls::cell##_iterator cls::end_##plural() { \
    return cell##_iterator(super::end_##plural()); \
  } \
  cls::cell##_const_iterator cls::end_##plural() const { \
    return cell##_const_iterator(super::end_##plural()); \
  } \
  out *cls::get_any_##cell() { \
    return static_cast<out*>(super::get_any_##cell()); \
  } \
  const out *cls::get_any_##cell() const { \
    return static_cast<const out*>(super::get_any_##cell()); \
  }

#define define_iterators_canon(cls, cell, plural, out) \
  cls::cell##_iterator cls::begin_##plural() { \
    return cell##_iterator(super::begin_##plural()); \
  } \
  cls::cell##_const_iterator cls::begin_##plural() const { \
    return cell##_const_iterator(super::begin_##plural()); \
  } \
  cls::cell##_iterator cls::end_##plural() { \
    return cell##_iterator(super::end_##plural()); \
  } \
  cls::cell##_const_iterator cls::end_##plural() const { \
    return cell##_const_iterator(super::end_##plural()); \
  } \
  out *cls::get_canon_##cell() { \
    return static_cast<out*>(super::get_canon_##cell()); \
  } \
  const out *cls::get_canon_##cell() const { \
    return static_cast<const out*>(super::get_canon_##cell()); \
  }


/*******************************************/
/*                Cell                     */
/*******************************************/

static unsigned long id_counter = 0; 
Cell::Cell() : id(id_counter++)
{
}



/*******************************************/
/*                VertexCell               */
/*******************************************/
VertexCell::VertexCell(PersistantStore& store) : edges_(store)
{
  edges_.reserve(6);
}

VertexCell::~VertexCell()
{
}

void VertexCell::add_edge(EdgeCell *e)
{
  assert(e);
  assert(!has_edge(e));
  edges_.push_back(e);
}

void VertexCell::delete_edge(EdgeCell *e)
{
  assert(e);
  edge_iterator it = find(begin_edges(), end_edges(), e);
  assert(it != end_edges());
  edges_.erase(it);
}

bool VertexCell::has_edge(const EdgeCell *e) const
{
  edge_const_iterator it = find(begin_edges(), end_edges(), e);
  return (it != end_edges());
}

struct hasfacep {
  const FaceCell *f_;
  hasfacep(const FaceCell *f) : f_(f) { }
  bool operator()(const EdgeCell *e) const {
    return e->has_face(f_);
  }
};

bool VertexCell::has_face(const FaceCell *f) const
{
  edge_const_iterator it = find_if(begin_edges(), end_edges(), hasfacep(f));
  return (it != end_edges());
}

/* !\brief Get some edge, doesn't matter which one.  */
const EdgeCell *VertexCell::get_any_edge() const {
  assert(!edges_.empty());
  // return the back in case the call is going to be
  // followed by delete_edge
  return edges_.back();
}
EdgeCell *VertexCell::get_any_edge() {
  assert(!edges_.empty());
  return edges_.back();
}

/*******************************************/
/*                EdgeCell                 */
/*******************************************/

EdgeCell::EdgeCell(PersistantStore& store, VertexCell *v0, VertexCell *v1)
: vertices_(store), faces_(store)
{
  for (int i = 0; i < 2; i++){
    faces_.access(i) = NULL;
  }
  vertices_.access(0) = v0;
  vertices_.access(1) = v1;
}


void EdgeCell::add_face(FaceCell *f)
{
  assert(num_faces() < 2);
  assert(!has_face(f));
  for(int i = 0 ; i < 2; i++) {
    if(faces_[i] == NULL) {
      faces_.access(i) = f;
      return;
    }
  }
  assert(0);
}

void EdgeCell::delete_face(FaceCell *f)
{
  if(f==faces_[0]){
    faces_.access(0) = faces_[1];
    faces_.access(1) = NULL;
  }
  else if(f==faces_[1]){
    faces_.access(1) = NULL;
  }
  else {
    assert(0);
  }
}

bool EdgeCell::has_vertex(const VertexCell *v) const
{
    for(int i=0; i<2; i++) if(v == vertices_[i]) return true;
    return false;
}

bool EdgeCell::has_face(const FaceCell *f) const
{
    for(int i=0; i<2; i++) if(f == faces_[i]) return true;
    return false;
}

int EdgeCell::num_faces() const
{
    int num=0;
    if(faces_[0]) num++;
    if(faces_[1]) num++;
    return num;
}

VertexCell *EdgeCell::get_canon_vertex()
{
  assert(vertices_[0] != NULL);
  return vertices_[0];
}

const VertexCell *EdgeCell::get_canon_vertex() const
{
  assert(vertices_[0] != NULL);
  return vertices_[0];
}

FaceCell *EdgeCell::get_any_face()
{
  assert(faces_[0] != NULL);
  return faces_[0];
}

const FaceCell *EdgeCell::get_any_face() const
{
  assert(faces_[0] != NULL);
  return faces_[0];
}

/*******************************************/
/*              FaceCell                   */
/*******************************************/

FaceCell::FaceCell(PersistantStore& store) : edges_(store)
{
  edges_.reserve(3);
}

FaceCell::~FaceCell()
{
}

void FaceCell::add_edge(EdgeCell *e)
{
  assert(e);
  assert(!has_edge(e));
  edges_.push_back(e);
}

void FaceCell::delete_edge(EdgeCell *e)
{
  edge_iterator it = find(begin_edges(), end_edges(), e);
  assert(it != end_edges());
  edges_.erase(it);
}

struct hasvertexp {
  const VertexCell *v_;
  hasvertexp(const VertexCell *v) : v_(v) { }
  bool operator()(const EdgeCell *e) const {
    return e->has_vertex(v_);
  }
};

bool FaceCell::has_vertex(const VertexCell *v) const
{
  return find_if(begin_edges(), end_edges(), hasvertexp(v)) != end_edges();
}

bool FaceCell::has_edge(const EdgeCell *e) const
{
  return find(begin_edges(), end_edges(), e) != end_edges();
}

const EdgeCell *FaceCell::get_canon_edge() const {
  assert(num_edges() != 0);
  return *begin_edges();
}

EdgeCell *FaceCell::get_canon_edge() {
  assert(num_edges() != 0);
  return *begin_edges();
}

/*******************************************/
/*              BoundaryVertex             */
/*******************************************/
BoundaryVertex::BoundaryVertex(PersistantStore& store, const ControlPoint& cp)
  : VertexCell(store), bezier_vertex_(NULL), cp_(cp), fixed_(FixedNone)
{
}

void BoundaryVertex::increase_fixed(Movement fixed)
{
  switch(fixed) {
    case FixedNone:
      return;
    case FixedHorizontal:
      switch(get_fixed()) {
        case FixedNone:
        case FixedHorizontal:
          fixed_ = FixedHorizontal;
          return;
        case FixedVertical:
        case FixedAll:
          fixed_ = FixedAll;
          return;
      }
      assert(0);
    case FixedVertical:
      switch(get_fixed()) {
        case FixedNone:
        case FixedVertical:
          fixed_ = FixedVertical;
          return;
        case FixedHorizontal:
        case FixedAll:
          fixed_ = FixedAll;
          return;
      }
      assert(0);
    case FixedAll:
      fixed_ = FixedAll;
      return;
  }
  assert(0);
}

void BoundaryVertex::set_bezier(BezierVertex *bez)
{
    assert(get_bezier() == NULL);
    bezier_vertex_ = bez;
}

void BoundaryVertex::set_cp(const Point2D &p)
{
  access_cp() = p;
}

const Point2D& BoundaryVertex::get_cp() const
{
    return *cp_;
}
Point2D& BoundaryVertex::access_cp()
{
    return cp_.access();
}

const BezierVertex *BoundaryVertex::get_bezier() const
{
  return bezier_vertex_;
}
BezierVertex *BoundaryVertex::get_bezier() {
  return bezier_vertex_;
}

const ControlPoint& BoundaryVertex::get_control_point() const
{
    return cp_;
}

Movement BoundaryVertex::get_fixed() const
{
  return fixed_;
}

void BoundaryVertex::print() const
{
   std::cout<<*this;
}

ostream& operator<<( ostream &stream, const BoundaryVertex& v)
{
  stream<<"BoundryVertex["<<&v<<"]: {point:="<< v.get_cp() <<", edges:=[";
  bool first = true;
  for(BoundaryVertex::edge_const_iterator it = v.begin_edges();
      it != v.end_edges(); ++it) {
    if (!first) { stream << ", "; } else { first = false; }
    stream << *it;
  }
  stream<<"] fixed:="<<v.get_fixed()
          <<", bezier_vertex:="<<v.get_bezier()<<" }"<<endl;
  return stream;
}

define_iterators_any(BoundaryVertex, edge, edges, BoundaryEdge);

/*******************************************/
/*               BoundaryEdge              */
/*******************************************/

BoundaryEdge::BoundaryEdge(PersistantStore& store,
                           BoundaryVertex *v0,
                           BoundaryVertex *v1,
                           const std::vector<ControlPoint> &deboor,
                           const std::vector<double> &knots,
                           DataStore *datastore)
: EdgeCell(store, v0, v1),
  spline_(new QBSpline(deboor, knots, datastore, (v0 == v1))),
  fixed_(FixedNone),
  color_(0)
{
}

BoundaryEdge::~BoundaryEdge()
{
  delete spline_;
}


const BezierVertex* BoundaryEdge::get_d0_vertex(int i) const
{
    assert(i >= 0); assert(i < 2);

    /* Closed splines should have the same BoundaryVertex for both vertexs */
    if( get_spline()->closed ) {
      return get_vertex(0)->get_bezier();
    }

    /* Use the QBSpline::b vector to find which of the vertexs is the first */
    ControlPoint tofind;
    switch(i) {
      case 0: tofind = get_spline()->b[0]; break;
      case 1: tofind = get_spline()->b.back(); break;
      default: assert(0);
    }
    /* Find the vertex on which get_control_point() returns tofind. */
    for(vertex_const_iterator it = begin_vertices(); it != end_vertices();++it){
      if ((*it)->get_control_point() == tofind) {
        return (*it)->get_bezier();
      }
    }
    assert(0);
    return NULL;
}

BezierVertex* BoundaryEdge::get_d0_vertex(int i) {
  return const_cast<BezierVertex*>(
      const_cast<const BoundaryEdge*>(this)->get_d0_vertex(i));
}

int BoundaryEdge::get_num_d1_vertexs() const
{
    return get_spline()->vertexs.size();
}

const BezierVertex* BoundaryEdge::get_d1_vertex(int i) const
{
    assert(i >= 0); assert(i < get_num_d1_vertexs());
    return get_spline()->vertexs[i];
}
BezierVertex* BoundaryEdge::get_d1_vertex(int i) {
  return const_cast<BezierVertex*>(
      const_cast<const BoundaryEdge*>(this)->get_d1_vertex(i));
}

const BezierVertex* BoundaryEdge::get_bezier_vertex(int i) const
{
    int num = get_num_d1_vertexs();
    /* out of range */
    if(i<0 || i>=num+2) return NULL;

    /* first D0 */
    if(i==0) return get_d0_vertex(0);

    /* D1 */
    if(i>0 && i<=num) return get_d1_vertex(i-1);

    /* Last D0 */
    return get_d0_vertex(1);
}
BezierVertex* BoundaryEdge::get_bezier_vertex(int i) {
  return const_cast<BezierVertex*>(
      const_cast<const BoundaryEdge*>(this)->get_bezier_vertex(i));
}

void BoundaryEdge::set_fixed(Movement fixed)
{
    fixed_ = fixed;
    get_vertex(0)->increase_fixed(fixed);
    get_vertex(1)->increase_fixed(fixed);
}

void BoundaryEdge::set_color(int color)
{
    color_ = color;
}

void BoundaryEdge::set_restlength(double restlength)
{
    get_spline()->restlength = restlength;
}

const BoundaryVertex* BoundaryEdge::get_vertex(int i) const
{
  return static_cast<const BoundaryVertex*>(super::get_vertex(i));
}
BoundaryVertex* BoundaryEdge::get_vertex(int i)
{
  return static_cast<BoundaryVertex*>(super::get_vertex(i));
}

const QBSpline *BoundaryEdge::get_spline() const
{
  return spline_;
}

QBSpline *BoundaryEdge::get_spline()
{
  return spline_;
}

Movement BoundaryEdge::get_fixed() const
{
  return fixed_;
}

int BoundaryEdge::get_color() const
{
  return color_;
}

void BoundaryEdge::print() const
{
    std::cout<<*this;
}

ostream &operator<<( ostream &stream, const BoundaryEdge& e){
  bool first;
  stream << "BoundaryEdge[" << &e << "]: {Vertices:=[";
  first = true;
  for(BoundaryEdge::vertex_const_iterator it = e.begin_vertices();
      it != e.end_vertices(); ++it) {
    if(!first) { stream << ", "; } else { first = false; }
    stream << *it;
  }
  stream <<"], Faces:=[";
  first = true;
  for(BoundaryEdge::face_const_iterator it = e.begin_faces();
      it != e.end_faces(); ++it) {
    if(!first) { stream << ", "; } else { first = false; }
    stream << *it;
  }
  stream << "], fixed:=" << e.get_fixed() << ", color:=" << e.get_color()
         << "}\n" << *e.get_spline();
    return stream;
}

define_iterators_canon(BoundaryEdge, vertex, vertices, BoundaryVertex);
define_iterators_any(BoundaryEdge, face, faces, BoundaryFace);

/*******************************************/
/*               BoundaryFace              */
/*******************************************/

BoundaryFace::BoundaryFace(PersistantStore& store,
                           double min_angle, int color) : FaceCell(store),
                                              min_angle_(min_angle),
                                              color_(color)
{
}

void BoundaryFace::set_min_angle(double min_angle)
{
    min_angle_ = min_angle;
}

void BoundaryFace::set_color(int color)
{
    color_ = color;
}

double BoundaryFace::get_min_angle() const
{
  return min_angle_;
}

int BoundaryFace::get_color() const
{
  return color_;
}

void BoundaryFace::print() const
{
    std::cout<<*this;
}


ostream &operator<<( ostream &stream, const BoundaryFace& f){
    stream<<"BoundaryFace["<<&f<<"]: { Edges:=[";
    bool first = true;
    for(BoundaryFace::edge_const_iterator it = f.begin_edges();
        it != f.end_edges(); ++it) {
      if(!first) { stream << ", "; } else { first = false; }
      stream << *it;
    }
    stream<<"] min_angle:=" << f.get_min_angle()
          <<" color:=" << f.get_color() << "}\n";
    return stream;
}

define_iterators_canon(BoundaryFace, edge, edges, BoundaryEdge);

/*******************************************/
/*               BezierVertex              */
/*******************************************/

BezierVertex::BezierVertex(PersistantStore& store,
                           const ControlPoint& cp, const DataPoint& dp)
  : VertexCell(store), cp_(cp), dp_(dp)
  , bdry_edge_(NULL), bdry_vertex_(NULL), u_(-1.0)
{
}

void BezierVertex::set_cp(const Point2D &p )
{
    access_cp() = p;
}

void BezierVertex::set_dp(const LinearData &d )
{
    access_dp() = d;
}

void BezierVertex::set_data(int pos, double data)
{
    access_dp().set(data, pos);
}

double BezierVertex::get_data(int pos) const
{
    return get_dp()[pos];
}


int BezierVertex::containment_dimension() const
{
    if( bdry_vertex_ ) return 0;
    if( bdry_edge_ ) return 1;
    return 2;
}
bool BezierVertex::is_boundary() const
{
  return containment_dimension() < 2;
}

const Point2D& BezierVertex::get_cp() const
{
    return *cp_;
}

Point2D& BezierVertex::access_cp()
{
    return cp_.access();
}

ControlPoint BezierVertex::get_control_point() const
{
  return cp_;
}

const LinearData& BezierVertex::get_dp() const
{
    return *dp_;
}

LinearData& BezierVertex::access_dp()
{
    return dp_.access();
}

DataPoint BezierVertex::get_data_point() const
{
  return dp_;
}

void BezierVertex::print() const
{
    std::cout<<*this;
}

void BezierVertex::set_bdry(BoundaryEdge *b,double param)
{
  bdry_vertex_ = NULL;
  bdry_edge_ = b;
  u_ = param;

  /* register with spline */
  b->get_spline()->set_bezier_vertex(get_u(), this);
}


void BezierVertex::set_bdry(BoundaryEdge *b,int idx)
{
    QBSpline *spline = b->get_spline();
    assert( (idx >= 1) && (idx <= (int)spline->k.size()-2) );
    bdry_vertex_ = NULL;
    bdry_edge_ = b;
    u_ = spline->k[idx];

    /* register with spline */
    spline->set_bezier_vertex(idx, this);
}

void BezierVertex::set_bdry(BoundaryVertex *v)
{
  bdry_vertex_ = v;
  bdry_edge_ = NULL;

  /* register with boundary vertex */
  v->set_bezier(this);
}

const BoundaryEdge *BezierVertex::get_bdry_edge() const
{
  assert(containment_dimension() < 2);
  return bdry_edge_;
}

BoundaryEdge *BezierVertex::get_bdry_edge()
{
  assert(containment_dimension() < 2);
  return bdry_edge_;
}

const BoundaryVertex *BezierVertex::get_bdry_vertex() const
{
  assert(containment_dimension() < 1);
  return bdry_vertex_;
}

BoundaryVertex *BezierVertex::get_bdry_vertex()
{
  assert(containment_dimension() < 1);
  return bdry_vertex_;
}


double BezierVertex::get_u(const BezierEdge *e) const
{
    /* fail if this is not one of our edges, or it is not a boundary*/
    assert(has_edge(e));
    const BoundaryEdge *be = e->get_bdry_edge();
    assert(be);
    const BezierVertex *opposite;

    if( this == e->get_vertex(0) ) opposite = e->get_vertex(1);
    else opposite = e->get_vertex(0);

    return get_u(be,opposite);
}

double BezierVertex::get_u(const BoundaryEdge *be,
                           const BezierVertex *opposite) const
{
    assert(containment_dimension() < 2);
    /* if this is a D1 vertex simply return known u value */
    if(containment_dimension() == 1) {
      return get_u();
    }

    /* otherwise we are D0 and we need to figure out if we are
     * at the beginning or end of the spline, look for our
     * opposing vertex and see which end of spline->vertexs it is at
     */
    const QBSpline *spline = be->get_spline();
    int num_verts = spline->k.size()-2;
    assert(spline->k.size()-2 == spline->vertexs.size());
    if(num_verts > 1){
        assert((spline->vertexs[0] == opposite) || (spline->vertexs[num_verts-1] == opposite));
        if(spline->vertexs[0] == opposite) return spline->k.front();
        else return spline->k.back();
    }

    /* if there are one or less D1 vertex in the spline then we will evaluate
     * at the first and last knot, and see who is closer to us.
     * since there must be at least two D1 vertexs in a closed spline
     * these points must be different
     */
    assert(!spline->closed);
    Point2D p  = get_cp();
    Point2D p0 = spline->evaluate(spline->k.front());
    Point2D p1 = spline->evaluate(spline->k.back());
    bool is_first = (p0 - p).mag() < (p1 - p).mag();
    if(is_first) return spline->k.front();
    else return spline->k.back();
}

double BezierVertex::get_u() const {
  assert(containment_dimension() == 1);
  return u_;
}

void BezierVertex::replace_cp(const ControlPoint& newp) {
  cp_ = newp;
}


ostream &operator<<( ostream &stream, const BezierVertex& v){
    stream<<"BezierVertex "<< &v <<": {control point:="<< v.get_cp()
          <<", data point:="<< v.get_dp() <<", edges:=[";
    bool first = true;
    for(BezierVertex::edge_const_iterator it = v.begin_edges();
        it != v.end_edges(); ++it) {
      const BezierEdge *e = *it;
      stream << (first ? "" : ", ") << e;
      first = false;
    }

    stream << "], containment dimension " << v.containment_dimension();
    switch (v.containment_dimension()) {
      case 0:
        stream << ", bdry_vertex=" << v.get_bdry_vertex();
        break;
      case 1:
        stream << ", bdry_edge=" << v.get_bdry_edge() << ", u=" << v.get_u();
        break;
      case 2:
        break;
      default:
        assert(0);
    }
    stream << "}\n";
    return stream;
}

define_iterators_any(BezierVertex, edge, edges, BezierEdge);

/*******************************************/
/*               BezierEdge                */
/*******************************************/

BezierEdge::BezierEdge(PersistantStore& pstore,
                       BezierVertex *v1, BezierVertex *v2,
                       const ControlPoint& cp,
                       const DataStore& dstore)
: EdgeCell(pstore, v1, v2), bdry_edge_(NULL)
{
  cp_[0] = v1->get_control_point();
  cp_[1] = cp;
  cp_[2] = v2->get_control_point();
  for(int i = 0; i < get_num_cp(); ++i) {
    dp_[i] = dstore[get_control_point(i)];
  }
  u_[0] = u_[1] = -1.0;
}


void BezierEdge::set_bdry(BoundaryEdge *b, double u0, double u1)
{
  bdry_edge_ =b;
  u_[0] = u0;
  u_[1] = u1;
}

void BezierEdge::replace_cp(const ControlPoint& oldp, const ControlPoint& newp)
{
  for (int i = 0; i < get_num_cp(); i++) {
    if (cp_[i] == oldp) {
      cp_[i] = newp;
      return;
    }
  }
  assert(0);
}

double BezierEdge::get_u(int i) const
{
  assert(i >= 0); assert(i < 2);
  return u_[i];
}

Point2D BezierEdge::eval(double u) const
{
  double v = 1-u;
  Point2D val;
  val += get_cp(0) * (v*v);
  val += get_cp(1) * (2*u*v);
  val += get_cp(2) * (u*u);
  return val;
}

LinearData BezierEdge::dataeval(double u) const
{
  double v = 1-u;
  LinearData val;
  val =  get_dp(0) * (v*v);
  val += get_dp(1) * (2*u*v);
  val += get_dp(2) * (u*u);
  return val;
}

void BezierEdge::evalIntermediate(double u, Point2D &p, Point2D &pc0, Point2D &pc1) const
{
  double v=1-u;
  pc0 =  get_cp(0) * v;
  pc0 += get_cp(1) * u;

  pc1 =  get_cp(1) * v;
  pc1 += get_cp(2) * u;

  p  = pc0 * u;
  p += pc1 * v;
}


void BezierEdge::dataevalIntermediate(double u, LinearData &d, LinearData &dc0, LinearData &dc1) const
{
  double v=1-u;
  dc0  = get_dp(0) * v;
  dc0 += get_dp(1) * u;

  dc1  = get_dp(1) * v;
  dc1 += get_dp(2) * u;

  d =  dc0 * u;
  d += dc1 * v;
}


bool BezierEdge::is_encroached(const Point2D& p) const
{
  typedef Tumble::iterators::dereferencer<const ControlPoint*, const Point2D> 
    deref;
  return encroaches(p, deref(cp_), deref(cp_ + get_num_cp()));
}

double BezierEdge::curvature_angle() const
{
  // vectors for the control arms
  Point2D v1 = get_cp(1) - get_cp(0);
  Point2D v2 = get_cp(2) - get_cp(1);

  // vector sizes for normalization
  double v1mag= v1.mag();
  double v2mag = v2.mag();
  // safety
  if (v1mag == 0 || v2mag == 0)
    return 0.0; // really this is undefined

  double cos_theta = v1.dot(v2) / (v1.mag() * v2.mag());
  return acos(cos_theta);
}

int BezierEdge::left_of_curve(Point2D& p, int stop, double& approx) const
{
  approx=0.0;

  Point2D p1 = get_cp(0);
  Point2D p2 = get_cp(2);
  Point2D ctr = get_cp(1);

  bool convex      = ctr.is_left_of( p1,p2 );
  bool carrierside =   p.is_left_of( p1,p2 );
  bool leg1side    =   p.is_left_of( p1,ctr);
  bool leg2side    =   p.is_left_of( ctr,p2);

  if( carrierside != convex) { return (convex ? 0 : 1); }
  if( leg1side == convex)    { return (convex ? 1 : 0); }
  if( leg2side == convex)    { return (convex ? 1 : 0); }

  /*now p is inside convex hull of p1,p2,cp*/
  double t = 0.5;
  double dt = 0.25;
  Point2D c1,c2,mp;
  bool topside,carrier1,carrier2;
  while( stop > 0 ){
    c1 = ctr*0.5 + p1*0.5;
    c2 = p2*0.5 + ctr*0.5;
    mp = c1*0.5 + c2*0.5;

    topside  = p.is_left_of(c1,c2);
    carrier1 = p.is_left_of(p1,mp);
    carrier2 = p.is_left_of(mp,p2);

    if(topside == convex)    { return (convex ? 1 : 0); }
    if(carrier1 == carrier2) { return (convex ? 0 : 1); }

    stop--;
    if(carrier1 == convex){
      p2 = mp;
      ctr = c1;
      t -= dt;
    }
    else{
      p1 = mp;
      ctr = c2;
      t += dt;
    }
    dt = dt*0.5;
  }
  approx = t;
  return 2;
}

void BezierEdge::set_cp(const Point2D &p, int i)
{
    assert(i>=0 && i<=2);
    access_cp(i) = p;
}

void BezierEdge::set_dp(const LinearData &d, int i)
{
    assert(i>=0 && i<=2);
    access_dp(i) = d;
}

const Point2D& BezierEdge::get_cp(int i) const
{
    assert(i >= 0); assert(i < get_num_cp());
    return *cp_[i];
}
Point2D& BezierEdge::access_cp(int i)
{
    assert(i >= 0); assert(i < get_num_cp());
    return cp_[i].access();
}

ControlPoint BezierEdge::get_control_point(int i) const
{
  assert(i >= 0); assert(i < get_num_cp());
  return cp_[i];
}

const LinearData& BezierEdge::get_dp(int i) const
{
    assert(i >= 0); assert(i < get_num_cp());
    return *dp_[i];
}

LinearData& BezierEdge::access_dp(int i)
{
    assert(i >= 0); assert(i < get_num_cp());
    return dp_[i].access();
}

DataPoint BezierEdge::get_data_point(int i) const
{
  assert(i >= 0); assert(i < get_num_cp());
  return dp_[i];
}

void BezierEdge::set_data(int i, int pos, double data)
{
    assert(i>=0 && i<=2);
    access_dp(i).set(data, pos);
}

const BezierVertex* BezierEdge::get_vertex(int i) const
{
    return static_cast<const BezierVertex*>(super::get_vertex(i));
}
BezierVertex* BezierEdge::get_vertex(int i)
{
    return static_cast<BezierVertex*>(super::get_vertex(i));
}

bool BezierEdge::is_boundary() const
{
  return bdry_edge_ != NULL;
}

const BoundaryEdge *BezierEdge::get_bdry_edge() const {
  assert(is_boundary());
  return bdry_edge_;
}

BoundaryEdge *BezierEdge::get_bdry_edge() {
  assert(is_boundary());
  return bdry_edge_;
}

void BezierEdge::print() const
{
    std::cout<<*this;
}

ostream &operator<<( ostream &stream, const BezierEdge& e){
    bool first;
    stream<<"BezierEdge "<<&e<<": {cps:=["
            << e.get_cp(0) <<","
            << e.get_cp(1) <<","
            << e.get_cp(2) <<"]"
            <<" dps:=["
            << e.get_dp(0) <<","
            << e.get_dp(1) <<","
            << e.get_dp(2) <<"]";
    first = true;
    stream << " Vertices:=[";
    for(BezierEdge::vertex_const_iterator it = e.begin_vertices();
        it != e.end_vertices(); ++it) {
      if(!first) { stream << ", "; } else { first = false; }
      stream << *it;
    }
    stream <<"], Faces:=[";
    first = true;
    for(BezierEdge::face_const_iterator it = e.begin_faces();
        it != e.end_faces(); ++it) {
      if(!first) { stream << ", "; } else { first = false; }
      stream << *it;
    }

    stream  <<"] ";
    if (e.is_boundary()) 
      {
    stream <<  e.get_bdry_edge()<<" ,u0="<<e.get_u(0)<<" ,u1="<<e.get_u(1);
      }
    stream << "}\n";
    
    return stream;
}

define_iterators_canon(BezierEdge, vertex, vertices, BezierVertex);
define_iterators_any(BezierEdge, face, faces, BezierTriangle);


/*******************************************/
/*               BezierTriangle            */
/*******************************************/

/* map an index [0-2] into an edge's control points to the appropriate
   index for the edge, if it were relatively-ccw with the triangle. */
static int map_idx(int i, bool ccw) {
  assert(i >= 0); assert(i < 3);
  if (ccw) {
    return i;
  } else {
    return 2 - i;
  }
}

BezierTriangle::BezierTriangle(PersistantStore& store,
                               BezierEdge * const ein[3],
                               const DataStore& datastore,
                   bool inverse_handed)
: FaceCell(store), bdry_face_(NULL)
{
  /* Discover the three vertices. */
  std::vector<BezierVertex*> v;
  v.reserve(3);

  /* Discover the orientation of each linear edge: 'true' indicate in that
   * the edge is numbered consistent with CCW order, 'false' is CW order. */
  bool is_ccw[3];

  if (!inverse_handed) 
    {
      is_ccw[0] = true;
      is_ccw[1] = ( ( ein[1]->get_vertex(0) == ein[0]->get_vertex(1) )
            || (ein[1]->get_vertex(1) == ein[0]->get_vertex(0) ) );
      is_ccw[2] = ( ( ein[2]->get_vertex(1) == ein[0]->get_vertex(0) )
            || (ein[2]->get_vertex(0) == ein[0] -> get_vertex(1) ) );
    } else 
    {
      is_ccw[0] = false;
      is_ccw[1] = ( ( ein[1]->get_vertex(0) == ein[0]->get_vertex(0) )
            || (ein[1]->get_vertex(1) == ein[0]->get_vertex(1) ) );
      is_ccw[2] = ( ( ein[2]->get_vertex(0) == ein[0]->get_vertex(0) )
            || (ein[2]->get_vertex(1) == ein[0]->get_vertex(1) ) );
    }

  v.push_back(ein[0]->get_vertex(0));
  v.push_back(ein[0]->get_vertex(1));
  if (ein[1]->get_vertex(0) == v[0] || ein[1]->get_vertex(0) == v[1])
    v.push_back(ein[1]->get_vertex(1));
  else
    v.push_back(ein[1]->get_vertex(0));
  
  /* Now order the edges themselves (and reorder is_ccw accordingly). */
  BezierEdge *e[3];
  e[0] = ein[0];
  if (ein[0]->get_control_point(map_idx(2, is_ccw[0]))
        == ein[1]->get_control_point(map_idx(0, is_ccw[1]))) {
    e[1] = ein[1];
    e[2] = ein[2];
  } else {
    // inverse_handed triangle!
    assert(ein[0]->get_control_point(map_idx(2, is_ccw[0]))
        == ein[2]->get_control_point(map_idx(0, is_ccw[2])));
    //assert(0 && "creating an inverse-handed triangle");
    e[1] = ein[2];
    e[2] = ein[1];
    bool tmp = is_ccw[1];
    is_ccw[1] = is_ccw[2];
    is_ccw[2] = tmp;
  }

  /* check that this all matches up */
#ifndef NDEBUG
  for(int i = 0; i < 3; i++) {
    int j = (i + 1) % 3;
    assert(e[i]->get_control_point(map_idx(2, is_ccw[i]))
        == e[j]->get_control_point(map_idx(0, is_ccw[j])));
  }
#endif

  /* Pick up the control points appropriately, with the fixed edge
     numbering. See the doxygen comments for the numbering scheme. */
  cp_[0] = e[0]->get_control_point(map_idx(0, is_ccw[0]));
  cp_[1] = e[0]->get_control_point(map_idx(1, is_ccw[0]));
  cp_[2] = e[0]->get_control_point(map_idx(2, is_ccw[0]));
  cp_[3] = e[2]->get_control_point(map_idx(1, is_ccw[2]));
  cp_[4] = e[1]->get_control_point(map_idx(1, is_ccw[1]));
  cp_[5] = e[1]->get_control_point(map_idx(2, is_ccw[1]));
  // check to make sure the numbering is all consistant: at the vertices,
  // we have redundancy
  assert(cp_[0] == e[2]->get_control_point(map_idx(2, is_ccw[2])));
  assert(cp_[2] == e[1]->get_control_point(map_idx(0, is_ccw[1])));
  assert(cp_[5] == e[2]->get_control_point(map_idx(0, is_ccw[2])));

  for(int i = 0; i < 6; i++) {
    dp_[i] = datastore[cp_[i]];
  }
}

/* !\brief Set the boundary face pointer.
 * This can only be called once. */
void BezierTriangle::set_boundary_face(BoundaryFace *f)
{
  assert(bdry_face_ == NULL);
  assert(f != NULL);
  bdry_face_ = f;
}

const Point2D& BezierTriangle::get_cp(int i) const
{
  assert(i >= 0); assert(i < get_num_cp());
  return *cp_[i];
}

Point2D& BezierTriangle::access_cp(int i)
{
  assert(i >= 0); assert(i < get_num_cp());
  return cp_[i].access();
}

void BezierTriangle::set_cp(int i, const Point2D& p)
{
  access_cp(i) = p;
}

ControlPoint BezierTriangle::get_control_point(int i) const
{
  assert(i >= 0); assert(i < get_num_cp());
  return cp_[i];
}

const LinearData& BezierTriangle::get_dp(int i) const
{
  assert(i >= 0); assert(i < get_num_dp());
  return *dp_[i];
}

LinearData& BezierTriangle::access_dp(int i)
{
  assert(i >= 0); assert(i < get_num_cp());
  return dp_[i].access();
}

void BezierTriangle::set_dp(int i, const LinearData& d)
{
  access_dp(i) = d;
}

DataPoint BezierTriangle::get_data_point(int i) const
{
  assert(i >= 0); assert(i < get_num_dp());
  return dp_[i];
}

BezierEdge *BezierTriangle::get_edge(int i) {
  assert(i >= 0); assert(i < get_num_edges());
  return begin_edges()[i];
}
const BezierEdge *BezierTriangle::get_edge(int i) const {
  assert(i >= 0); assert(i < get_num_edges());
  return begin_edges()[i];
}

bool BezierTriangle::has_bdry_face() const {
  return bdry_face_;
}
const BoundaryFace *BezierTriangle::get_bdry_face() const {
  assert(has_bdry_face());
  return bdry_face_;
}
BoundaryFace *BezierTriangle::get_bdry_face() {
  assert(has_bdry_face());
  return bdry_face_;
}
BoundaryFace *BezierTriangle::get_bdry_face_or_null() {
  return bdry_face_;
}

double BezierTriangle::get_min_angle() const {
  return get_bdry_face()->get_min_angle();
}
int BezierTriangle::get_color() const {
  return get_bdry_face()->get_color();
}

const BezierEdge* BezierTriangle::get_edge(int p0, int p1) const
{
    return get_edge(get_edge_index_by_cps(p0,p1));
}
BezierEdge* BezierTriangle::get_edge(int p0, int p1)
{
    return get_edge(get_edge_index_by_cps(p0,p1));
}

Point2D BezierTriangle::eval(double u, double v) const
{
  double w=1.0-u-v;
  Point2D val;
  val  = get_cp(0) * (w * w);
  val += get_cp(1) * (2.0 * v * w);
  val += get_cp(2) * (v * v);
  val += get_cp(3) * (2.0 * u * w);
  val += get_cp(4) * (2.0 * u * v);
  val += get_cp(5) * (u * u);
  return val;
}

LinearData BezierTriangle::dataeval(double u, double v) const
{
  double w=1.0-u-v;
  LinearData val;
  val =  get_dp(0) * (w * w);
  val += get_dp(1) * (2.0 * v * w);
  val += get_dp(2) * (v * v);
  val += get_dp(3) * (2.0 * u * w);
  val += get_dp(4) * (2.0 * u * v);
  val += get_dp(5) * (u * u);
  return val;
}

void BezierTriangle::evalIntermediate(double u, double v, Point2D &p, Point2D &pc0, Point2D &pc1, Point2D &pc2) const
{
    double w = 1.0-u-v;
    pc0 =  get_cp(0) * w;
    pc0 += get_cp(3) * u;
    pc0 += get_cp(1) * v;

    pc1 =  get_cp(3) * w;
    pc1 += get_cp(5) * u;
    pc1 += get_cp(4) * v;

    pc2 =  get_cp(1) * w;
    pc2 += get_cp(4) * u;
    pc2 += get_cp(2) * v;

    p =  pc0 * w;
    p += pc1 * u;
    p += pc2 * v;
}


void BezierTriangle::dataevalIntermediate(double u, double v, LinearData &d, LinearData &dc0, LinearData &dc1, LinearData &dc2) const
{
    double w=1.0-u-v;
    dc0 =  get_dp(0) * w;
    dc0 += get_dp(3) * u;
    dc0 += get_dp(1) * v;

    dc1 =  get_dp(3) * w;
    dc1 += get_dp(5) * u;
    dc1 += get_dp(4) * v;

    dc2 =  get_dp(1) * w;
    dc2 += get_dp(4) * u;
    dc2 += get_dp(2) * v;

    d =  dc0 * w;
    d += dc1 * u;
    d += dc2 * v;
}

double BezierTriangle::jacobian(double u, double v) const
{
  Point2D pdu,pdv;
  jacobian_matrix(u,v,pdu,pdv);
  return -pdu.cross(pdv);
}

void BezierTriangle::jacobian_matrix(double u,double v,Point2D& pdu,Point2D& pdv) const
{
  double w=1.0-u-v;
  pdu =  (get_cp(5) - get_cp(3))*u;
  pdu += (get_cp(4) - get_cp(1))*v;
  pdu += (get_cp(3) - get_cp(0))*w;
  pdu *= 2.0;

  pdv =  (get_cp(4) - get_cp(3))*u;
  pdv += (get_cp(2) - get_cp(1))*v;
  pdv += (get_cp(1) - get_cp(0))*w;
  pdv *= 2.0;
}

static const double large_num = 1.0E9;

void BezierTriangle::bound_jacobian(double& min, double& max,double& area) const
{
  Point2D vu[3];
  Point2D vv[3];

  vu[0] = get_cp(3) - get_cp(0);
  vu[1] = get_cp(5) - get_cp(3);
  vu[2] = get_cp(4) - get_cp(1);
  vv[0] = get_cp(1) - get_cp(0);
  vv[1] = get_cp(4) - get_cp(3);
  vv[2] = get_cp(2) - get_cp(1);

  int i,j;
  double cro;
  min = large_num;
  max = -large_num;
  area = 0.0;
  for(i=0; i<3; i++){
    for(j=0; j<3; j++){
      cro = - vu[i].cross(vv[j]);
      if( cro < min ) { min = cro; }
      if( cro > max ) { max = cro; }
      if( i == j ) { cro *= 2.0; }
      area += cro;
    }
  }
  area /= 12.0;
  return;
}

bool BezierTriangle::newton_find(const Point2D &p, double &u, double &v, int &niter, int &edge_num)
{
    int en;
    bool ret = CurvedTriangle::newton_find(p, u, v, niter, en);

    switch(en) {
      case -1: edge_num = -1; break;
      case 0: edge_num = get_edge_index_by_cps(0, 2); break;
      case 1: edge_num = get_edge_index_by_cps(2, 5); break;
      case 2: edge_num = get_edge_index_by_cps(5, 0); break;
      default: assert(0); break;
    }
    //if(en>=0) cout<<"Newton Find converged in "<<niter<<" to an external point"<<endl
    //    <<"origE#:"<<en<<" newE#:"<<edge_num<<" [u:"<<u<<", v:"<<v<<", u+v:"<<u+v<<"]"<<endl;
    return ret;
}

double BezierTriangle::area() const
{
  double dummy1, dummy2, area;
  bound_jacobian(dummy1, dummy2, area);
  return area;
}

bool BezierTriangle::small_angle() const
{
    double cos_threshold;
    int e0_idx, e1_idx;
    const BezierEdge *e0, *e1;
    const BezierVertex *v0, *v1, *v2, *v3;
    double cos_ang;

    /* we are comparing cosines so convert to radians and take cosine */
    cos_threshold = cos(get_min_angle() * (PI/180.0));

    /* Iterate over all 3 angles (angles are represented by two adjacent edges)*/
    for(int i=0; i<3; ++i){
        e0_idx = (0 + i) % 3;
        e1_idx = (1 + i) % 3;
        e0 = get_edge(e0_idx);
        e1 = get_edge(e1_idx);

        /* skip angles that are a result of boundaries */
        if ((e0->is_boundary() && e1->is_boundary())
            && (e0->get_bdry_edge() != e1->get_bdry_edge())) {
          continue;
        }

        /* now find vertexs of e0 and e1 so that v1 is the shared vertex */
        v0 = e0->get_vertex(0);
        v1 = e0->get_vertex(1);
        v2 = e1->get_vertex(0);
        v3 = e1->get_vertex(1);
        if(v2 == v0){
            v0 = v1;
            v1 = v2;
            v2 = v3;
        }
        else if(v2 == v1){
            v2 = v3;
        }
        else if(v3 == v0){
            v0 = v1;
            v1 = v3;
        }
        /* find cosine of angle */
        cos_ang = v1->get_cp().angle(v0->get_cp(), v2->get_cp());

        if( (cos_ang > 0.0) && (cos_ang > cos_threshold) ){
          //cout<<"Triangle angle "<<i<<" is small: "<<(180.0/PI)*acos(cos_ang)<<endl;
          return true;
        }
    }
    return false;
}

bool BezierTriangle::is_linear_inverted() const
{
  Point2D a = get_cp(0);
  Point2D b = get_cp(2);
  Point2D c = get_cp(5);

  return c.is_left_of(b,a);
}

int BezierTriangle::is_inverted() const
{
    bool is_inverted = is_definitely_inverted();
    bool is_not_inverted = is_definitely_not_inverted();

    /* definitely Inverted */
    if( is_inverted && !is_not_inverted) return 1;

    /* Definitely _NOT_ inverted */
    if( !is_inverted && is_not_inverted) return 0;

    /* This is the indecisive case */
    if( !is_inverted && !is_not_inverted)
      return -1;

    /* This case should never happen */
    if( is_inverted && is_not_inverted ) {
      assert(0);
    }
    return -2;
}

bool BezierTriangle::is_definitely_inverted() const
{
    if( jacobian(0.0, 0.0) < 0.0 ) return true;
    if( jacobian(0.0, 1.0) < 0.0 ) return true;
    if( jacobian(1.0, 0.0) < 0.0 ) return true;
    if( jacobian(1.0/3.0, 1.0/3.0) < 0.0 ) return true;

    for(int i=0; i<7; i++){
      if( jacobian(ALPHA[i], BETA[i]) < 0.0 ) return true;
    }
    return false;
}

bool BezierTriangle::is_definitely_not_inverted() const
{
    /* These are the control points for the jacobian determinate
     * thus they are the coefficients for the jacobian dterminate in the Beizer basis
     * as long as all of these are positive then the jacobian determinate must be positive
     */

    Point2D vu[3];
    Point2D vv[3];

    vu[0] = get_cp(3) - get_cp(0);
    vu[1] = get_cp(5) - get_cp(3);
    vu[2] = get_cp(4) - get_cp(1);
    vv[0] = get_cp(1) - get_cp(0);
    vv[1] = get_cp(4) - get_cp(3);
    vv[2] = get_cp(2) - get_cp(1);

    int i,j,k;
    for(i=0; i<3; i++){
      if( vv[i].cross(vu[i]) < 0.0 )
    return false;
      // This means a corner control point of the jacobian is negative

      j = (i+1) % 3;
      k = (i+2) % 3;
      if (vv[j].cross(vu[k]) + vv[k].cross(vu[j]) < 0.0)
      return false;
      // This means a midside control point of the jacobian is negative
    }
    return true;
}

Point2D BezierTriangle::circumcenter()  const
{
    Point2D a = get_cp(0);
    Point2D b = get_cp(2);
    Point2D c = get_cp(5);

    double c13 = 0.5 * ( c - a ).dot( c + a );
    double c23 = 0.5 * ( c - b ).dot( c + b );
    double dety = ( c[ 0 ] - a[ 0 ] ) * c23 - ( c[ 0 ] - b[ 0 ] ) * c13;
    double detx = ( c[ 1 ] - b[ 1 ] ) * c13 - ( c[ 1 ] - a[ 1 ] ) * c23;
    double det = ( c[ 0 ] - a[ 0 ] ) * ( c[ 1 ] - b[ 1 ] ) - ( c[ 0 ] - b[ 0 ] ) * ( c[ 1 ] - a[ 1 ] );
    Point2D p( detx / det, dety / det );
    return p;
}

Point2D BezierTriangle::offcenter() const
{
  double min_ang_radians = get_min_angle() * (PI/180.0);
  if (min_ang_radians == 0.0) return offcenter(100.0);
  double beta = 0.5 / sin(min_ang_radians);

  return offcenter(beta);
}

Point2D BezierTriangle::offcenter(double beta) const
{
  Point2D a = get_cp(0);
  Point2D b = get_cp(2);
  Point2D c = get_cp(5);
  Point2D cc = circumcenter(); // get circumcenter

  double cr = (cc-a).mag(); //get circumradius
  double abdist = (a-b).mag();
  double bcdist = (b-c).mag();
  double acdist = (a-c).mag();

  // Find the midpoint of the shortest edge and the length of the shortest edge
  Point2D mp = (a+b)*0.5; // The midpoint of the shortest edge
  double se = abdist; // The length of the shortest edge
  if (acdist < bcdist) {
    if (acdist < abdist) {
      mp = (a+c)*0.5;
      se = acdist;
    }
  } else {
    if (bcdist < abdist) {
      mp = (b+c)*0.5;
      se = bcdist;
    }
  }

  // vec is a vector normal to the shortest edge pointing toward the circumcenter
  Point2D vec = (cc-mp);
  vec /= vec.mag();
  Point2D offcenter = (mp + (vec * (2.0 * beta * se)));

  // If the circumcenter is nearer than the offcenter, use the circumcenter
  if (cr > 2.0 * beta*se) {
    return offcenter;
  } else {
    return cc;
  }
}

void BezierTriangle::print() const
{
    std::cout<<*this;
}

void BezierTriangle::print_mathematica() const
{
    cout<<"{";
    cout<<"{"<<get_cp(0).x()<<", "<<get_cp(0).y()<<"}, ";
    cout<<"{"<<get_cp(2).x()<<", "<<get_cp(2).y()<<"}, ";
    cout<<"{"<<get_cp(5).x()<<", "<<get_cp(5).y()<<"}, ";
    cout<<"{"<<get_cp(1).x()<<", "<<get_cp(1).y()<<"}, ";
    cout<<"{"<<get_cp(4).x()<<", "<<get_cp(4).y()<<"}, ";
    cout<<"{"<<get_cp(3).x()<<", "<<get_cp(3).y()<<"}";
    cout<<"}"<<endl;
}

ostream &operator<<( ostream &stream, const BezierTriangle &t )
{
  stream<<"BezierTriangle "<<&t<<": {cps:=["
          << t.get_cp(0) <<","
          << t.get_cp(1) <<","
          << t.get_cp(2) <<","
          << t.get_cp(3) <<","
          << t.get_cp(4) <<","
          << t.get_cp(5) <<"]"
          <<", dps:=["
          << t.get_dp(0) <<","
          << t.get_dp(1) <<","
          << t.get_dp(2) <<","
          << t.get_dp(3) <<","
          << t.get_dp(4) <<","
          << t.get_dp(5) <<"]"
          <<", edges:=[";

  stream<<"[";
  bool first = true;
  for(BezierTriangle::edge_const_iterator it = t.begin_edges();
      it != t.end_edges(); ++it) {
    if(!first) { stream << ", "; } else { first = false; }
    stream << *it;
  }
  stream << "]";
  if (t.has_bdry_face()) {
    stream << "bdry_face:=" << t.get_bdry_face();
  }
  stream << " }\n";
  return stream;
}

define_iterators_canon(BezierTriangle, edge, edges, BezierEdge);

int BezierTriangle::get_edge_index_by_cps(int p0, int p1) const
{
    assert(p0 == 0 || p0 == 2 || p0 == 5);
    assert(p1 == 0 || p1 == 2 || p1 == 5);
    ControlPoint tp0 = get_control_point(p0);
    ControlPoint tp1 = get_control_point(p1);
    for(int i = 0; i < get_num_edges(); ++i) {
        const BezierEdge *e = get_edge(i);
        ControlPoint v0cp = e->get_vertex(0)->get_control_point();
        ControlPoint v1cp = e->get_vertex(1)->get_control_point();
        if((tp0 == v0cp  && tp1 == v1cp) || (tp1 == v0cp  && tp0 == v1cp)) {
          return i;
        }
    }
    /* couldn't find the two points! */
    assert(0);
    return -1;
}

/* !\brief For use during bootstrapping only: replace a control point.
 * The control point must be found.
 */
void BezierTriangle::replace_cp(const ControlPoint& oldp,
                                const ControlPoint& newp)
{
  for (int i = 0; i < get_num_cp(); i++) {
    if (get_control_point(i) == oldp) {
      cp_[i] = newp;
      return;
    }
  }
  assert(0);
}


/* Ghost triangle methods duplicate BezierTriangle methods only
 * CPs and DPs are true Point2D and LinearData, not pointers
 */

/*******************************************/
/*               GhostTriangle             */
/*******************************************/

GhostTriangle::GhostTriangle(const BezierTriangle &bt)
{
    for(int i=0; i < 6; i++){
        cp[i] = bt.get_cp(i);
        dp[i] = bt.get_dp(i);
    }
}

GhostTriangle::GhostTriangle()
{
}

void GhostTriangle::set_cp(int i, Point2D p)
{
  cp[i] = p;
}

Point2D GhostTriangle::get_cp(int i) const
{
  return cp[i];
}

double GhostTriangle::jacobian(double u, double v) const
{
  Point2D pdu,pdv;
  jacobian_matrix(u,v,pdu,pdv);
  return -pdu.cross(pdv);
}

void GhostTriangle::jacobian_matrix(double u, double v, Point2D &pdu, Point2D &pdv) const
{
    double w = 1.0-u-v;
    pdu =  (cp[5] - cp[3])*u;
    pdu += (cp[4] - cp[1])*v;
    pdu += (cp[3] - cp[0])*w;
    pdu *= 2.0;

    pdv =  (cp[4] - cp[3])*u;
    pdv += (cp[2] - cp[1])*v;
    pdv += (cp[1] - cp[0])*w;
    pdv *= 2.0;
}

Point2D GhostTriangle::eval(double u, double v) const
{
  double w=1.0-u-v;
  Point2D val;
  val = cp[0] * (w * w);
  val += cp[1] * (2.0 * v * w);
  val += cp[2] * (v * v);
  val += cp[3] * (2.0 * u * w);
  val += cp[4] * (2.0 * u * v);
  val += cp[5] * (u * u);
  return val;
}

LinearData GhostTriangle::dataeval(double u, double v) const
{
  double w=1.0-u-v;
  LinearData val;
  val =  dp[0] * (w * w);
  val += dp[1] * (2.0 * v * w);
  val += dp[2] * (v * v);
  val += dp[3] * (2.0 * u * w);
  val += dp[4] * (2.0 * u * v);
  val += dp[5] * (u * u);
  return val;
}

void GhostTriangle::print() const
{
    cout<<*this;
}

void GhostTriangle::print_mathematica() const
{
    cout<<"{";
    cout<<"{"<<cp[0].x()<<", "<<cp[0].y()<<"}, ";
    cout<<"{"<<cp[2].x()<<", "<<cp[2].y()<<"}, ";
    cout<<"{"<<cp[5].x()<<", "<<cp[5].y()<<"}, ";
    cout<<"{"<<cp[1].x()<<", "<<cp[1].y()<<"}, ";
    cout<<"{"<<cp[4].x()<<", "<<cp[4].y()<<"}, ";
    cout<<"{"<<cp[3].x()<<", "<<cp[3].y()<<"}";
    cout<<"}"<<endl;
}

ostream &operator<<( ostream &stream, const GhostTriangle &t )
{
  stream<<"GhostTriangle "<<&t<<": {cps:=["
      << t.cp[0] <<","
      << t.cp[1] <<","
      << t.cp[2] <<","
      << t.cp[3] <<","
      << t.cp[4] <<","
      << t.cp[5] <<"]"
      <<", dps:=["
      << t.dp[0] <<","
      << t.dp[1] <<","
      << t.dp[2] <<","
      << t.dp[3] <<","
      << t.dp[4] <<","
      << t.dp[5] <<"]"<<endl;
  return stream;
}


/*******************************************/
/*               CurvedTriangle            */
/*******************************************/

/* These are global values for newton find */
static const double newton_optimal_tolerance = 1.0E-12;
static const double newton_zero_tolerance = 1.0E-5;
static const double newton_tolerance = 1.0E-6;
static const unsigned newton_iterations = 30;

bool CurvedTriangle::newton_find(const Point2D& p,double& u,double& v, int& niter, int &edge_num) const
{
    Point2D pdu, pdv, pt;
    double dJinv;
    double diff;
    //double w;
    //double temp;

    u = 0.33;
    v = 0.33;
    edge_num = -1;
#ifdef DEBUG_NEWTON_FIND
    cout << "[NewtonFind] searching for " << p << " in " << this << endl;
#endif
    for(niter=1; niter <= (int)newton_iterations; niter++) {
        jacobian_matrix(u,v,pdu,pdv);

        pt = eval(u,v) - p;
        dJinv = 1.0 / pdv.cross(pdu);
        u += dJinv * pt.cross(pdv);
        v += dJinv * pdu.cross(pt);

        diff = pt.magsq();
#ifdef DEBUG_NEWTON_FIND
        cout << "[NewtonFind " << niter << "] (" << u << ", " << v << ")"
          << " = " << eval(u,v) << " ; diff=" << diff << endl;
#endif

        /* Check for convergence within optimal tolerance band.
         * If this is our last iteration, then prepare to sacrifice some tolerance to get a convergence
         * at less optimal tolerance levels.
         */
        if( diff < newton_optimal_tolerance || (niter == (int)newton_iterations && diff < newton_tolerance) ){
            /* Now that we have converged, check that the parameters of our convergence are within
             * the triangle.
             *
             * If a value is within newton_zero_tolerance of the cut-off then set it to be
             * exactly the cut-off value
             */
            if( fabs(u) < newton_zero_tolerance ) u = 0.0;
            if( fabs(v) < newton_zero_tolerance ) v = 0.0;
        if( fabs(u+v-1.0) < newton_zero_tolerance ) {
              assert(u != 0.0 || v != 0.0);
          if (v > 1.0)
        v = 1.0 - u;
          else
        u = 1.0 - v;
        }

            //cout<<"In "<<niter<<" iterations: Converged to: u="<<u<<" v="<<v<<" w="<<1.0-u-v<<" with tol="<<diff<<endl;

            /* Return which edge we have crossed over or are on. */
            if( u <= 0.0) edge_num = 0;
            else if( u+v >= 1.0 - newton_zero_tolerance) edge_num = 1;
            else if( v <= 0.0) edge_num = 2;
            else edge_num = -1;

            /* Return true if we have converged in the parameter space of the
             * triangle, false otherwise. */
            return ((u >= 0.0) && (v >= 0.0)
                && (u+v <= 1.0 + newton_zero_tolerance));
        }
    }
    niter = -1;
    //cout<<"Convergence failure!"<<"[u:"<<u<<", v:"<<v<<", u+v:"<<u+v<<"]"<<endl;
    return false;
}

bool CurvedTriangle::newton_find(const Point2D& p,double& u,double& v) const
{
    int niter;
    int edge;
    return newton_find(p, u, v, niter, edge);
}

---