/*
    This file is part of Kig, a KDE program for Interactive Geometry.
    SPDX-FileCopyrightText: 2002 Maurizio Paolini <paolini@dmf.unicatt.it>

    SPDX-License-Identifier: GPL-2.0-or-later
*/

#include "conic-common.h"

#include <math.h>

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

#include <algorithm>
#include <cmath>

#include <config-kig.h>

#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 -> parabola
    //      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 parabola (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;
            /* mp: this threshold test for a point at infinity allows to
             * solve Bug https://bugs.kde.org/show_bug.cgi?id=316693
             */
            if (fabs(t) > 1e15)
                return Coordinate::invalidCoord();
        }

        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 parabolas!
        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 std::isfinite(coeffs[0]);
}