// Geom - Copyright (C) 2010-2013 T. Mouton
//
// See the LICENSE.txt file for license information. Please report all
// bugs and problems to <thibaud.mouton@gmail.com>.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>


#ifndef _geom_include
#define _geom_include

namespace geom
{
inline double ccw ( double x1,double y1,double x2,double y2,double x3,double y3 )
{
  return ( x1-x3 ) * ( y2-y3 )- ( x2-x3 ) * ( y1-y3 );
}

inline double ccw ( const double*p1,const double*p2,const double*p3 )
{
  return ccw ( p1[0],p1[1],p2[0],p2[1],p3[0],p3[1] );
}

inline void vec2d ( const double*p1,const double*p2, double*v )
{
  v[0] = p2[0]-p1[0];
  v[1] = p2[1]-p1[1];
}

inline void vec3d ( const double*p1,const double*p2, double*v )
{
  v[0] = p2[0]-p1[0];
  v[1] = p2[1]-p1[1];
  v[2] = p2[2]-p1[2];
}

inline double sqrsum2d ( double x,double y )
{
  return x*x + y*y;
}

inline double sqrsum3d ( double x,double y,double z )
{
  return x*x + y*y + z*z;
}

inline double sqrtdistance2d ( const double *p1, const double *p2 )
{
  return sqrsum2d ( ( p2[0]-p1[0] ), ( p2[1]-p1[1] ) );
}

inline double sqrtdistance3d ( const double *p1, const double *p2 )
{
  return sqrsum3d ( ( p2[0]-p1[0] ), ( p2[1]-p1[1] ), ( p2[2]-p1[2] ) );
}

inline double distance2d ( const double *p1, const double *p2 )
{
  return sqrt ( sqrtdistance2d ( p1, p2 ) );
}

inline double distance3d ( const double *p1, const double *p2 )
{
  return sqrt ( sqrtdistance3d ( p1, p2 ) );
}

// we suppose w is the squared original value
inline double power_distance2d ( const double *a, const double *b, const double w )
{
  return sqrsum2d ( b[0]-a[0],b[1]-a[1] )-w;
}

// we suppose w is the squared original value
inline double power_distance3d ( const double *a, const double *b, const double w )
{
  return sqrsum3d ( b[0]-a[0],b[1]-a[1],b[2]-a[2] )-w;
}

inline double max_distance2d ( const double *a, const double *b )
{
  double vec[2];
  vec[0] = fabs ( b[0]-a[0] );
  vec[1] = fabs ( b[1]-a[1] );
  if ( vec[0] >= vec[1] )
    return vec[0];
  else
    return vec[1];
}

// inline double angled_max_distance (const double *a, const double *b, const double angle )
// {
//     double vec[2];
//     double x, y;
//
//     x = b[0]-a[0];
//     y = b[1]-a[1];
//     vec[0] = x*cos ( angle ) + y*sin ( angle );
//     vec[1] = -x*sin ( angle ) + y*cos ( angle );
//
//     if ( fabs ( vec[0] ) >= fabs ( vec[1] ) )
//         return fabs ( vec[0] );
//     else
//         return fabs ( vec[1] );
// }

inline double angled_max_distance ( const double *a, const double *b, const double cosangle,  const double sinangle )
{
  double vec[2];
  double x, y;

  x = b[0]-a[0];
  y = b[1]-a[1];
  vec[0] = x*cosangle + y*sinangle;
  vec[1] = -x*sinangle + y*cosangle;

  if ( fabs ( vec[0] ) >= fabs ( vec[1] ) )
    return fabs ( vec[0] );
  else
    return fabs ( vec[1] );
}

inline double norm2d ( double *a )
{
  return sqrt ( sqrsum2d ( a[0],a[1] ) );
}

inline double norm3d ( double *a )
{
  return sqrt ( sqrsum3d ( a[0],a[1],a[2] ) );
}

inline double sqrnorm2d ( double *a )
{
  return sqrsum2d ( a[0],a[1] );
}

inline double sqrnorm3d ( double *a )
{
  return sqrsum3d ( a[0],a[1],a[2] );
}

inline void normalize2d ( double *a )
{
  double norm = norm2d ( a );
//   assert ( norm != 0.0 );
  a[0] /= norm;
  a[1] /= norm;
}

inline void normalize3d ( double *a )
{
  double norm = norm3d ( a );
//   assert ( norm != 0.0 );
  a[0] /= norm;
  a[1] /= norm;
  a[2] /= norm;
}

inline double scalar_product2d ( double *v0, double *v1 )
{
  return v0[0]*v1[0] + v0[1]*v1[1];
}

inline double scalar_product2d ( double a, double b, double c, double *v0, double *v1 )
{
  return a*v0[0]*v1[0] + b*v0[0]*v1[1] + b*v0[1]*v1[0] + c*v0[1]*v1[1];
}

inline double scalar_product3d ( double *v0, double *v1 )
{
  return v0[0]*v1[0] + v0[1]*v1[1] + v0[2]*v1[2];
}

inline double cross_product2d ( double *v0, double *v1 )
{
  return v0[0]*v1[1]-v1[0]*v0[1];
}

inline double cross_product2d ( double *p0, double *p1, double *p2 )
{
  double v0[2];
  double v1[2];
  v0[0] = p1[0] - p0[0];
  v0[1] = p1[1] - p0[1];
  v1[0] = p2[0] - p0[0];
  v1[1] = p2[1] - p0[1];
  return cross_product2d ( v0, v1 );
}

inline void cross_product3d ( double *v0, double *v1, double *cross )
{
  cross[0] = v0[1]*v1[2]-v1[1]*v0[2];
  cross[1] = v0[2]*v1[0]-v1[2]*v0[0];
  cross[2] = v0[0]*v1[1]-v1[0]*v0[1];
}

inline void cross_product3d ( double *p0, double *p1, double *p2, double *cross )
{
  double v0[3];
  double v1[3];
  vec3d ( p0, p1, v0 );
  vec3d ( p0, p2, v1 );
  return cross_product3d ( v0, v1, cross );
}

inline double scalar_product2d ( double *a0, double *b0, double *a1, double *b1 )
{
  return ( b0[0]-a0[0] ) * ( b1[0]-a1[0] ) + ( b0[1]-a0[1] ) * ( b1[1]-a1[1] );
}

inline double trivolume ( double *p0, double *p1, double *p2 )
{
  return ( ( p1[0]-p0[0] ) * ( p2[1]-p0[1] ) - ( p1[1]-p0[1] ) * ( p2[0]-p0[0] ) ) /*/2.0*/;
}

// inline void project_on_line ( double *p, double *orig, double *dir, double *pp )
// {
//   double op[3], n[3], nn[3];
//   vec3d ( orig, p, op );
//   cross_product3d ( dir, op, n );
//   cross_product3d ( n, dir, nn );
//   normalize3d ( nn );
//   double d = scalar_product3d ( op, n );
//   translate3d(p, nn, -d, pp);
// }

inline void project_point_on_plane ( double *p, double *plane, double *pp )
{
  double d = plane[0]*p[0] + plane[1]*p[1] + plane[2]*p[2] + plane[3];
  pp[0] = p[0] - d*plane[0];
  pp[1] = p[1] - d*plane[1];
  pp[2] = p[2] - d*plane[2];
}

inline void project_vector_on_plane ( double *vec, double *plane, double *pp )
{
  double d = scalar_product3d ( vec, plane ); // we assume that plane is already normed
  printf("vec --> %lf %lf %lf\n", vec[0], vec[1], vec[2]);
  printf("plane --> %lf %lf %lf\n", plane[0], plane[1], plane[2]);
  printf("d --> %lf\n", d);
  double p[3];
  p[0] = plane[0]*d;
  p[1] = plane[1]*d;
  p[2] = plane[2]*d;
  pp[0] = vec[0] - p[0];
  pp[1] = vec[1] - p[1];
  pp[2] = vec[2] - p[2];
}

// inline void plane ( double *n, double *p, double *plane )
// {
//   plane[0] = n[0];
//   plane[1] = n[1];
//   plane[2] = n[2];
//   plane[3] = -plane[0]*p[0]-plane[1]*p[1]-plane[2]*p[2];
// }

inline void normal ( double *v0, double *v1, double *n )
{
  cross_product3d ( v0, v1, n );
  normalize3d ( n );
}

inline void normal ( double *a, double *b, double *c, double *n )
{
  double ab[3], ac[3];
  vec3d ( a, b, ab );
  vec3d ( a, c, ac );
  normal ( ab, ac, n );
}

inline void plane_from_vectors ( double *v0, double *v1, double *p, double *plane )
{
  printf ( "v0 --> %lf %lf %lf\n", v0[0], v0[1], v0[2] );
  printf ( "v1 --> %lf %lf %lf\n", v1[0], v1[1], v1[2] );
  normal ( v0, v1, plane );
  plane[3] = -plane[0]*p[0]-plane[1]*p[1]-plane[2]*p[2];
}

inline void plane_from_points ( double *a, double *b, double *c, double *plane )
{
  normal ( a, b, c, plane );
  plane[3] = -plane[0]*a[0]-plane[1]*a[1]-plane[2]*a[2];
}

inline void plane_segment_intersection ( double *a, double *b, double *c, double *plane )
{
  normal ( a, b, c, plane );
  plane[3] = -plane[0]*a[0]-plane[1]*a[1]-plane[2]*a[2];
}


inline void create_triedre ( double *n, double *u, double *v )
{
  if ( fabs ( n[1] ) > 1e-2 )
  {
    u[0] = -n[1];
    u[1] = n[0];
    u[2] = n[2];
  }
  else if ( fabs ( n[0] ) > 1e-2 )
  {
    u[0] = -n[2];
    u[1] = n[1];
    u[2] = n[0];
  }
  else
  {
    u[0] = n[0];
    u[1] = n[2];
    u[2] = -n[1];
  }
  cross_product3d ( n, u, v );
  cross_product3d ( v, n, u );
  geom::normalize3d ( u );
  geom::normalize3d ( v );
}

inline void change_frame ( double *p, double *orig, double *u, double *v, double *w, double *pp )
{
//   printf("norms --> %lf %lf %lf\n", geom::norm3d(u), geom::norm3d(v), geom::norm3d(w));
  double op[3];
  vec3d ( orig, p, op );
  pp[0] = scalar_product3d ( op, u );
  pp[1] = scalar_product3d ( op, v );
  pp[2] = scalar_product3d ( op, w );

}

inline int inside_tri2d ( double *p, double *a, double *b, double *c, double *bc )
{
  double vp[2];
  double v0[2];
  double v1[2];
  vec2d ( a, b, v0 );
  vec2d ( a, c, v1 );
  double at = cross_product2d ( v0, v1 );
  if ( at == 0.0 )
    return 0;

  vec2d ( a, p, vp );
  bc[2] = cross_product2d ( v0, vp ) /at;
  if ( bc[2] < -1e-10 )
    return 0;
  vec2d ( b, p, vp );
  vec2d ( b, c, v0 );
  bc[0] = cross_product2d ( v0, vp ) /at;
  if ( bc[0] < -1e-10 )
    return 0;
  vec2d ( c, p, vp );
  vec2d ( c, a, v0 );
  bc[1] = cross_product2d ( v0, vp ) /at;
  if ( bc[1] < -1e-10 )
    return 0;
  return 1;
}

inline void copyvec2d ( double *orig, double *dest )
{
  dest[0] = orig[0];
  dest[1] = orig[1];
}

inline void copyvec3d ( double *orig, double *dest )
{
  dest[0] = orig[0];
  dest[1] = orig[1];
  dest[2] = orig[2];
}

inline void translate2d ( double *orig, double *vec, double factor, double *newp )
{
  newp[0] = orig[0] + vec[0]*factor;
  newp[1] = orig[1] + vec[1]*factor;
}

inline void translate3d ( double *orig, double *vec, double factor, double *newp )
{
  newp[0] = orig[0] + vec[0]*factor;
  newp[1] = orig[1] + vec[1]*factor;
  newp[2] = orig[2] + vec[2]*factor;
}

inline void addvec2d ( double *orig, double *vec, double factor = 1.0 )
{
  orig[0] += vec[0]*factor;
  orig[1] += vec[1]*factor;
}

inline void addvec3d ( double *orig, double *vec, double factor = 1.0 )
{
  orig[0] += vec[0]*factor;
  orig[1] += vec[1]*factor;
  orig[2] += vec[2]*factor;
}

inline void divvec2d ( double *orig, double factor )
{
  orig[0] /= factor;
  orig[1] /= factor;
}

inline void divvec3d ( double *orig, double factor )
{
  orig[0] /= factor;
  orig[1] /= factor;
  orig[2] /= factor;
}

inline void multvec2d ( double *orig, double factor )
{
  orig[0] *= factor;
  orig[1] *= factor;
}

inline void multvec3d ( double *orig, double factor )
{
  orig[0] *= factor;
  orig[1] *= factor;
  orig[2] *= factor;
}

inline void inter3d ( double *a, double *b, double t, double *res )
{
  double ab[3];
  vec3d ( a, b, ab );
  translate3d ( a, ab, t, res );
}

inline double det2 ( double a[2][2] )
{
  return a[0][0]*a[1][1]-a[0][1]*a[1][0];
}

inline int is_between(double *u, double *v0, double *v1)
{
  return (geom::cross_product2d ( u, v0 ) >= 0.0 && geom::cross_product2d ( v1, u ) >= 0.0);
}

inline void inv2x2 ( double a[2][2], double inva[2][2] )
{  
  // \mathbf{A}^{-1} = \begin{bmatrix} a & b \\ c & d \\ \end{bmatrix}^{-1}
  //                 = \frac{1}{\det(\mathbf{A})} \begin{bmatrix} \,\,\,d & \!\!-b \\ -c & \,a \\ \end{bmatrix}
  //                 = \frac{1}{ad - bc} \begin{bmatrix} \,\,\,d & \!\!-b \\ -c & \,a \\ \end{bmatrix}. 
  
  // determinants
  double det = det2 ( a );
  double invdet = 1.0/det;
  inva[0][0] = invdet*a[1][1];
  inva[0][1] = -invdet*a[0][1];
  inva[1][0] = -invdet*a[1][0];
  inva[1][1] = invdet*a[0][0];
}

inline double det3 ( double a[3][3] )
{
  return ( a[0][0]*a[1][1]*a[2][2] )- ( a[0][0]*a[1][2]*a[2][1] )
         + ( a[0][1]*a[1][2]*a[2][0] )- ( a[0][1]*a[1][0]*a[2][2] )
         + ( a[0][2]*a[1][0]*a[2][1] )- ( a[0][2]*a[1][1]*a[2][0] );
}

inline double det3x ( double a[3][3], double b[3] )
{
  return ( b[0]*a[1][1]*a[2][2] )- ( b[0]*a[1][2]*a[2][1] )
         + ( a[0][1]*a[1][2]*b[2] )- ( a[0][1]*b[1]*a[2][2] )
         + ( a[0][2]*b[1]*a[2][1] )- ( a[0][2]*a[1][1]*b[2] );
}

inline double det3y ( double a[3][3], double b[3] )
{
  return ( a[0][0]*b[1]*a[2][2] )- ( a[0][0]*a[1][2]*b[2] )
         + ( b[0]*a[1][2]*a[2][0] )- ( b[0]*a[1][0]*a[2][2] )
         + ( a[0][2]*a[1][0]*b[2] )- ( a[0][2]*b[1]*a[2][0] );
}

inline double det3z ( double a[3][3], double b[3] )
{
  return ( a[0][0]*a[1][1]*b[2] )- ( a[0][0]*b[1]*a[2][1] )
         + ( a[0][1]*b[1]*a[2][0] )- ( a[0][1]*a[1][0]*b[2] )
         + ( b[0]*a[1][0]*a[2][1] )- ( b[2]*a[1][1]*a[2][0] );
}

inline void inv3x3 ( double a[3][3], double inva[3][3] )
{
  //   \mathbf{A}^{-1} = \begin{bmatrix} a & b & c\\ d & e & f \\ g & h & i\\ \end{bmatrix}^{-1} 
  //                   = \frac{1}{\det(\mathbf{A})} \begin{bmatrix} \, A & \, B & \,C \\ \, D & \, E & \, F \\ \, G & \, H & \, I\\ \end{bmatrix}^T 
  //                   = \frac{1}{\det(\mathbf{A})} \begin{bmatrix} \, A & \, D & \,G \\ \, B & \, E & \,H \\ \, C & \,F & \, I\\ \end{bmatrix} 
  
  // determinant
  double det = det3 ( a );
  double invdet = 1.0/det;
  
  // \begin{matrix} A = (ei-fh) & D = -(bi-ch) & G = (bf-ce) \\ B = -(di-fg) & E = (ai-cg) & H = -(af-cd) \\ C = (dh-eg) & F = -(ah-bg) & I = (ae-bd) \\ \end{matrix} 
  inva[0][0] = invdet*(a[1][1]*a[2][2]-a[1][2]*a[2][1]);
  inva[0][1] = invdet*(-(a[0][1]*a[2][2]-a[0][2]*a[2][1]));
  inva[0][2] = invdet*(a[0][1]*a[1][2]-a[0][2]*a[1][1]);
  
  inva[1][0] = invdet*(-(a[1][0]*a[2][2]-a[1][2]*a[2][0]));
  inva[1][1] = invdet*(a[0][0]*a[2][2]-a[0][2]*a[2][0]);
  inva[1][2] = invdet*(-(a[0][0]*a[1][2]-a[0][2]*a[1][0]));
  
  inva[2][0] = invdet*(a[1][0]*a[2][1]-a[1][1]*a[2][0]);
  inva[2][1] = invdet*(-(a[0][0]*a[2][1]-a[0][1]*a[2][0]));
  inva[2][2] = invdet*(a[0][0]*a[1][1]-a[0][1]*a[1][0]);
}

// Uses Cramer's Rule to solve a linear system
inline void solve3x3 ( double a[3][3], double b[3], double *p )
{
  // determinants
  double det = det3 ( a );
  double detx = det3x ( a, b );
  double dety = det3y ( a, b );
  double detz = det3z ( a, b );
  if ( det != 0.0 )
  {
    p[0] = detx/det;
    p[1] = dety/det;
    p[2] = detz/det;
  }
  else
    printf ( "pb --> determinant = 0\n" );
}

inline void tri_inscribed_circle3d ( double *a, double *b, double *c, double *center )
{
  double mab[3], mac[3];
  inter3d ( a, b, 0.5, mab );
  inter3d ( a, c, 0.5, mac );
  double bm[3],cm[3];
  vec3d ( c, mab, cm );
  vec3d ( b, mac, bm );
  double n[3];
  normal ( a, b, c, n );
  double pl[4];
  cross_product3d ( bm, n, pl );
  normalize3d ( pl );
  pl[3] = -pl[0]*b[0]-pl[1]*b[1]-pl[2]*b[2];
  double t = ( pl[0]*c[0]+pl[1]*c[1]+pl[2]*c[2]+pl[3] ) / ( pl[0]*cm[0]+pl[1]*cm[1]+pl[2]*cm[2] );
  translate3d ( c, cm, t, center );
}

inline double triquality2d ( double *a, double *b, double *c )
{
  double twosqrt3 = 3.46410161513775458705;
  double ab[2], ac[2], bc[2];
  vec2d ( a, b, ab );
  vec2d ( a, c, ac );
  vec2d ( b, c, bc );

  /* orientation */
  double aire = fabs ( cross_product2d ( ab, ac ) );
//   if ( aire < 1e-10 )
//     return ( 0.0 );

  /* edge lengths */
  double h1,h2,h3;
  h1 = sqrnorm2d ( ab );
  h2 = sqrnorm2d ( ac );
  h3 = sqrnorm2d ( bc );

  return ( aire * twosqrt3 / ( h1+h2+h3 ) );
}

inline double triquality3d ( double *a, double *b, double *c )
{
  double twosqrt3 = 3.46410161513775458705;
  double ab[3], ac[3], bc[3];
  vec3d ( a, b, ab );
  vec3d ( a, c, ac );
  vec3d ( b, c, bc );

  /* orientation */
  double n[3];
  cross_product3d ( ab, ac, n );
  double aire = norm3d ( n );

  /* edge lengths */
  double h1,h2,h3;
  h1 = sqrnorm3d ( ab );
  h2 = sqrnorm3d ( ac );
  h3 = sqrnorm3d ( bc );

  return ( aire * twosqrt3 / ( h1+h2+h3 ) );
}

inline double distance_to_line3d ( double *orig, double *dir, double *p )
{
  double op[3], n[3], nn[3];
  vec3d ( orig, p, op );
  cross_product3d ( dir, op, n );
  cross_product3d ( n, dir, nn );
  normalize3d ( nn );
  return scalar_product3d ( op, n );
}

inline void line2d_dir_to_coeff ( double *pt, double *dir, double *coeff, double epsilon )
{
  if ( fabs ( dir[0]/dir[1] ) < epsilon )
    dir[0] = 0.0;
  if ( fabs ( dir[1]/dir[0] ) < epsilon )
    dir[1] = 0.0;

  if ( dir[0] != 0.0 && dir[1] != 0.0 )
  {
    coeff[0] = -dir[1];
    coeff[1] = dir[0];
    coeff[2] = -coeff[0]*pt[0]-coeff[1]*pt[1];

    coeff[1] /= coeff[0];
    coeff[2] /= coeff[0];
    coeff[0] = 1.0;
  }
  else if ( dir[1] == 0.0 )
  {
    coeff[0] = 0.0;
    coeff[1] = 1.0;
    coeff[2] = -pt[1];
  }
  else if ( dir[0] == 0.0 )
  {
    coeff[0] = 1.0;
    coeff[1] = 0.0;
    coeff[2] = -pt[0];
  }
}

inline void line3d_dir_to_coeff ( double *pt, double *dir, double *coeff, double epsilon )
{
  if ( fabs ( dir[0]/dir[1] ) < epsilon )
    dir[0] = 0.0;
  if ( fabs ( dir[1]/dir[0] ) < epsilon )
    dir[1] = 0.0;

  if ( dir[0] != 0.0 && dir[1] != 0.0 )
  {
    coeff[0] = -dir[1];
    coeff[1] = dir[0];
    coeff[2] = -coeff[0]*pt[0]-coeff[1]*pt[1];

    coeff[1] /= coeff[0];
    coeff[2] /= coeff[0];
    coeff[0] = 1.0;
  }
  else if ( dir[1] == 0.0 )
  {
    coeff[0] = 0.0;
    coeff[1] = 1.0;
    coeff[2] = -pt[1];
  }
  else if ( dir[0] == 0.0 )
  {
    coeff[0] = 1.0;
    coeff[1] = 0.0;
    coeff[2] = -pt[0];
  }
}

// inline int colinear_segment ( double *p0, double *p1, double *p2, double *p3, double epsilon )
// {
//    return fabs((p0[0]-p1[0])*(p2[1]-p3[1]) - (p0[1]-p1[1])*(p2[0]-p3[0]) < epsilon);
// }
//
// inline int segment_intersection ( double *p0, double *p1, double *p2, double *p3, double epsilon, double *pt )
// {
//     int d = (p0[0]-p1[0])*(p2[1]-p3[1]) - (p0[1]-p1[1])*(p2[0]-p3[0]);
//     if (fabs(d) < epsilon)
//     {
//       return 0;
//     }
//     int xi = ((x3-x4)*(x1*y2-y1*x2)-(x1-x2)*(x3*y4-y3*x4))/d;
//     int yi = ((y3-y4)*(x1*y2-y1*x2)-(y1-y2)*(x3*y4-y3*x4))/d;
// }

inline int colinear_lines2d ( double *coeff0, double *coeff1, double epsilon )
{
  if ( fabs ( coeff1[0] - coeff0[0] ) < epsilon && fabs ( coeff1[1] - coeff0[1] ) < epsilon )
  {
    if ( coeff1[2] != coeff0[2] )
      return 1;
    else
      return 2;
  }
  return 0;
}

inline int lines2d_intersection ( double *coeff0, double *coeff1, double epsilon, double *pt )
{
  int cr = colinear_lines2d ( coeff0, coeff1, epsilon );
  if ( cr == 1 )
    return 0;
  if ( cr == 2 )
  {
    pt[0] = INFINITY;
    pt[1] = INFINITY;
    return 1;
  }

  if ( coeff1[0] == 0.0 )
  {
    pt[1] = -coeff1[2] /coeff1[1];
    pt[0] = -coeff0[1]*pt[1]-coeff0[2];
    return 1;
  }
  if ( coeff0[0] == 0.0 )
  {
    pt[1] = -coeff0[2]/coeff0[1];
    pt[0] = -coeff1[1] *pt[1]-coeff1[2];
    return 1;
  }
  if ( coeff1[1] == 0.0 )
  {
    pt[0] = -coeff1[2];
    pt[1] = ( -pt[0]-coeff0[2] ) /coeff0[1];
    return 1;
  }
  if ( coeff0[1] == 0.0 )
  {
    pt[0] = -coeff0[2];
    pt[1] = ( -pt[0]-coeff1[2] ) /coeff1[1];
    return 1;
  }

  pt[1] = ( coeff1[2]-coeff0[2] ) / ( coeff0[1]-coeff1[1] );
  pt[0] = -coeff0[1]*pt[1]-coeff0[2];
  return 1;
}

inline int point_inside_segment2d ( double *p0, double *p1, double epsilon, double *pt )
{
  for ( int i = 0; i < 2; i++ )
  {
    if ( p0[i] != p1[i] )
    {
      if ( p0[i] < p1[i] && pt[i] >= p0[i] && pt[i] <= p1[i] )
        return 1;
      if ( p0[i] > p1[i] && pt[i] >= p1[i] && pt[i] <= p0[i] )
        return 1;
    }
  }
  if ( ( fabs ( pt[0] - p0[0] ) < epsilon && fabs ( pt[1] - p0[1] ) < epsilon )
       || ( fabs ( pt[0] - p1[0] ) < epsilon && fabs ( pt[1] - p1[1] ) < epsilon ) )
    return 1;

//   printf ( "in bounds : trajectory %lf %lf -- %lf %lf : point %lf %lf\n", bound0[0], bound0[1], bound1[0], bound1[1], pt[0], pt[1] );
  return 0;
}

inline int segments2d_intersection ( double *coeff0, double *p0, double *p1, double *coeff1, double *p2, double *p3, double epsilon, double *pt )
{
  if ( lines2d_intersection ( coeff0, coeff1, epsilon, pt ) )
  {
    if ( pt[0] == INFINITY && pt[1] == INFINITY ) // colinear
    {
      if ( point_inside_segment2d ( p0, p1, epsilon, p2 ) && point_inside_segment2d ( p2, p3, epsilon, p1 ) )
      {
        pt[0] = p2[0];
        pt[1] = p2[1];
        return 1;
      }
      if ( point_inside_segment2d ( p0, p1, epsilon, p3 ) && point_inside_segment2d ( p2, p3, epsilon, p0 ) )
      {
        pt[0] = p3[0];
        pt[1] = p3[1];
        return 1;
      }
      return 0;
    }
    if ( point_inside_segment2d ( p0, p1, epsilon, pt ) && point_inside_segment2d ( p2, p3, epsilon, pt ) )
      return 1;
  }
  return 0;
}

inline int colinear_segments2d ( double *p0, double *p1, double *p2, double *p3, double epsilon )
{
  double u[2], v[2], w[2];
  vec2d ( p0, p1, u );
  vec2d ( p2, p3, v );
  vec2d ( p2, p0, w );

  if ( cross_product2d ( u, v ) < epsilon ) // colinearity
  {
    if ( cross_product2d ( u, w ) < epsilon ) // alignment
      return 2;
    else
      return 2;
  }
  return 0;
}

inline int segments2d_intersection ( double *p0, double *p1, double *p2, double *p3, double epsilon, double *pt )
{
  double u[2], v[2], w[2];
  vec2d ( p0, p1, u );
  vec2d ( p2, p3, v );
  vec2d ( p2, p0, w );

//   if ( cross_product2d ( u, v ) < epsilon ) // colinearity
//   {
//     if ( cross_product2d ( u, w ) < epsilon ) // alignment
//     {
//       if ( point_inside_segment2d ( p0, p1, epsilon, p2 ) && point_inside_segment2d ( p2, p3, epsilon, p1 ) )
//       {
//         pt[0] = p2[0];
//         pt[1] = p2[1];
//         return 1;
//       }
//       if ( point_inside_segment2d ( p0, p1, epsilon, p3 ) && point_inside_segment2d ( p2, p3, epsilon, p0 ) )
//       {
//         pt[0] = p3[0];
//         pt[1] = p3[1];
//         return 1;
//       }
//       return 0;
//     }
//     return 0;
//   }

//   printf("segment intersection : u, v, w --> %lf %lf, %lf %lf, %lf %lf\n", u[0], u[1], v[0], v[1], w[0], w[1]);
  double a = v[1]*w[0]-v[0]*w[1];
  double b = v[0]*u[1]-v[1]*u[0];
  if (b == 0.0)
    return 0;
  double t = a / b;
//   printf("segment intersection : t --> %.20lf\n", t);
  if ( t >= 0.0 && t <= 1.0 )
  {
    translate2d ( p0, u, t, pt );
    return 1;
  }
  return 0;
}

inline int lines2d_intersection ( double *p0, double *p1, double *u, double *v, double epsilon, double *pt )
{
  if (fabs(cross_product2d(u, v)) < epsilon) // parallel lines
    return 0;
  
  double w[2];
  vec2d ( p1, p0, w );

  double t = ( v[1]*w[0]-v[0]*w[1] ) / ( v[0]*u[1]-v[1]*u[0] );
//   printf("segment intersection : t --> %.20lf\n", t);
  if ( t > epsilon )
  {
    translate2d ( p0, u, t, pt );
    return 1;
  }
  return 0;
}

inline int segment3d_plane_intersection ( double *p0, double *p1, double *plane, double epsilon, double *pt )
{
  double u[3];
  vec3d ( p0, p1, u );
  double denom = scalar_product3d ( plane, u );
  if ( fabs ( denom ) < epsilon )
    return 0;
  // compute parameter t
  double t = - ( plane[0]*p0[0]+plane[1]*p0[1]+plane[2]*p0[2]+plane[3] ) / denom;

//   printf("plane intersection : t --> %.20lf\n", t);
  if ( t > epsilon && t < 1.0-epsilon )
  {
    translate3d ( p0, u, t, pt );
    return 1;
  }
  return 0;
}

inline int line3d_plane_intersection ( double *p0, double *vec, double *plane, double epsilon, double *pt )
{
//   printf("line --> %lf %lf %lf\n", vec[0], vec[1], vec[2]);
  double denom = scalar_product3d ( plane, vec );
  if ( fabs ( denom ) < epsilon )
    return 0;
  // compute parameter t
  double t = - ( plane[0]*p0[0]+plane[1]*p0[1]+plane[2]*p0[2]+plane[3] ) / denom;

//   printf("plane intersection : t --> %.20lf\n", t);
//   printf("p --> %lf %lf %lf\n", p0[0] + t*vec[0], p0[1] + t*vec[1], p0[2] + t*vec[2]);

  if ( t <= 0.0 )
    return 0;

  translate3d ( p0, vec, t, pt );
  return 1;
}

//rotate/flip a quadrant appropriately
inline void hilbert_rot ( int n, int *x, int *y, int rx, int ry )
{
  if ( ry == 0 )
  {
    if ( rx == 1 )
    {
      *x = n-1 - *x;
      *y = n-1 - *y;
    }

    //Swap x and y
    int t  = *x;
    *x = *y;
    *y = t;
  }
}

//convert (x,y) to d
inline int hilbert_xy2d ( int n, int x, int y )
{
  int rx, ry, s, d=0;
  for ( s=n/2; s>0; s/=2 )
  {
    rx = ( x & s ) > 0;
    ry = ( y & s ) > 0;
    d += s * s * ( ( 3 * rx ) ^ ry );
    hilbert_rot ( s, &x, &y, rx, ry );
  }
  return d;
}

//convert d to (x,y)
inline void hilbert_d2xy ( int n, int d, int *x, int *y )
{
  int rx, ry, s, t=d;
  *x = *y = 0;
  for ( s=1; s<n; s*=2 )
  {
    rx = 1 & ( t/2 );
    ry = 1 & ( t ^ rx );
    hilbert_rot ( s, x, y, rx, ry );
    *x += s * rx;
    *y += s * ry;
    t /= 4;
  }
}

};





// inline double pseudo_angle(double *p0, double *p1, double *p2)
// {
//     double v0[2];
//     double v1[2];
//     v0[0] = p1[0] - p0[0];
//     v0[1] = p1[1] - p0[1];
//     v1[0] = p2[0] - p0[0];
//     v1[1] = p2[1] - p0[1];
//
//     normalize(v0);
//     normalize(v1);
//
//     return cross_product2D(v0, v1);
// }

// inline double exact_angle(double *p0, double *p1, double *p2)
// {
//     double v0[2];
//     double v1[2];
//     v0[0] = p1[0] - p0[0];
//     v0[1] = p1[1] - p0[1];
//     v1[0] = p2[0] - p0[0];
//     v1[1] = p2[1] - p0[1];
//
//     return atan2(v0[1], v0[0]) - atan2(v1[1], v1[0]);
// }
// inline int intersect(double e11[2], double e12[2], double e21[2], double e22[2])
// {
// //   printf(" ccw1 = %lf, ccw2 = %lf\n", ccw (e11, e22, e21), ccw (e21, e22, e12));
//    if (ccw (e11, e22, e21) > 0 && ccw (e21, e22, e12) > 0)
//      return 1;
//    else
//      return 0;
// }
//
// inline double linear_normalized_length(double p1[2], double h1, double p2[2], double h2)
// {
//    assert(0.0<h1);
//    assert(0.0<h2);
//
//    double d = distance(p1, p2);
//    double dh = h2 - h1;
//    double lenght;
//
//    if(h1/h2 < 0.999 || h2/h1 < 0.999)
//    {
//    double dln = log(h2) - log(h1);
//    lenght = (d*dln)/dh;
//    }
//    else lenght = d/h1;
//
//    return lenght;
// }



// inline double geom_normalized_length(double p1[2], double h1, double p2[2], double h2)
// {
//    assert(0.0<h1);
//    assert(0.0<h2);
//
//    double d = dist(p1, p2);
//    double dh = h2 - h1;
//    double lenght;
//
//    /** geometric */
//    if(fabs(dh) <= 0.001) lenght = d/a->h;
//    else
//    {
//    double dln = log(b->h) - log(a->h);
//    lenght = (d*dh)/(a->h*b->h*dln);
//    }
//
//    return lenght;
// }

inline double get_abs_curv ( double l, double lt, double h1, double h2 )
{
  double abs;
  double dh = h2-h1;
  if ( fabs ( dh ) < 0.001 )
    abs = ( l/lt ) *h1;
  else
    abs = ( ( exp ( l*dh/lt ) *h1 )-h1 ) /dh;
  return abs;
}

#endif