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();
+}