/**
 This file is part of Kig, a KDE program for Interactive Geometry...
 Copyright (C) 2002  Maurizio Paolini <paolini@dmf.unicatt.it>

 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301
 USA
**/

#include <config.h>

#include "conic-common.h"

#include "common.h"
#include "kigtransform.h"

#include <cmath>
#include <algorithm>

#ifdef HAVE_IEEEFP_H
#include <ieeefp.h>
#endif

ConicCartesianData::ConicCartesianData(
  const ConicPolarData& polardata
  )
{
  double ec = polardata.ecostheta0;
  double es = polardata.esintheta0;
  double p = polardata.pdimen;
  double fx = polardata.focus1.x;
  double fy = polardata.focus1.y;

  double a = 1 - ec*ec;
  double b = 1 - es*es;
  double c = - 2*ec*es;
  double d = - 2*p*ec;
  double e = - 2*p*es;
  double f = - p*p;

  f += a*fx*fx + b*fy*fy + c*fx*fy - d*fx - e*fy;
  d -= 2*a*fx + c*fy;
  e -= 2*b*fy + c*fx;

  coeffs[0] = a;
  coeffs[1] = b;
  coeffs[2] = c;
  coeffs[3] = d;
  coeffs[4] = e;
  coeffs[5] = f;
}

ConicPolarData::ConicPolarData( const ConicCartesianData& cartdata )
{
  double a = cartdata.coeffs[0];
  double b = cartdata.coeffs[1];
  double c = cartdata.coeffs[2];
  double d = cartdata.coeffs[3];
  double e = cartdata.coeffs[4];
  double f = cartdata.coeffs[5];

  // 1. Compute theta (tilt of conic)
  double theta = std::atan2(c, b - a)/2;

  // now I should possibly add pi/2...
  double costheta = std::cos(theta);
  double sintheta = std::sin(theta);
  // compute new coefficients (c should now be zero)
  double aa = a*costheta*costheta + b*sintheta*sintheta - c*sintheta*costheta;
  double bb = a*sintheta*sintheta + b*costheta*costheta + c*sintheta*costheta;
  if (aa*bb < 0)
  {   // we have a hyperbola we need to check the correct orientation
    double dd = d*costheta - e*sintheta;
    double ee = d*sintheta + e*costheta;
    double xc = - dd / ( 2*aa );
    double yc = - ee / ( 2*bb );
    double ff = f + aa*xc*xc + bb*yc*yc + dd*xc + ee*yc;
    if (ff*aa > 0)    // wrong orientation
    {
      if (theta > 0) theta -= M_PI/2;
      else theta += M_PI/2;
      costheta = cos(theta);
      sintheta = sin(theta);
      aa = a*costheta*costheta + b*sintheta*sintheta - c*sintheta*costheta;
      bb = a*sintheta*sintheta + b*costheta*costheta + c*sintheta*costheta;
    }
  }
  else
  {
    if ( std::fabs (bb) < std::fabs (aa) )
    {
      if (theta > 0) theta -= M_PI/2;
      else theta += M_PI/2;
      costheta = cos(theta);
      sintheta = sin(theta);
      aa = a*costheta*costheta + b*sintheta*sintheta - c*sintheta*costheta;
      bb = a*sintheta*sintheta + b*costheta*costheta + c*sintheta*costheta;
    }
  }

  double cc = 2*(a - b)*sintheta*costheta +
              c*(costheta*costheta - sintheta*sintheta);
  //  cc should be zero!!!   cout << "cc = " << cc << "\n";
  double dd = d*costheta - e*sintheta;
  double ee = d*sintheta + e*costheta;

  a = aa;
  b = bb;
  c = cc;
  d = dd;
  e = ee;

  // now b cannot be zero (otherwise conic is degenerate)
  a /= b;
  c /= b;
  d /= b;
  e /= b;
  f /= b;
  b = 1.0;

  // 2. compute y coordinate of focuses

  double yf = - e/2;

  // new values:
  f += yf*yf + e*yf;
  e += 2*yf;   // this should be zero!

  // now: a > 0 -> ellipse
  //      a = 0 -> parabula
  //      a < 0 -> hyperbola

  double eccentricity = sqrt(1.0 - a);

  double sqrtdiscrim = sqrt(d*d - 4*a*f);
  if (d < 0.0) sqrtdiscrim = -sqrtdiscrim;
  double xf = (4*a*f - 4*f - d*d)/(d + eccentricity*sqrtdiscrim) / 2;

  // 3. the focus needs to be rotated back into position
  focus1.x = xf*costheta + yf*sintheta;
  focus1.y = -xf*sintheta + yf*costheta;

  // 4. final touch: the pdimen
  pdimen = -sqrtdiscrim/2;

  ecostheta0 = eccentricity*costheta;
  esintheta0 = -eccentricity*sintheta;
  if ( pdimen < 0)
  {
    pdimen = -pdimen;
    ecostheta0 = -ecostheta0;
    esintheta0 = -esintheta0;
  }
}

const ConicCartesianData calcConicThroughPoints (
  const std::vector<Coordinate>& points,
  const LinearConstraints c1,
  const LinearConstraints c2,
  const LinearConstraints c3,
  const LinearConstraints c4,
  const LinearConstraints c5 )
{
  assert( 0 < points.size() && points.size() <= 5 );
  // points is a vector of up to 5 points through which the conic is
  // constrained.
  // this routine should compute the coefficients in the cartesian equation
  //    a x^2 + b y^2 + c xy + d x + e y + f = 0
  // they are defined up to a multiplicative factor.
  // since we don't know (in advance) which one of them is nonzero, we
  // simply keep all 6 parameters, obtaining a 5x6 linear system which
  // we solve using gaussian elimination with complete pivoting
  // If there are too few, then we choose some cool way to fill in the
  // empty parts in the matrix according to the LinearConstraints
  // given..

  // 5 rows, 6 columns..
  double row0[6];
  double row1[6];
  double row2[6];
  double row3[6];
  double row4[6];
  double *matrix[5] = {row0, row1, row2, row3, row4};
  double solution[6];
  int scambio[6];
  LinearConstraints constraints[] = {c1, c2, c3, c4, c5};

  int numpoints = points.size();
  int numconstraints = 5;

  // fill in the matrix elements
  for ( int i = 0; i < numpoints; ++i )
  {
    double xi = points[i].x;
    double yi = points[i].y;
    matrix[i][0] = xi*xi;
    matrix[i][1] = yi*yi;
    matrix[i][2] = xi*yi;
    matrix[i][3] = xi;
    matrix[i][4] = yi;
    matrix[i][5] = 1.0;
  }

  for ( int i = 0; i < numconstraints; i++ )
  {
    if (numpoints >= 5) break;    // don't add constraints if we have enough
    for (int j = 0; j < 6; ++j) matrix[numpoints][j] = 0.0;
    // force the symmetry axes to be
    // parallel to the coordinate system (zero tilt): c = 0
    if (constraints[i] == zerotilt) matrix[numpoints][2] = 1.0;
    // force a parabula (if zerotilt): b = 0
    if (constraints[i] == parabolaifzt) matrix[numpoints][1] = 1.0;
    // force a circle (if zerotilt): a = b
    if (constraints[i] == circleifzt) {
      matrix[numpoints][0] = 1.0;
      matrix[numpoints][1] = -1.0; }
    // force an equilateral hyperbola: a + b = 0
    if (constraints[i] == equilateral) {
      matrix[numpoints][0] = 1.0;
      matrix[numpoints][1] = 1.0; }
    // force symmetry about y-axis: d = 0
    if (constraints[i] == ysymmetry) matrix[numpoints][3] = 1.0;
    // force symmetry about x-axis: e = 0
    if (constraints[i] == xsymmetry) matrix[numpoints][4] = 1.0;

    if (constraints[i] != noconstraint) ++numpoints;
  }

  if ( ! GaussianElimination( matrix, numpoints, 6, scambio ) )
    return ConicCartesianData::invalidData();
  // fine della fase di eliminazione
  BackwardSubstitution( matrix, numpoints, 6, scambio, solution );

  // now solution should contain the correct coefficients..
  return ConicCartesianData( solution );
}

const ConicPolarData calcConicBFFP(
  const std::vector<Coordinate>& args,
  int type )
{
  assert( args.size() >= 2 && args.size() <= 3 );
  assert( type == 1 || type == -1 );

  ConicPolarData ret;

  Coordinate f1 = args[0];
  Coordinate f2 = args[1];
  Coordinate d;
  double eccentricity, d1, d2, dl;

  Coordinate f2f1 = f2 - f1;
  double f2f1l = f2f1.length();
  ret.ecostheta0 = f2f1.x / f2f1l;
  ret.esintheta0 = f2f1.y / f2f1l;

  if ( args.size() == 3 )
  {
    d = args[2];
    d1 = ( d - f1 ).length();
    d2 = ( d - f2 ).length();
    dl = fabs( d1 + type * d2 );
    eccentricity = f2f1l/dl;
  }
  else
  {
    if ( type > 0 ) eccentricity = 0.7; else eccentricity = 2.0;
    dl = f2f1l/eccentricity;
  }

  double rhomax = (dl + f2f1l) /2.0;

  ret.ecostheta0 *= eccentricity;
  ret.esintheta0 *= eccentricity;
  ret.pdimen = type*(1 - eccentricity)*rhomax;
  ret.focus1 = type == 1 ? f1 : f2;
  return ret;
}

const LineData calcConicPolarLine (
  const ConicCartesianData& data,
  const Coordinate& cpole,
  bool& valid )
{
  double x = cpole.x;
  double y = cpole.y;
  double a = data.coeffs[0];
  double b = data.coeffs[1];
  double c = data.coeffs[2];
  double d = data.coeffs[3];
  double e = data.coeffs[4];
  double f = data.coeffs[5];

  double alpha = 2*a*x + c*y + d;
  double beta = c*x + 2*b*y + e;
  double gamma = d*x + e*y + 2*f;

  double normsq = alpha*alpha + beta*beta;

  if (normsq < 1e-10)          // line at infinity
  {
    valid = false;
    return LineData();
  }
  valid = true;

  Coordinate reta = -gamma/normsq * Coordinate (alpha, beta);
  Coordinate retb = reta + Coordinate (-beta, alpha);
  return LineData( reta, retb );
}

const Coordinate calcConicPolarPoint (
  const ConicCartesianData& data,
  const LineData& polar )
{
  Coordinate p1 = polar.a;
  Coordinate p2 = polar.b;

  double alpha = p2.y - p1.y;
  double beta = p1.x - p2.x;
  double gamma = p1.y*p2.x - p1.x*p2.y;

  double a11 = data.coeffs[0];
  double a22 = data.coeffs[1];
  double a12 = data.coeffs[2]/2.0;
  double a13 = data.coeffs[3]/2.0;
  double a23 = data.coeffs[4]/2.0;
  double a33 = data.coeffs[5];

//  double detA = a11*a22*a33 - a11*a23*a23 - a22*a13*a13 - a33*a12*a12 +
//                2*a12*a23*a13;

  double a11inv = a22*a33 - a23*a23;
  double a22inv = a11*a33 - a13*a13;
  double a33inv = a11*a22 - a12*a12;
  double a12inv = a23*a13 - a12*a33;
  double a23inv = a12*a13 - a23*a11;
  double a13inv = a12*a23 - a13*a22;

  double x = a11inv*alpha + a12inv*beta + a13inv*gamma;
  double y = a12inv*alpha + a22inv*beta + a23inv*gamma;
  double z = a13inv*alpha + a23inv*beta + a33inv*gamma;

  if (fabs(z) < 1e-10)          // point at infinity
  {
    return Coordinate::invalidCoord();
  }

  x /= z;
  y /= z;
  return Coordinate (x, y);
}

const Coordinate calcConicLineIntersect( const ConicCartesianData& c,
                                         const LineData& l,
					 double knownparam,
                                         int which )
{
  assert( which == 1 || which == -1 || which == 0 );

  double aa = c.coeffs[0];
  double bb = c.coeffs[1];
  double cc = c.coeffs[2];
  double dd = c.coeffs[3];
  double ee = c.coeffs[4];
  double ff = c.coeffs[5];

  double x = l.a.x;
  double y = l.a.y;
  double dx = l.b.x - l.a.x;
  double dy = l.b.y - l.a.y;

  double aaa = aa*dx*dx + bb*dy*dy + cc*dx*dy;
  double bbb = 2*aa*x*dx + 2*bb*y*dy + cc*x*dy + cc*y*dx + dd*dx + ee*dy;
  double ccc = aa*x*x + bb*y*y + cc*x*y + dd*x + ee*y + ff;

  double t;
  if ( which == 0 )  /* i.e. we have a known intersection */
  {
    t = - bbb/aaa - knownparam;
    return l.a + t*(l.b - l.a);
  }

  double discrim = bbb*bbb - 4*aaa*ccc;
  if (discrim < 0.0)
  {
    return Coordinate::invalidCoord();
  }
  else
  {
    if ( which*bbb > 0 )
    {
      t = bbb + which*sqrt(discrim);
      t = - 2*ccc/t;
    } else {
      t = -bbb + which*sqrt(discrim);
      t /= 2*aaa;
    }

    return l.a + t*(l.b - l.a);
  }
}

ConicPolarData::ConicPolarData(
  const Coordinate& f, double d,
  double ec, double es )
  : focus1( f ), pdimen( d ), ecostheta0( ec ), esintheta0( es )
{
}

ConicPolarData::ConicPolarData()
  : focus1(), pdimen( 0 ), ecostheta0( 0 ), esintheta0( 0 )
{
}

const ConicPolarData calcConicBDFP(
  const LineData& directrix,
  const Coordinate& cfocus,
  const Coordinate& cpoint )
{
  ConicPolarData ret;

  Coordinate ba = directrix.dir();
  double bal = ba.length();
  ret.ecostheta0 = -ba.y / bal;
  ret.esintheta0 = ba.x / bal;

  Coordinate pa = cpoint - directrix.a;

  double distpf = (cpoint - cfocus).length();
  double distpd = ( pa.y*ba.x - pa.x*ba.y)/bal;

  double eccentricity = distpf/distpd;
  ret.ecostheta0 *= eccentricity;
  ret.esintheta0 *= eccentricity;

  Coordinate fa = cfocus - directrix.a;
  ret.pdimen = ( fa.y*ba.x - fa.x*ba.y )/bal;
  ret.pdimen *= eccentricity;
  ret.focus1 = cfocus;

  return ret;
}

ConicCartesianData::ConicCartesianData( const double incoeffs[6] )
{
  std::copy( incoeffs, incoeffs + 6, coeffs );
}

const LineData calcConicAsymptote(
  const ConicCartesianData data,
  int which, bool &valid )
{
  assert( which == -1 || which == 1 );

  LineData ret;
  double a=data.coeffs[0];
  double b=data.coeffs[1];
  double c=data.coeffs[2];
  double d=data.coeffs[3];
  double e=data.coeffs[4];

  double normabc = a*a + b*b + c*c;
  double delta = c*c - 4*a*b;
  if (fabs(delta) < 1e-6*normabc) { valid = false; return ret; }

  double yc = (2*a*e - c*d)/delta;
  double xc = (2*b*d - c*e)/delta;
  // let c be nonnegative; we no longer need d, e, f.
  if (c < 0)
  {
    c *= -1;
    a *= -1;
    b *= -1;
  }

  if ( delta < 0 )
  {
    valid = false;
    return ret;
  }

  double sqrtdelta = sqrt(delta);
  Coordinate displacement;
  if (which > 0)
    displacement = Coordinate(-2*b, c + sqrtdelta);
  else
    displacement = Coordinate(c + sqrtdelta, -2*a);
  ret.a = Coordinate(xc, yc);
  ret.b = ret.a + displacement;
  return ret;
}

const ConicCartesianData calcConicByAsymptotes(
  const LineData& line1,
  const LineData& line2,
  const Coordinate& p )
{
  Coordinate p1 = line1.a;
  Coordinate p2 = line1.b;
  double x = p.x;
  double y = p.y;

  double c1 = p1.x*p2.y - p2.x*p1.y;
  double b1 = p2.x - p1.x;
  double a1 = p1.y - p2.y;

  p1 = line2.a;
  p2 = line2.b;

  double c2 = p1.x*p2.y - p2.x*p1.y;
  double b2 = p2.x - p1.x;
  double a2 = p1.y - p2.y;

  double a = a1*a2;
  double b = b1*b2;
  double c = a1*b2 + a2*b1;
  double d = a1*c2 + a2*c1;
  double e = b1*c2 + c1*b2;

  double f = a*x*x + b*y*y + c*x*y + d*x + e*y;
  f = -f;

  return ConicCartesianData( a, b, c, d, e, f );
}

const LineData calcConicRadical( const ConicCartesianData& cequation1,
                                 const ConicCartesianData& cequation2,
                                 int which, int zeroindex, bool& valid )
{
  assert( which == 1 || which == -1 );
  assert( 0 < zeroindex && zeroindex < 4 );
  LineData ret;
  valid = true;

  double a = cequation1.coeffs[0];
  double b = cequation1.coeffs[1];
  double c = cequation1.coeffs[2];
  double d = cequation1.coeffs[3];
  double e = cequation1.coeffs[4];
  double f = cequation1.coeffs[5];

  double a2 = cequation2.coeffs[0];
  double b2 = cequation2.coeffs[1];
  double c2 = cequation2.coeffs[2];
  double d2 = cequation2.coeffs[3];
  double e2 = cequation2.coeffs[4];
  double f2 = cequation2.coeffs[5];

// background: the family of conics c + lambda*c2 has members that
// degenerate into a union of two lines. The values of lambda giving
// such degenerate conics is the solution of a third degree equation.
// The coefficients of such equation are given by:
// (Thanks to Dominique Devriese for the suggestion of this approach)
// domi: (And thanks to Maurizio for implementing it :)

  double df = 4*a*b*f - a*e*e - b*d*d - c*c*f + c*d*e;
  double cf = 4*a2*b*f + 4*a*b2*f + 4*a*b*f2
     - 2*a*e*e2 - 2*b*d*d2 - 2*f*c*c2
     - a2*e*e - b2*d*d - f2*c*c
     + c2*d*e + c*d2*e + c*d*e2;
  double bf = 4*a*b2*f2 + 4*a2*b*f2 + 4*a2*b2*f
     - 2*a2*e2*e - 2*b2*d2*d - 2*f2*c2*c
     - a*e2*e2 - b*d2*d2 - f*c2*c2
     + c*d2*e2 + c2*d*e2 + c2*d2*e;
  double af = 4*a2*b2*f2 - a2*e2*e2 - b2*d2*d2 - c2*c2*f2 + c2*d2*e2;

// assume both conics are nondegenerate, renormalize so that af = 1

  df /= af;
  cf /= af;
  bf /= af;
  af = 1.0;   // not needed, just for consistency

// computing the coefficients of the Sturm sequence

  double p1a = 2*bf*bf - 6*cf;
  double p1b = bf*cf - 9*df;
  double p0a = cf*p1a*p1a + p1b*(3*p1b - 2*bf*p1a);
  double fval, fpval, lambda;

  if (p0a < 0 && p1a < 0)
  {
    // -+--   ---+
    valid = false;
    return ret;
  }

  lambda = -bf/3.0;    //inflection point
  double displace = 1.0;
  if (p1a > 0)         // with two stationary points
  {
    displace += sqrt(p1a);  // should be enough.  The important
                            // thing is that it is larger than the
                            // semidistance between the stationary points
  }
  // compute the value at the inflection point using Horner scheme
  fval = bf + lambda;              // b + x
  fval = cf + lambda*fval;         // c + xb + xx
  fval = df + lambda*fval;         // d + xc + xxb + xxx

  if (fabs(p0a) < 1e-7)
  {   // this is the case if we intersect two vertical parabulas!
    p0a = 1e-7;  // fall back to the one zero case
  }
  if (p0a < 0)
  {
    // we have three roots..
    // we select the one we want ( defined by mzeroindex.. )
    lambda += ( 2 - zeroindex )* displace;
  }
  else
  {
    // we have just one root
    if( zeroindex > 1 )  // cannot find second and third root
    {
      valid = false;
      return ret;
    }

    if (fval > 0)      // zero on the left
    {
      lambda -= displace;
    } else {           // zero on the right
      lambda += displace;
    }

    // p0a = 0 means we have a root with multiplicity two
  }

//
// find a root of af*lambda^3 + bf*lambda^2 + cf*lambda + df = 0
// (use a Newton method starting from lambda = 0.  Hope...)
//

  double delta;

  int iterations = 0;
  const int maxiterations = 30;
  while (iterations++ < maxiterations)   // using Newton, iterations should be very few
  {
    // compute value of function and polinomial
    fval = fpval = af;
    fval = bf + lambda*fval;         // b + xa
    fpval = fval + lambda*fpval;     // b + 2xa
    fval = cf + lambda*fval;         // c + xb + xxa
    fpval = fval + lambda*fpval;     // c + 2xb + 3xxa
    fval = df + lambda*fval;         // d + xc + xxb + xxxa

    delta = fval/fpval;
    lambda -= delta;
    if (fabs(delta) < 1e-6) break;
  }
  if (iterations >= maxiterations) { valid = false; return ret; }

  // now we have the degenerate conic: a, b, c, d, e, f

  a += lambda*a2;
  b += lambda*b2;
  c += lambda*c2;
  d += lambda*d2;
  e += lambda*e2;
  f += lambda*f2;

  // domi:
  // this is the determinant of the matrix of the new conic.  It
  // should be zero, for the new conic to be degenerate.
  df = 4*a*b*f - a*e*e - b*d*d - c*c*f + c*d*e;

  //lets work in homogeneous coordinates...

  double dis1 = e*e - 4*b*f;
  double maxval = fabs(dis1);
  int maxind = 1;
  double dis2 = d*d - 4*a*f;
  if (fabs(dis2) > maxval)
  {
    maxval = fabs(dis2);
    maxind = 2;
  }
  double dis3 = c*c - 4*a*b;
  if (fabs(dis3) > maxval)
  {
    maxval = fabs(dis3);
    maxind = 3;
  }
  // one of these must be nonzero (otherwise the matrix is ...)
  // exchange elements so that the largest is the determinant of the
  // first 2x2 minor
  double temp;
  switch (maxind)
  {
    case 1:  // exchange 1 <-> 3
    temp = a; a = f; f = temp;
    temp = c; c = e; e = temp;
    temp = dis1; dis1 = dis3; dis3 = temp;
    break;

    case 2:  // exchange 2 <-> 3
    temp = b; b = f; f = temp;
    temp = c; c = d; d = temp;
    temp = dis2; dis2 = dis3; dis3 = temp;
    break;
  }

  // domi:
  // this is the negative of the determinant of the top left of the
  // matrix.  If it is 0, then the conic is a parabola, if it is < 0,
  // then the conic is an ellipse, if positive, the conic is a
  // hyperbola.  In this case, it should be positive, since we have a
  // degenerate conic, which is a degenerate case of a hyperbola..
  // note that it is negative if there is no valid conic to be
  // found ( e.g. not enough intersections.. )
  //  double discrim = c*c - 4*a*b;

  if (dis3 < 0)
  {
    // domi:
    // i would put an assertion here, but well, i guess it doesn't
    // really matter, and this prevents crashes if the math i still
    // recall from high school happens to be wrong :)
    valid = false;
    return ret;
  };

  double r[3];   // direction of the null space
  r[0] = 2*b*d - c*e;
  r[1] = 2*a*e - c*d;
  r[2] = dis3;

  // now remember the switch:
  switch (maxind)
  {
    case 1:  // exchange 1 <-> 3
    temp = a; a = f; f = temp;
    temp = c; c = e; e = temp;
    temp = dis1; dis1 = dis3; dis3 = temp;
    temp = r[0]; r[0] = r[2]; r[2] = temp;
    break;

    case 2:  // exchange 2 <-> 3
    temp = b; b = f; f = temp;
    temp = c; c = d; d = temp;
    temp = dis2; dis2 = dis3; dis3 = temp;
    temp = r[1]; r[1] = r[2]; r[2] = temp;
    break;
  }

  // Computing a Householder reflection transformation that
  // maps r onto [0, 0, k]

  double w[3];
  double rnormsq = r[0]*r[0] + r[1]*r[1] + r[2]*r[2];
  double k = sqrt( rnormsq );
  if ( k*r[2] < 0) k = -k;
  double wnorm = sqrt( 2*rnormsq + 2*k*r[2] );
  w[0] = r[0]/wnorm;
  w[1] = r[1]/wnorm;
  w[2] = (r[2] + k)/wnorm;

  // matrix transformation using Householder matrix, the resulting
  // matrix is zero on third row and column
  // [q0,q1,q2]^t = A w
  // alpha = w^t A w
  double q0 = a*w[0] + c*w[1]/2 + d*w[2]/2;
  double q1 = b*w[1] + c*w[0]/2 + e*w[2]/2;
  double alpha = a*w[0]*w[0] + b*w[1]*w[1] + c*w[0]*w[1] +
                 d*w[0]*w[2] + e*w[1]*w[2] + f*w[2]*w[2];
  double a00 = a - 4*w[0]*q0 + 4*w[0]*w[0]*alpha;
  double a11 = b - 4*w[1]*q1 + 4*w[1]*w[1]*alpha;
  double a01 = c/2 - 2*w[1]*q0 - 2*w[0]*q1 + 4*w[0]*w[1]*alpha;

  double dis = a01*a01 - a00*a11;
  assert ( dis >= 0 );
  double sqrtdis = sqrt( dis );
  double px, py;
  if ( which*a01 > 0 )
  {
    px = a01 + which*sqrtdis;
    py = a11;
  } else {
    px = a00;
    py = a01 - which*sqrtdis;
  }
  double p[3];   // vector orthogonal to one of the two planes
  double pscalw = w[0]*px + w[1]*py;
  p[0] = px - 2*pscalw*w[0];
  p[1] = py - 2*pscalw*w[1];
  p[2] =    - 2*pscalw*w[2];

  // "r" is the solution of the equation A*(x,y,z) = (0,0,0) where
  // A is the matrix of the degenerate conic.  This is what we
  // called in the conic theory we saw in high school a "double
  // point".  It has the unique property that any line going through
  // it is a "polar line" of the conic at hand.  It only exists for
  // degenerate conics.  It has another unique property that if you
  // take any other point on the conic, then the line between it and
  // the double point is part of the conic.
  // this is what we use here: we find the double point ( ret.a
  // ), and then find another points on the conic.

  ret.a = -p[2]/(p[0]*p[0] + p[1]*p[1]) * Coordinate (p[0],p[1]);
  ret.b = ret.a + Coordinate (-p[1],p[0]);
  valid = true;

  return ret;
}

const ConicCartesianData calcConicTransformation (
  const ConicCartesianData& data, const Transformation& t, bool& valid )
{
  double a[3][3];
  double b[3][3];

  a[1][1] = data.coeffs[0];
  a[2][2] = data.coeffs[1];
  a[1][2] = a[2][1] = data.coeffs[2]/2;
  a[0][1] = a[1][0] = data.coeffs[3]/2;
  a[0][2] = a[2][0] = data.coeffs[4]/2;
  a[0][0] = data.coeffs[5];

  Transformation ti = t.inverse( valid );
  if ( ! valid ) return ConicCartesianData();

  double supnorm = 0.0;
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      b[i][j] = 0.;
      for (int ii = 0; ii < 3; ii++)
      {
        for (int jj = 0; jj < 3; jj++)
        {
	  b[i][j] += a[ii][jj]*ti.data( ii, i )*ti.data( jj, j );
	}
      }
      if ( std::fabs( b[i][j] ) > supnorm ) supnorm = std::fabs( b[i][j] );
    }
  }

  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      b[i][j] /= supnorm;
    }
  }

  return ConicCartesianData ( b[1][1], b[2][2], b[1][2] + b[2][1],
                              b[0][1] + b[1][0], b[0][2] + b[2][0], b[0][0] );
}

ConicCartesianData::ConicCartesianData()
{
}

bool operator==( const ConicPolarData& lhs, const ConicPolarData& rhs )
{
  return lhs.focus1 == rhs.focus1 &&
         lhs.pdimen == rhs.pdimen &&
     lhs.ecostheta0 == rhs.ecostheta0 &&
     lhs.esintheta0 == rhs.esintheta0;
}

ConicCartesianData ConicCartesianData::invalidData()
{
  ConicCartesianData ret;
  ret.coeffs[0] = double_inf;
  return ret;
}

bool ConicCartesianData::valid() const
{
  return finite( coeffs[0] );
}