diff options
Diffstat (limited to 'kig/objects/centerofcurvature_type.cc')
-rw-r--r-- | kig/objects/centerofcurvature_type.cc | 304 |
1 files changed, 304 insertions, 0 deletions
diff --git a/kig/objects/centerofcurvature_type.cc b/kig/objects/centerofcurvature_type.cc new file mode 100644 index 00000000..8111410f --- /dev/null +++ b/kig/objects/centerofcurvature_type.cc @@ -0,0 +1,304 @@ +// Copyright (C) 2004 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 "centerofcurvature_type.h" + +#include "bogus_imp.h" +#include "conic_imp.h" +#include "cubic_imp.h" +//#include "other_imp.h" +#include "point_imp.h" +//#include "line_imp.h" + +#include "../misc/common.h" +#include "../misc/conic-common.h" +//#include "../misc/calcpaths.h" +#include "../kig/kig_part.h" +#include "../kig/kig_view.h" + +static const char constructcenterofcurvaturepoint[] = "SHOULDNOTBESEEN"; +// I18N_NOOP( "Construct the center of curvature corresponding to this point" ); +static const char selectcoc1[] = I18N_NOOP( "Select the curve..." ); +static const char selectcoc2[] = I18N_NOOP( "Select a point on the curve..." ); + +static const ArgsParser::spec argsspecCocConic[] = +{ + { ConicImp::stype(), "SHOULDNOTBESEEN", selectcoc1, false }, + { PointImp::stype(), constructcenterofcurvaturepoint, selectcoc2, false } +}; + +KIG_INSTANTIATE_OBJECT_TYPE_INSTANCE( CocConicType ) + +CocConicType::CocConicType() + : ArgsParserObjectType( "CocConic", argsspecCocConic, 2 ) +{ +} + +CocConicType::~CocConicType() +{ +} + +const CocConicType* CocConicType::instance() +{ + static const CocConicType t; + return &t; +} + +ObjectImp* CocConicType::calc( const Args& args, const KigDocument& doc ) const +{ + if ( !margsparser.checkArgs( args ) ) + return new InvalidImp; + + const ConicImp* conic = static_cast<const ConicImp*>( args[0] ); + const Coordinate& p = static_cast<const PointImp*>( args[1] )->coordinate(); + + if ( !conic->containsPoint( p, doc ) ) + return new InvalidImp; + + double x = p.x; + double y = p.y; + ConicCartesianData data = conic->cartesianData(); +// double aconst = data.coeffs[5]; + double ax = data.coeffs[3]; + double ay = data.coeffs[4]; + double axx = data.coeffs[0]; + double axy = data.coeffs[2]; + double ayy = data.coeffs[1]; + +/* + * mp: we need to compute the normal vector and the curvature + * of the curve. The curve (conic) is given in implicit form + * f(x,y) = 0; the gradient of f gives the direction of the + * normal; for the curvature we can use the following formula: + * k = div(grad f/|grad f|) + * + * the hessian matrix has elements [hfxx, hfxy] + * [hfxy, hfyy] + * + * kgf is the curvature multiplied by the norm of grad f + */ + + double gradfx = 2*axx*x + axy*y + ax; + double gradfy = axy*x + 2*ayy*y + ay; + Coordinate gradf = Coordinate ( gradfx, gradfy ); + + double hfxx = 2*axx; + double hfyy = 2*ayy; + double hfxy = axy; + + double kgf = hfxx + hfyy + - (hfxx*gradfx*gradfx + hfyy*gradfy*gradfy + 2*hfxy*gradfx*gradfy) + /(gradfx*gradfx + gradfy*gradfy); + + bool ok = true; + + const Coordinate coc = p - 1/kgf*gradf; + + if ( !ok ) + return new InvalidImp; + + return new PointImp( coc ); +} + +const ObjectImpType* CocConicType::resultId() const +{ + return PointImp::stype(); +} + +/**** Cubic starts here ****/ + +static const ArgsParser::spec argsspecCocCubic[] = +{ + { CubicImp::stype(), "SHOULDNOTBESEEN", selectcoc1, false }, + { PointImp::stype(), constructcenterofcurvaturepoint, selectcoc2, false } +}; + +KIG_INSTANTIATE_OBJECT_TYPE_INSTANCE( CocCubicType ) + +CocCubicType::CocCubicType() + : ArgsParserObjectType( "CocCubic", argsspecCocCubic, 2 ) +{ +} + +CocCubicType::~CocCubicType() +{ +} + +const CocCubicType* CocCubicType::instance() +{ + static const CocCubicType t; + return &t; +} + +ObjectImp* CocCubicType::calc( const Args& args, const KigDocument& doc ) const +{ + if ( !margsparser.checkArgs( args ) ) + return new InvalidImp; + + const CubicImp* cubic = static_cast<const CubicImp*>( args[0] ); + const Coordinate& p = static_cast<const PointImp*>( args[1] )->coordinate(); + + if ( !cubic->containsPoint( p, doc ) ) + return new InvalidImp; + + double x = p.x; + double y = p.y; + CubicCartesianData data = cubic->data(); +// double aconst = data.coeffs[0]; + double ax = data.coeffs[1]; + double ay = data.coeffs[2]; + double axx = data.coeffs[3]; + double axy = data.coeffs[4]; + double ayy = data.coeffs[5]; + double axxx = data.coeffs[6]; + double axxy = data.coeffs[7]; + double axyy = data.coeffs[8]; + double ayyy = data.coeffs[9]; + + /* + * we use here the same mechanism as for the + * conics, see above + */ + + double gradfx = 3*axxx*x*x + 2*axxy*x*y + axyy*y*y + 2*axx*x + axy*y + ax; + double gradfy = axxy*x*x + 2*axyy*x*y + 3*ayyy*y*y + axy*x + 2*ayy*y + ay; + Coordinate gradf = Coordinate ( gradfx, gradfy ); + + double hfxx = 6*axxx*x + 2*axxy*y + 2*axx; + double hfyy = 6*ayyy*y + 2*axyy*x + 2*ayy; + double hfxy = 2*axxy*x + 2*axyy*y + axy; + + double kgf = hfxx + hfyy + - (hfxx*gradfx*gradfx + hfyy*gradfy*gradfy + 2*hfxy*gradfx*gradfy) + /(gradfx*gradfx + gradfy*gradfy); + + bool ok = true; + + const Coordinate coc = p - 1/kgf*gradf; + + if ( !ok ) + return new InvalidImp; + + return new PointImp( coc ); +} + +const ObjectImpType* CocCubicType::resultId() const +{ + return PointImp::stype(); +} + +/**** Curve starts here ****/ + +static const ArgsParser::spec argsspecCocCurve[] = +{ + { CurveImp::stype(), "SHOULDNOTBESEEN", selectcoc1, false }, + { PointImp::stype(), constructcenterofcurvaturepoint, selectcoc2, false } +}; + +KIG_INSTANTIATE_OBJECT_TYPE_INSTANCE( CocCurveType ) + +CocCurveType::CocCurveType() + : ArgsParserObjectType( "CocCurve", argsspecCocCurve, 2 ) +{ +} + +CocCurveType::~CocCurveType() +{ +} + +const CocCurveType* CocCurveType::instance() +{ + static const CocCurveType t; + return &t; +} + +ObjectImp* CocCurveType::calc( const Args& args, const KigDocument& doc ) const +{ + if ( !margsparser.checkArgs( args ) ) + return new InvalidImp; + + const CurveImp* curve = static_cast<const CurveImp*>( args[0] ); + const Coordinate& p = static_cast<const PointImp*>( args[1] )->coordinate(); + + if ( !curve->containsPoint( p, doc ) ) + return new InvalidImp; + + + const double t = curve->getParam( p, doc ); + const double tau0 = 5e-4; + const double sigmasq = 1e-12; + const int maxiter = 20; + + double tau = tau0; + Coordinate gminus, g, gplus, tang, acc, curv, err; + double velsq, curvsq; + double tplus = t + tau; + double tminus = t - tau; + double t0 = t; + if ( tplus > 1 ) {tplus = 1; t0 = 1 - tau; tminus = 1 - 2*tau;} + if ( tminus < 0 ) {tminus = 0; t0 = tau; tplus = 2*tau;} + gminus = curve->getPoint( tminus, doc ); + g = curve->getPoint( t0, doc ); + gplus = curve->getPoint( tplus, doc ); + tang = (gplus - gminus)/(2*tau); + acc = (gminus + gplus - 2*g)/(tau*tau); + velsq = tang.x*tang.x + tang.y*tang.y; + tang = tang/velsq; + Coordinate curvold = acc/velsq - (acc.x*tang.x + acc.y*tang.y)*tang; + curvsq = curvold.x*curvold.x + curvold.y*curvold.y; + curvold = curvold/curvsq; + + for (int i = 0; i < maxiter; i++) + { + tau = tau/2; + tplus = t + tau; + tminus = t - tau; + t0 = t; + if ( tplus > 1 ) {tplus = 1; t0 = 1 - tau; tminus = 1 - 2*tau;} + if ( tminus < 0 ) {tminus = 0; t0 = tau; tplus = 2*tau;} + + gminus = curve->getPoint( tminus, doc ); + g = curve->getPoint( t0, doc ); + gplus = curve->getPoint( tplus, doc ); + tang = (gplus - gminus)/(2*tau); + acc = (gminus + gplus - 2*g)/(tau*tau); + velsq = tang.x*tang.x + tang.y*tang.y; + tang = tang/velsq; + curv = acc/velsq - (acc.x*tang.x + acc.y*tang.y)*tang; + curvsq = curv.x*curv.x + curv.y*curv.y; + curv = curv/curvsq; + + err = (curvold - curv)/3; + /* + * curvsq is the inverse squared of the norm of curvsq + * so this is actually a relative test + * in the end we return an extrapolated value + */ + if (err.squareLength() < sigmasq/curvsq) + { + curv = (4*curv - curvold)/3; + return new PointImp( p + curv ); + } + curvold = curv; + } + return new InvalidImp; +} + +const ObjectImpType* CocCurveType::resultId() const +{ + return PointImp::stype(); +} |