You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
tdeedu/kig/misc/cubic-common.cpp

528 lines
14 KiB

// Copyright (C) 2003 Dominique Devriese <devriese@kde.org>
// 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 "cubic-common.h"
#include "kignumerics.h"
#include "kigtransform.h"
#ifdef HAVE_IEEEFP_H
#include <ieeefp.h>
#endif
/*
* coefficients of the cartesian equation for cubics
*/
CubicCartesianData::CubicCartesianData()
{
std::fill( coeffs, coeffs + 10, 0 );
}
CubicCartesianData::CubicCartesianData(
const double incoeffs[10] )
{
std::copy( incoeffs, incoeffs + 10, coeffs );
}
const CubicCartesianData calcCubicThroughPoints (
const std::vector<Coordinate>& points )
{
// points is a vector of at most 9 points through which the cubic is
// constrained.
// this routine should compute the coefficients in the cartesian equation
// 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 10 parameters, obtaining a 9x10 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..
// 9 rows, 10 columns..
double row0[10];
double row1[10];
double row2[10];
double row3[10];
double row4[10];
double row5[10];
double row6[10];
double row7[10];
double row8[10];
double *matrix[9] = {row0, row1, row2, row3, row4, row5, row6, row7, row8};
double solution[10];
int scambio[10];
int numpoints = points.size();
int numconstraints = 9;
// 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] = 1.0;
matrix[i][1] = xi;
matrix[i][2] = yi;
matrix[i][3] = xi*xi;
matrix[i][4] = xi*yi;
matrix[i][5] = yi*yi;
matrix[i][6] = xi*xi*xi;
matrix[i][7] = xi*xi*yi;
matrix[i][8] = xi*yi*yi;
matrix[i][9] = yi*yi*yi;
}
for ( int i = 0; i < numconstraints; i++ )
{
if (numpoints >= 9) break; // don't add constraints if we have enough
for (int j = 0; j < 10; ++j) matrix[numpoints][j] = 0.0;
bool addedconstraint = true;
switch (i)
{
case 0:
matrix[numpoints][7] = 1.0;
matrix[numpoints][8] = -1.0;
break;
case 1:
matrix[numpoints][7] = 1.0;
break;
case 2:
matrix[numpoints][9] = 1.0;
break;
case 3:
matrix[numpoints][4] = 1.0;
break;
case 4:
matrix[numpoints][5] = 1.0;
break;
case 5:
matrix[numpoints][3] = 1.0;
break;
case 6:
matrix[numpoints][1] = 1.0;
break;
default:
addedconstraint = false;
break;
}
if (addedconstraint) ++numpoints;
}
if ( ! GaussianElimination( matrix, numpoints, 10, scambio ) )
return CubicCartesianData::invalidData();
// fine della fase di eliminazione
BackwardSubstitution( matrix, numpoints, 10, scambio, solution );
// now solution should contain the correct coefficients..
return CubicCartesianData( solution );
}
const CubicCartesianData calcCubicCuspThroughPoints (
const std::vector<Coordinate>& points )
{
// points is a vector of at most 4 points through which the cubic is
// constrained. Moreover the cubic is required to have a cusp at the
// origin.
// 9 rows, 10 columns..
double row0[10];
double row1[10];
double row2[10];
double row3[10];
double row4[10];
double row5[10];
double row6[10];
double row7[10];
double row8[10];
double *matrix[9] = {row0, row1, row2, row3, row4, row5, row6, row7, row8};
double solution[10];
int scambio[10];
int numpoints = points.size();
int numconstraints = 9;
// 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] = 1.0;
matrix[i][1] = xi;
matrix[i][2] = yi;
matrix[i][3] = xi*xi;
matrix[i][4] = xi*yi;
matrix[i][5] = yi*yi;
matrix[i][6] = xi*xi*xi;
matrix[i][7] = xi*xi*yi;
matrix[i][8] = xi*yi*yi;
matrix[i][9] = yi*yi*yi;
}
for ( int i = 0; i < numconstraints; i++ )
{
if (numpoints >= 9) break; // don't add constraints if we have enough
for (int j = 0; j < 10; ++j) matrix[numpoints][j] = 0.0;
bool addedconstraint = true;
switch (i)
{
case 0:
matrix[numpoints][0] = 1.0; // through the origin
break;
case 1:
matrix[numpoints][1] = 1.0;
break;
case 2:
matrix[numpoints][2] = 1.0; // no first degree term
break;
case 3:
matrix[numpoints][3] = 1.0; // a011 (x^2 coeff) = 0
break;
case 4:
matrix[numpoints][4] = 1.0; // a012 (xy coeff) = 0
break;
case 5:
matrix[numpoints][7] = 1.0;
matrix[numpoints][8] = -1.0;
break;
case 6:
matrix[numpoints][7] = 1.0;
break;
case 7:
matrix[numpoints][9] = 1.0;
break;
case 8:
matrix[numpoints][6] = 1.0;
break;
default:
addedconstraint = false;
break;
}
if (addedconstraint) ++numpoints;
}
if ( ! GaussianElimination( matrix, numpoints, 10, scambio ) )
return CubicCartesianData::invalidData();
// fine della fase di eliminazione
BackwardSubstitution( matrix, numpoints, 10, scambio, solution );
// now solution should contain the correct coefficients..
return CubicCartesianData( solution );
}
const CubicCartesianData calcCubicNodeThroughPoints (
const std::vector<Coordinate>& points )
{
// points is a vector of at most 6 points through which the cubic is
// constrained. Moreover the cubic is required to have a node at the
// origin.
// 9 rows, 10 columns..
double row0[10];
double row1[10];
double row2[10];
double row3[10];
double row4[10];
double row5[10];
double row6[10];
double row7[10];
double row8[10];
double *matrix[9] = {row0, row1, row2, row3, row4, row5, row6, row7, row8};
double solution[10];
int scambio[10];
int numpoints = points.size();
int numconstraints = 9;
// 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] = 1.0;
matrix[i][1] = xi;
matrix[i][2] = yi;
matrix[i][3] = xi*xi;
matrix[i][4] = xi*yi;
matrix[i][5] = yi*yi;
matrix[i][6] = xi*xi*xi;
matrix[i][7] = xi*xi*yi;
matrix[i][8] = xi*yi*yi;
matrix[i][9] = yi*yi*yi;
}
for ( int i = 0; i < numconstraints; i++ )
{
if (numpoints >= 9) break; // don't add constraints if we have enough
for (int j = 0; j < 10; ++j) matrix[numpoints][j] = 0.0;
bool addedconstraint = true;
switch (i)
{
case 0:
matrix[numpoints][0] = 1.0;
break;
case 1:
matrix[numpoints][1] = 1.0;
break;
case 2:
matrix[numpoints][2] = 1.0;
break;
case 3:
matrix[numpoints][7] = 1.0;
matrix[numpoints][8] = -1.0;
break;
case 4:
matrix[numpoints][7] = 1.0;
break;
case 5:
matrix[numpoints][9] = 1.0;
break;
case 6:
matrix[numpoints][4] = 1.0;
break;
case 7:
matrix[numpoints][5] = 1.0;
break;
case 8:
matrix[numpoints][3] = 1.0;
break;
default:
addedconstraint = false;
break;
}
if (addedconstraint) ++numpoints;
}
if ( ! GaussianElimination( matrix, numpoints, 10, scambio ) )
return CubicCartesianData::invalidData();
// fine della fase di eliminazione
BackwardSubstitution( matrix, numpoints, 10, scambio, solution );
// now solution should contain the correct coefficients..
return CubicCartesianData( solution );
}
/*
* computation of the y value corresponding to some x value
*/
double calcCubicYvalue ( double x, double ymin, double ymax, int root,
CubicCartesianData data, bool& valid,
int &numroots )
{
valid = true;
// compute the third degree polinomial:
double a000 = data.coeffs[0];
double a001 = data.coeffs[1];
double a002 = data.coeffs[2];
double a011 = data.coeffs[3];
double a012 = data.coeffs[4];
double a022 = data.coeffs[5];
double a111 = data.coeffs[6];
double a112 = data.coeffs[7];
double a122 = data.coeffs[8];
double a222 = data.coeffs[9];
// first the y^3 coefficient, it coming only from a222:
double a = a222;
// next the y^2 coefficient (from a122 and a022):
double b = a122*x + a022;
// next the y coefficient (from a112, a012 and a002):
double c = a112*x*x + a012*x + a002;
// finally the constant coefficient (from a111, a011, a001 and a000):
double d = a111*x*x*x + a011*x*x + a001*x + a000;
return calcCubicRoot ( ymin, ymax, a, b, c, d, root, valid, numroots );
}
const Coordinate calcCubicLineIntersect( const CubicCartesianData& cu,
const LineData& l,
int root, bool& valid )
{
assert( root == 1 || root == 2 || root == 3 );
double a, b, c, d;
calcCubicLineRestriction ( cu, l.a, l.b-l.a, a, b, c, d );
int numroots;
double param =
calcCubicRoot ( -1e10, 1e10, a, b, c, d, root, valid, numroots );
return l.a + param*(l.b - l.a);
}
/*
* calculate the cubic polynomial resulting from the restriction
* of a cubic to a line (defined by two "Coordinates": a point and a
* direction)
*/
void calcCubicLineRestriction ( CubicCartesianData data,
Coordinate p, Coordinate v,
double& a, double& b, double& c, double& d )
{
a = b = c = d = 0;
double a000 = data.coeffs[0];
double a001 = data.coeffs[1];
double a002 = data.coeffs[2];
double a011 = data.coeffs[3];
double a012 = data.coeffs[4];
double a022 = data.coeffs[5];
double a111 = data.coeffs[6];
double a112 = data.coeffs[7];
double a122 = data.coeffs[8];
double a222 = data.coeffs[9];
// zero degree term
d += a000;
// first degree terms
d += a001*p.x + a002*p.y;
c += a001*v.x + a002*v.y;
// second degree terms
d += a011*p.x*p.x + a012*p.x*p.y + a022*p.y*p.y;
c += 2*a011*p.x*v.x + a012*(p.x*v.y + v.x*p.y) + 2*a022*p.y*v.y;
b += a011*v.x*v.x + a012*v.x*v.y + a022*v.y*v.y;
// third degree terms: a111 x^3 + a222 y^3
d += a111*p.x*p.x*p.x + a222*p.y*p.y*p.y;
c += 3*(a111*p.x*p.x*v.x + a222*p.y*p.y*v.y);
b += 3*(a111*p.x*v.x*v.x + a222*p.y*v.y*v.y);
a += a111*v.x*v.x*v.x + a222*v.y*v.y*v.y;
// third degree terms: a112 x^2 y + a122 x y^2
d += a112*p.x*p.x*p.y + a122*p.x*p.y*p.y;
c += a112*(p.x*p.x*v.y + 2*p.x*v.x*p.y) + a122*(v.x*p.y*p.y + 2*p.x*p.y*v.y);
b += a112*(v.x*v.x*p.y + 2*v.x*p.x*v.y) + a122*(p.x*v.y*v.y + 2*v.x*v.y*p.y);
a += a112*v.x*v.x*v.y + a122*v.x*v.y*v.y;
}
const CubicCartesianData calcCubicTransformation (
const CubicCartesianData& data,
const Transformation& t, bool& valid )
{
double a[3][3][3];
double b[3][3][3];
CubicCartesianData dataout;
int icount = 0;
for (int i=0; i < 3; i++)
{
for (int j=i; j < 3; j++)
{
for (int k=j; k < 3; k++)
{
a[i][j][k] = data.coeffs[icount++];
if ( i < k )
{
if ( i == j ) // case aiik
{
a[i][i][k] /= 3.;
a[i][k][i] = a[k][i][i] = a[i][i][k];
}
else if ( j == k ) // case aijj
{
a[i][j][j] /= 3.;
a[j][i][j] = a[j][j][i] = a[i][j][j];
}
else // case aijk (i<j<k)
{
a[i][j][k] /= 6.;
a[i][k][j] = a[j][i][k] = a[j][k][i] =
a[k][i][j] = a[k][j][i] = a[i][j][k];
}
}
}
}
}
Transformation ti = t.inverse( valid );
if ( ! valid ) return dataout;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
b[i][j][k] = 0.;
for (int ii = 0; ii < 3; ii++)
{
for (int jj = 0; jj < 3; jj++)
{
for (int kk = 0; kk < 3; kk++)
{
b[i][j][k] += a[ii][jj][kk]*ti.data( ii, i )*ti.data( jj, j )*ti.data( kk, k );
}
}
}
}
}
}
// assert (fabs(b[0][1][2] - b[1][2][0]) < 1e-8); // test a couple of cases
// assert (fabs(b[0][1][1] - b[1][1][0]) < 1e-8);
// apparently, the above assertions are wrong ( due to rounding
// errors, Maurizio and I hope :) ), so since the symmetry is not
// present, we just take the sum of the parts of the matrix elements
// that should be symmetric, instead of taking one of them, and
// multiplying it..
dataout.coeffs[0] = b[0][0][0];
dataout.coeffs[1] = b[0][0][1] + b[0][1][0] + b[1][0][0];
dataout.coeffs[2] = b[0][0][2] + b[0][2][0] + b[2][0][0];
dataout.coeffs[3] = b[0][1][1] + b[1][0][1] + b[1][1][0];
dataout.coeffs[4] = b[0][1][2] + b[0][2][1] + b[1][2][0] + b[1][0][2] + b[2][1][0] + b[2][0][1];
dataout.coeffs[5] = b[0][2][2] + b[2][0][2] + b[2][2][0];
dataout.coeffs[6] = b[1][1][1];
dataout.coeffs[7] = b[1][1][2] + b[1][2][1] + b[2][1][1];
dataout.coeffs[8] = b[1][2][2] + b[2][1][2] + b[2][2][1];
dataout.coeffs[9] = b[2][2][2];
return dataout;
}
bool operator==( const CubicCartesianData& lhs, const CubicCartesianData& rhs )
{
for ( int i = 0; i < 10; ++i )
if ( lhs.coeffs[i] != rhs.coeffs[i] )
return false;
return true;
}
CubicCartesianData CubicCartesianData::invalidData()
{
CubicCartesianData ret;
ret.coeffs[0] = double_inf;
return ret;
}
bool CubicCartesianData::valid() const
{
return finite( coeffs[0] );
}