kig/objects/centerofcurvature_type.cc

   1 // Copyright (C)  2004  Maurizio Paolini <paolini@dmf.unicatt.it>
   2
   3 // This program is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU General Public License
   5 // as published by the Free Software Foundation; either version 2
   6 // of the License, or (at your option) any later version.
   7
   8 // This program is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11 // GNU General Public License for more details.
  12
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
  16 // 02111-1307, USA.
  17
  18 #include "centerofcurvature_type.h"
  19
  20 #include "bogus_imp.h"
  21 #include "conic_imp.h"
  22 #include "cubic_imp.h"
  23 //#include "other_imp.h"
  24 #include "point_imp.h"
  25 //#include "line_imp.h"
  26
  27 #include "../misc/common.h"
  28 #include "../misc/conic-common.h"
  29 //#include "../misc/calcpaths.h"
  30 #include "../kig/kig_part.h"
  31 #include "../kig/kig_view.h"
  32
  33 static const char constructcenterofcurvaturepoint[] = "SHOULDNOTBESEEN";
  34 //  I18N_NOOP( "Construct the center of curvature corresponding to this point" );
  35 static const char selectcoc1[] = I18N_NOOP( "Select the curve..." );
  36 static const char selectcoc2[] = I18N_NOOP( "Select a point on the curve..." );
  37
  38 static const ArgsParser::spec argsspecCocConic[] =
  39 {
  40   { ConicImp::stype(), "SHOULDNOTBESEEN", selectcoc1, false },
  41   { PointImp::stype(), constructcenterofcurvaturepoint, selectcoc2, false }
  42 };
  43
  44 KIG_INSTANTIATE_OBJECT_TYPE_INSTANCE( CocConicType )
  45
  46 CocConicType::CocConicType()
  47   : ArgsParserObjectType( "CocConic", argsspecCocConic, 2 )
  48 {
  49 }
  50
  51 CocConicType::~CocConicType()
  52 {
  53 }
  54
  55 const CocConicType* CocConicType::instance()
  56 {
  57   static const CocConicType t;
  58   return &t;
  59 }
  60
  61 ObjectImp* CocConicType::calc( const Args& args, const KigDocument& doc ) const
  62 {
  63   if ( !margsparser.checkArgs( args ) )
  64     return new InvalidImp;
  65
  66   const ConicImp* conic = static_cast<const ConicImp*>( args[0] );
  67   const Coordinate& p = static_cast<const PointImp*>( args[1] )->coordinate();
  68
  69   if ( !conic->containsPoint( p, doc ) )
  70     return new InvalidImp;
  71
  72   double x = p.x;
  73   double y = p.y;
  74   ConicCartesianData data = conic->cartesianData();
  75 //  double aconst = data.coeffs[5];
  76   double ax = data.coeffs[3];
  77   double ay = data.coeffs[4];
  78   double axx = data.coeffs[0];
  79   double axy = data.coeffs[2];
  80   double ayy = data.coeffs[1];
  81
  82 /*
  83  * mp: we need to compute the normal vector and the curvature
  84  * of the curve.  The curve (conic) is given in implicit form
  85  * f(x,y) = 0;  the gradient of f gives the direction of the
  86  * normal; for the curvature we can use the following formula:
  87  * k = div(grad f/|grad f|)
  88  *
  89  * the hessian matrix has elements [hfxx, hfxy]
  90  *                                 [hfxy, hfyy]
  91  *
  92  * kgf is the curvature multiplied by the norm of grad f
  93  */
  94
  95   double gradfx = 2*axx*x + axy*y + ax;
  96   double gradfy = axy*x + 2*ayy*y + ay;
  97   Coordinate gradf = Coordinate ( gradfx, gradfy );
  98
  99   double hfxx = 2*axx;
 100   double hfyy = 2*ayy;
 101   double hfxy = axy;
 102
 103   double kgf = hfxx + hfyy
 104      - (hfxx*gradfx*gradfx + hfyy*gradfy*gradfy + 2*hfxy*gradfx*gradfy)
 105        /(gradfx*gradfx + gradfy*gradfy);
 106
 107   bool ok = true;
 108
 109   const Coordinate coc = p - 1/kgf*gradf;
 110
 111   if ( !ok )
 112     return new InvalidImp;
 113
 114   return new PointImp( coc );
 115 }
 116
 117 const ObjectImpType* CocConicType::resultId() const
 118 {
 119   return PointImp::stype();
 120 }
 121
 122 /**** Cubic starts here ****/
 123
 124 static const ArgsParser::spec argsspecCocCubic[] =
 125 {
 126   { CubicImp::stype(), "SHOULDNOTBESEEN", selectcoc1, false },
 127   { PointImp::stype(), constructcenterofcurvaturepoint, selectcoc2, false }
 128 };
 129
 130 KIG_INSTANTIATE_OBJECT_TYPE_INSTANCE( CocCubicType )
 131
 132 CocCubicType::CocCubicType()
 133   : ArgsParserObjectType( "CocCubic", argsspecCocCubic, 2 )
 134 {
 135 }
 136
 137 CocCubicType::~CocCubicType()
 138 {
 139 }
 140
 141 const CocCubicType* CocCubicType::instance()
 142 {
 143   static const CocCubicType t;
 144   return &t;
 145 }
 146
 147 ObjectImp* CocCubicType::calc( const Args& args, const KigDocument& doc ) const
 148 {
 149   if ( !margsparser.checkArgs( args ) )
 150     return new InvalidImp;
 151
 152   const CubicImp* cubic = static_cast<const CubicImp*>( args[0] );
 153   const Coordinate& p = static_cast<const PointImp*>( args[1] )->coordinate();
 154
 155   if ( !cubic->containsPoint( p, doc ) )
 156     return new InvalidImp;
 157
 158   double x = p.x;
 159   double y = p.y;
 160   CubicCartesianData data = cubic->data();
 161 //  double aconst = data.coeffs[0];
 162   double ax = data.coeffs[1];
 163   double ay = data.coeffs[2];
 164   double axx = data.coeffs[3];
 165   double axy = data.coeffs[4];
 166   double ayy = data.coeffs[5];
 167   double axxx = data.coeffs[6];
 168   double axxy = data.coeffs[7];
 169   double axyy = data.coeffs[8];
 170   double ayyy = data.coeffs[9];
 171
 172   /*
 173    * we use here the same mechanism as for the
 174    * conics, see above
 175    */
 176
 177   double gradfx = 3*axxx*x*x + 2*axxy*x*y + axyy*y*y + 2*axx*x + axy*y + ax;
 178   double gradfy = axxy*x*x + 2*axyy*x*y + 3*ayyy*y*y + axy*x + 2*ayy*y + ay;
 179   Coordinate gradf = Coordinate ( gradfx, gradfy );
 180
 181   double hfxx = 6*axxx*x + 2*axxy*y + 2*axx;
 182   double hfyy = 6*ayyy*y + 2*axyy*x + 2*ayy;
 183   double hfxy = 2*axxy*x + 2*axyy*y + axy;
 184
 185   double kgf = hfxx + hfyy
 186      - (hfxx*gradfx*gradfx + hfyy*gradfy*gradfy + 2*hfxy*gradfx*gradfy)
 187        /(gradfx*gradfx + gradfy*gradfy);
 188
 189   bool ok = true;
 190
 191   const Coordinate coc = p - 1/kgf*gradf;
 192
 193   if ( !ok )
 194     return new InvalidImp;
 195
 196   return new PointImp( coc );
 197 }
 198
 199 const ObjectImpType* CocCubicType::resultId() const
 200 {
 201   return PointImp::stype();
 202 }
 203
 204 /**** Curve starts here ****/
 205
 206 static const ArgsParser::spec argsspecCocCurve[] =
 207 {
 208   { CurveImp::stype(), "SHOULDNOTBESEEN", selectcoc1, false },
 209   { PointImp::stype(), constructcenterofcurvaturepoint, selectcoc2, false }
 210 };
 211
 212 KIG_INSTANTIATE_OBJECT_TYPE_INSTANCE( CocCurveType )
 213
 214 CocCurveType::CocCurveType()
 215   : ArgsParserObjectType( "CocCurve", argsspecCocCurve, 2 )
 216 {
 217 }
 218
 219 CocCurveType::~CocCurveType()
 220 {
 221 }
 222
 223 const CocCurveType* CocCurveType::instance()
 224 {
 225   static const CocCurveType t;
 226   return &t;
 227 }
 228
 229 ObjectImp* CocCurveType::calc( const Args& args, const KigDocument& doc ) const
 230 {
 231   if ( !margsparser.checkArgs( args ) )
 232     return new InvalidImp;
 233
 234   const CurveImp* curve = static_cast<const CurveImp*>( args[0] );
 235   const Coordinate& p = static_cast<const PointImp*>( args[1] )->coordinate();
 236
 237   if ( !curve->containsPoint( p, doc ) )
 238     return new InvalidImp;
 239
 240
 241   const double t = curve->getParam( p, doc );
 242   const double tau0 = 5e-4;
 243   const double sigmasq = 1e-12;
 244   const int maxiter = 20;
 245
 246   double tau = tau0;
 247   Coordinate gminus, g, gplus, tang, acc, curv, err;
 248   double velsq, curvsq;
 249   double tplus = t + tau;
 250   double tminus = t - tau;
 251   double t0 = t;
 252   if ( tplus > 1 ) {tplus = 1; t0 = 1 - tau; tminus = 1 - 2*tau;}
 253   if ( tminus < 0 ) {tminus = 0; t0 = tau; tplus = 2*tau;}
 254   gminus = curve->getPoint( tminus, doc );
 255   g = curve->getPoint( t0, doc );
 256   gplus = curve->getPoint( tplus, doc );
 257   tang = (gplus - gminus)/(2*tau);
 258   acc = (gminus + gplus - 2*g)/(tau*tau);
 259   velsq = tang.x*tang.x + tang.y*tang.y;
 260   tang = tang/velsq;
 261   Coordinate curvold = acc/velsq - (acc.x*tang.x + acc.y*tang.y)*tang;
 262   curvsq = curvold.x*curvold.x + curvold.y*curvold.y;
 263   curvold = curvold/curvsq;
 264
 265   for (int i = 0; i < maxiter; i++)
 266   {
 267     tau = tau/2;
 268     tplus = t + tau;
 269     tminus = t - tau;
 270     t0 = t;
 271     if ( tplus > 1 ) {tplus = 1; t0 = 1 - tau; tminus = 1 - 2*tau;}
 272     if ( tminus < 0 ) {tminus = 0; t0 = tau; tplus = 2*tau;}
 273
 274     gminus = curve->getPoint( tminus, doc );
 275     g = curve->getPoint( t0, doc );
 276     gplus = curve->getPoint( tplus, doc );
 277     tang = (gplus - gminus)/(2*tau);
 278     acc = (gminus + gplus - 2*g)/(tau*tau);
 279     velsq = tang.x*tang.x + tang.y*tang.y;
 280     tang = tang/velsq;
 281     curv = acc/velsq - (acc.x*tang.x + acc.y*tang.y)*tang;
 282     curvsq = curv.x*curv.x + curv.y*curv.y;
 283     curv = curv/curvsq;
 284
 285     err = (curvold - curv)/3;
 286     /*
 287      * curvsq is the inverse squared of the norm of curvsq
 288      * so this is actually a relative test
 289      * in the end we return an extrapolated value
 290      */
 291     if (err.squareLength() < sigmasq/curvsq)
 292     {
 293       curv = (4*curv - curvold)/3;
 294       return new PointImp( p + curv );
 295     }
 296     curvold = curv;
 297   }
 298   return new InvalidImp;
 299 }
 300
 301 const ObjectImpType* CocCurveType::resultId() const
 302 {
 303   return PointImp::stype();
 304 }