Working on BSPline interpolator

8 files changed, 338 insertions, 238 deletions

diff --git a/src/live_effects/lpe-interpolate_points.cpp b/src/live_effects/lpe-interpolate_points.cpp
index 7d4c88dc1..c745921c2 100644
--- a/src/live_effects/lpe-interpolate_points.cpp
+++ b/src/live_effects/lpe-interpolate_points.cpp
@@ -25,7 +25,8 @@ static const Util::EnumData<unsigned> InterpolatorTypeData[] = {
     {Geom::Interpolate::INTERP_CUBICBEZIER          , N_("CubicBezierFit"), "CubicBezierFit"},
     {Geom::Interpolate::INTERP_CUBICBEZIER_JOHAN     , N_("CubicBezierJohan"), "CubicBezierJohan"},
     {Geom::Interpolate::INTERP_SPIRO  , N_("SpiroInterpolator"), "SpiroInterpolator"},
-    {Geom::Interpolate::INTERP_CENTRIPETAL_CATMULLROM, N_("Centripetal Catmull-Rom"), "CentripetalCatmullRom"}
+    {Geom::Interpolate::INTERP_CENTRIPETAL_CATMULLROM, N_("Centripetal Catmull-Rom"), "CentripetalCatmullRom"},
+    {Geom::Interpolate::INTERP_BSPLINE          , N_("BSpline"), "BSpline"}
 };
 static const Util::EnumDataConverter<unsigned> InterpolatorTypeConverter(InterpolatorTypeData, sizeof(InterpolatorTypeData)/sizeof(*InterpolatorTypeData));
 
diff --git a/src/live_effects/lpe-powerstroke-interpolators.h b/src/live_effects/lpe-powerstroke-interpolators.h
index e3ab37e27..651eba057 100644
--- a/src/live_effects/lpe-powerstroke-interpolators.h
+++ b/src/live_effects/lpe-powerstroke-interpolators.h
@@ -15,7 +15,7 @@
 #include <2geom/path.h>
 #include <2geom/bezier-utils.h>
 #include <2geom/sbasis-to-bezier.h>
-
+#include <math.h>
 #include "live_effects/spiro.h"
 
 
@@ -27,6 +27,7 @@ enum InterpolatorType {
   INTERP_LINEAR,
   INTERP_CUBICBEZIER,
   INTERP_CUBICBEZIER_JOHAN,
+  INTERP_BSPLINE,
   INTERP_SPIRO,
   INTERP_CUBICBEZIER_SMOOTH,
   INTERP_CENTRIPETAL_CATMULLROM
@@ -65,6 +66,38 @@ private:
     Linear& operator=(const Linear&);
 };
 
+class BSpline : public Interpolator {
+public:
+    BSpline() {};
+    virtual ~BSpline() {};
+
+    virtual Path interpolateToPath(std::vector<Point> const &points) const {
+        Path path;
+        path.start( points.at(0) );
+        for (unsigned int i = 1 ; i < points.size(); ++i) {
+            Geom::Point pointA = points.at(i-1);
+            Geom::Point pointB = points.at(i);
+            Geom::Ray ray(pointA, pointB);
+            double angle = ray.angle();
+            if (angle == 0) {
+                continue;
+            }
+            std::cout << angle << "angle" << std::endl;
+            double k1 = (Geom::distance(pointA, pointB)*std::sin(angle))/2.0;
+                        std::cout << k1 << "k1" << std::endl;
+                                    std::cout << std::sin(angle) << "std::sin(angle)" << std::endl;
+            Geom::Point handle_1 = Geom::Point::polar(angle, k1) + pointA;
+            Geom::Point handle_2 = Geom::Point::polar(angle, k1 * 2) + pointA;
+            path.appendNew<CubicBezier>(handle_1, handle_2, pointB);
+        }
+        return path;
+    };
+
+private:
+    BSpline(const BSpline&);
+    BSpline& operator=(const BSpline&);
+};
+
 // this class is terrible
 class CubicBezierFit : public Interpolator {
 public:
@@ -302,6 +335,8 @@ Interpolator::create(InterpolatorType type) {
         return new Geom::Interpolate::CubicBezierSmooth();
       case INTERP_CENTRIPETAL_CATMULLROM:
         return new Geom::Interpolate::CentripetalCatmullRomInterpolator();
+      case INTERP_BSPLINE:
+        return new Geom::Interpolate::BSpline();
       default:
         return new Geom::Interpolate::Linear();
     }
diff --git a/src/live_effects/lpe-powerstroke.cpp b/src/live_effects/lpe-powerstroke.cpp
index e9943af4c..2b8ca85c7 100644
--- a/src/live_effects/lpe-powerstroke.cpp
+++ b/src/live_effects/lpe-powerstroke.cpp
@@ -123,7 +123,8 @@ static const Util::EnumData<unsigned> InterpolatorTypeData[] = {
     {Geom::Interpolate::INTERP_CUBICBEZIER          , N_("CubicBezierFit"), "CubicBezierFit"},
     {Geom::Interpolate::INTERP_CUBICBEZIER_JOHAN     , N_("CubicBezierJohan"), "CubicBezierJohan"},
     {Geom::Interpolate::INTERP_SPIRO  , N_("SpiroInterpolator"), "SpiroInterpolator"},
-    {Geom::Interpolate::INTERP_CENTRIPETAL_CATMULLROM, N_("Centripetal Catmull-Rom"), "CentripetalCatmullRom"}
+    {Geom::Interpolate::INTERP_CENTRIPETAL_CATMULLROM, N_("Centripetal Catmull-Rom"), "CentripetalCatmullRom"},
+    {Geom::Interpolate::INTERP_BSPLINE          , N_("BSpline"), "BSpline"}
 };
 static const Util::EnumDataConverter<unsigned> InterpolatorTypeConverter(InterpolatorTypeData, sizeof(InterpolatorTypeData)/sizeof(*InterpolatorTypeData));
 
@@ -626,12 +627,23 @@ LPEPowerStroke::doEffect_path (Geom::PathVector const & path_in)
     if (Geom::Interpolate::CubicBezierSmooth *smooth = dynamic_cast<Geom::Interpolate::CubicBezierSmooth*>(interpolator)) {
         smooth->setBeta(interpolator_beta);
     }
+    bool is_bspline = false;
+    if (Geom::Interpolate::BSpline *bspline = dynamic_cast<Geom::Interpolate::BSpline*>(interpolator)) {
+        is_bspline = true;
+    }
     Geom::Path strokepath = interpolator->interpolateToPath(ts);
-    delete interpolator;
+    std::vector<Geom::Point> mirrorts;
+    for(auto point:ts) {
+        Geom::Point p = Geom::Point(point[Geom::X],point[Geom::Y]*-1);
+        mirrorts.push_back(p);
+    }
+    Geom::Path strokepath_mirror = interpolator->interpolateToPath(mirrorts);
 
     // apply the inverse knot-xcoord scaling that was applied before the interpolation
     strokepath *= Scale(1/xcoord_scaling, 1);
-
+    strokepath_mirror *= Scale(1/xcoord_scaling, 1);
+    Geom::Path fixed_path = strokepath;
+    Geom::Path fixed_mirrorpath = strokepath_mirror;
     D2<Piecewise<SBasis> > patternd2 = make_cuts_independent(strokepath.toPwSb());
     Piecewise<SBasis> x = Piecewise<SBasis>(patternd2[0]);
     Piecewise<SBasis> y = Piecewise<SBasis>(patternd2[1]);
@@ -643,12 +655,67 @@ LPEPowerStroke::doEffect_path (Geom::PathVector const & path_in)
         x = portion(x, rtsmin.at(0), rtsmax.at(0));
         y = portion(y, rtsmin.at(0), rtsmax.at(0));
     }
+    if (!is_bspline) {
 
-    LineJoinType jointype = static_cast<LineJoinType>(linejoin_type.get_value());
-    Piecewise<D2<SBasis> > pwd2_out   =          compose(pwd2_in,x) + y*compose(n,x);
-    Piecewise<D2<SBasis> > mirrorpath = reverse( compose(pwd2_in,x) - y*compose(n,x));
-    Geom::Path fixed_path       = path_from_piecewise_fix_cusps( pwd2_out,   y,          jointype, miter_limit, LPE_CONVERSION_TOLERANCE);
-    Geom::Path fixed_mirrorpath = path_from_piecewise_fix_cusps( mirrorpath, reverse(y), jointype, miter_limit, LPE_CONVERSION_TOLERANCE);
+        LineJoinType jointype = static_cast<LineJoinType>(linejoin_type.get_value());
+        Piecewise<D2<SBasis> > pwd2_out   =          compose(pwd2_in,x) + y*compose(n,x);
+        Piecewise<D2<SBasis> > mirrorpath = reverse( compose(pwd2_in,x) - y*compose(n,x));
+        fixed_path       = path_from_piecewise_fix_cusps( pwd2_out,   y,          jointype, miter_limit, LPE_CONVERSION_TOLERANCE);
+        fixed_mirrorpath = path_from_piecewise_fix_cusps( mirrorpath, reverse(y), jointype, miter_limit, LPE_CONVERSION_TOLERANCE);
+    } else {
+        std::vector<Geom::Point> ts_pos;
+        for(auto point:ts) {
+            if (point[Geom::X] > pwd2_in.size() || point[Geom::X] < 0) {
+                g_warning("Broken powerstroke point at %f, I won't try to add that", point[Geom::X]);
+                continue;
+            }
+            point = pwd2_in.valueAt(point[Geom::X]) + (point[Geom::Y] * scale_width) * n.valueAt(point[Geom::X]);
+            ts_pos.push_back(point);
+        }
+        strokepath = interpolator->interpolateToPath(ts_pos);
+        std::vector<Geom::Point> ts_pos_mirror;
+        for(auto point:ts) {
+            point = Geom::Point(point[Geom::X],point[Geom::Y]*-1);
+            if (point[Geom::X] > pwd2_in.size() || point[Geom::X] < 0) {
+                g_warning("Broken powerstroke point at %f, I won't try to add that", point[Geom::X]);
+                continue;
+            }
+            point = pwd2_in.valueAt(point[Geom::X]) + (point[Geom::Y] * scale_width) * n.valueAt(point[Geom::X]);
+            ts_pos_mirror.push_back(point);
+        }
+        strokepath_mirror = interpolator->interpolateToPath(ts_pos_mirror);
+//        Geom::Path fixed_path;
+//        fixed_path.start(strokepath.initialPoint());
+//        for (Geom::Path::const_iterator curve = strokepath->begin(); curve !=  strokepath->end(); ++curve) {
+//            Geom::Point pointA = curve.initialPoint();
+//            Geom::Point pointB = curve.finalPoint();
+//            double angle = pointA.angle()-pointB.angle();
+//            if (angle==0) {
+//                continue;
+//            }
+//            double k1 = (Geom::distance(pointA, pointB)/cos(angle)/5.0) * 2;
+//            Geom::Point handle_1 = Geom::Point::polar(angle, k1) + pointA;
+//            Geom::Point handle_2 = Geom::Point::polar(angle, k1 * 2) + pointA;
+//            fixed_path..appendNew<CubicBezier>(handle_1, handle_2, pointB);
+//        }
+//        
+//        Geom::Path fixed_mirrorpath;
+//        fixed_mirrorpath.start(strokepath_mirror.initialPoint());
+//        for (Geom::Path::const_iterator curve = strokepath_mirror->begin(); curve !=  strokepath_mirror->end(); ++curve) {
+//            Geom::Point pointA = curve.initialPoint();
+//            Geom::Point pointB = curve.finalPoint();
+//            double angle = pointA.angle()-pointB.angle();
+//            if (angle==0) {
+//                continue;
+//            }
+//            double k1 = (Geom::distance(pointA, pointB)/cos(angle)/5.0) * 2;
+//            Geom::Point handle_1 = Geom::Point::polar(angle, k1) + pointA;
+//            Geom::Point handle_2 = Geom::Point::polar(angle, k1 * 2) + pointA;
+//            fixed_mirrorpath..appendNew<CubicBezier>(handle_1, handle_2, pointB);
+//        }
+        
+    }
+    delete interpolator;
     if (pathv[0].closed()) {
         fixed_path.close(true);
         path_out.push_back(fixed_path);
@@ -663,31 +730,31 @@ LPEPowerStroke::doEffect_path (Geom::PathVector const & path_in)
             case LINECAP_PEAK:
             {
                 Geom::Point end_deriv = -unitTangentAt( reverse(pwd2_in.segs.back()), 0.);
-                double radius = 0.5 * distance(pwd2_out.lastValue(), mirrorpath.firstValue());
-                Geom::Point midpoint = 0.5*(pwd2_out.lastValue() + mirrorpath.firstValue()) + radius*end_deriv;
+                double radius = 0.5 * distance(fixed_path.finalPoint(), fixed_mirrorpath.initialPoint());
+                Geom::Point midpoint = 0.5*(fixed_path.finalPoint() + fixed_mirrorpath.initialPoint()) + radius*end_deriv;
                 fixed_path.appendNew<LineSegment>(midpoint);
-                fixed_path.appendNew<LineSegment>(mirrorpath.firstValue());
+                fixed_path.appendNew<LineSegment>(fixed_mirrorpath.initialPoint());
                 break;
             }
             case LINECAP_SQUARE:
             {
                 Geom::Point end_deriv = -unitTangentAt( reverse(pwd2_in.segs.back()), 0.);
-                double radius = 0.5 * distance(pwd2_out.lastValue(), mirrorpath.firstValue());
-                fixed_path.appendNew<LineSegment>( pwd2_out.lastValue() + radius*end_deriv );
-                fixed_path.appendNew<LineSegment>( mirrorpath.firstValue() + radius*end_deriv );
-                fixed_path.appendNew<LineSegment>( mirrorpath.firstValue() );
+                double radius = 0.5 * distance(fixed_path.finalPoint(), fixed_mirrorpath.initialPoint());
+                fixed_path.appendNew<LineSegment>( fixed_path.finalPoint() + radius*end_deriv );
+                fixed_path.appendNew<LineSegment>( fixed_mirrorpath.initialPoint() + radius*end_deriv );
+                fixed_path.appendNew<LineSegment>( fixed_mirrorpath.initialPoint() );
                 break;
             }
             case LINECAP_BUTT:
             {
-                fixed_path.appendNew<LineSegment>( mirrorpath.firstValue() );
+                fixed_path.appendNew<LineSegment>( fixed_mirrorpath.initialPoint() );
                 break;
             }
             case LINECAP_ROUND:
             default:
             {
-                double radius1 = 0.5 * distance(pwd2_out.lastValue(), mirrorpath.firstValue());
-                fixed_path.appendNew<EllipticalArc>( radius1, radius1, M_PI/2., false, y.lastValue() < 0, mirrorpath.firstValue() );
+                double radius1 = 0.5 * distance(fixed_path.finalPoint(), fixed_mirrorpath.initialPoint());
+                fixed_path.appendNew<EllipticalArc>( radius1, radius1, M_PI/2., false, y.lastValue() < 0, fixed_mirrorpath.initialPoint() );
                 break;
             }
         }
@@ -700,31 +767,31 @@ LPEPowerStroke::doEffect_path (Geom::PathVector const & path_in)
             case LINECAP_PEAK:
             {
                 Geom::Point start_deriv = unitTangentAt( pwd2_in.segs.front(), 0.);
-                double radius = 0.5 * distance(pwd2_out.firstValue(), mirrorpath.lastValue());
-                Geom::Point midpoint = 0.5*(mirrorpath.lastValue() + pwd2_out.firstValue()) - radius*start_deriv;
+                double radius = 0.5 * distance(fixed_path.initialPoint(), fixed_mirrorpath.finalPoint());
+                Geom::Point midpoint = 0.5*(fixed_mirrorpath.finalPoint() + fixed_path.initialPoint()) - radius*start_deriv;
                 fixed_path.appendNew<LineSegment>( midpoint );
-                fixed_path.appendNew<LineSegment>( pwd2_out.firstValue() );
+                fixed_path.appendNew<LineSegment>( fixed_path.initialPoint() );
                 break;
             }
             case LINECAP_SQUARE:
             {
                 Geom::Point start_deriv = unitTangentAt( pwd2_in.segs.front(), 0.);
-                double radius = 0.5 * distance(pwd2_out.firstValue(), mirrorpath.lastValue());
-                fixed_path.appendNew<LineSegment>( mirrorpath.lastValue() - radius*start_deriv );
-                fixed_path.appendNew<LineSegment>( pwd2_out.firstValue() - radius*start_deriv );
-                fixed_path.appendNew<LineSegment>( pwd2_out.firstValue() );
+                double radius = 0.5 * distance(fixed_path.initialPoint(), fixed_mirrorpath.finalPoint());
+                fixed_path.appendNew<LineSegment>( fixed_mirrorpath.finalPoint() - radius*start_deriv );
+                fixed_path.appendNew<LineSegment>( fixed_path.initialPoint() - radius*start_deriv );
+                fixed_path.appendNew<LineSegment>( fixed_path.initialPoint() );
                 break;
             }
             case LINECAP_BUTT:
             {
-                fixed_path.appendNew<LineSegment>( pwd2_out.firstValue() );
+                fixed_path.appendNew<LineSegment>( fixed_path.initialPoint() );
                 break;
             }
             case LINECAP_ROUND:
             default:
             {
-                double radius2 = 0.5 * distance(pwd2_out.firstValue(), mirrorpath.lastValue());
-                fixed_path.appendNew<EllipticalArc>( radius2, radius2, M_PI/2., false, y.firstValue() < 0, pwd2_out.firstValue() );
+                double radius2 = 0.5 * distance(fixed_path.initialPoint(), fixed_mirrorpath.finalPoint());
+                fixed_path.appendNew<EllipticalArc>( radius2, radius2, M_PI/2., false, y.firstValue() < 0, fixed_path.initialPoint() );
                 break;
             }
         }
diff --git a/src/ui/tools/freehand-base.cpp b/src/ui/tools/freehand-base.cpp
index 15027b6f3..b685e461e 100644
--- a/src/ui/tools/freehand-base.cpp
+++ b/src/ui/tools/freehand-base.cpp
@@ -85,7 +85,7 @@ FreehandBase::FreehandBase(gchar const *const *cursor_shape)
     , green_anchor(NULL)
     , green_closed(false)
     , white_item(NULL)
-    , overwrite_curve(NULL)
+    , sa_overwrited(NULL)
     , sa(NULL)
     , ea(NULL)
     , waiting_LPE_type(Inkscape::LivePathEffect::INVALID_LPE)
@@ -144,7 +144,7 @@ void FreehandBase::setup() {
     this->green_closed = FALSE;
 
     // Create start anchor alternative curve
-    this->overwrite_curve = new SPCurve();
+    this->sa_overwrited = new SPCurve();
 
     this->attach = TRUE;
     spdc_attach_selection(this, this->selection);
@@ -254,15 +254,22 @@ static void spdc_apply_powerstroke_shape(std::vector<Geom::Point> points, Freeha
                 }
                 pt->points.push_back(Geom::Point(0, swidth));
             }
-            Effect::createAndApply(POWERSTROKE, dc->desktop->doc(), item);
             Effect* lpe = SP_LPE_ITEM(item)->getCurrentLPE();
-            lpe->getRepr()->setAttribute("sort_points", "true");
-            lpe->getRepr()->setAttribute("interpolator_type", "CentripetalCatmullRom");
-            lpe->getRepr()->setAttribute("interpolator_beta", "0.2");
-            lpe->getRepr()->setAttribute("miter_limit", "100");
-            lpe->getRepr()->setAttribute("linejoin_type", "miter");
-            static_cast<LPEPowerStroke*>(lpe)->offset_points.param_set_and_write_new_value(pt->points);
-            pt->points.clear();
+            LPEPowerStroke* ps = static_cast<LPEPowerStroke*>(lpe);
+            if (!ps) {
+                Effect::createAndApply(POWERSTROKE, dc->desktop->doc(), item);
+                lpe = SP_LPE_ITEM(item)->getCurrentLPE();
+                ps = static_cast<LPEPowerStroke*>(lpe);
+            }
+            if (ps) {
+                lpe->getRepr()->setAttribute("sort_points", "true");
+                lpe->getRepr()->setAttribute("interpolator_type", "CentripetalCatmullRom");
+                lpe->getRepr()->setAttribute("interpolator_beta", "0.2");
+                lpe->getRepr()->setAttribute("miter_limit", "100");
+                lpe->getRepr()->setAttribute("linejoin_type", "miter");
+                ps->offset_points.param_set_and_write_new_value(pt->points);
+                pt->points.clear();
+            }
             return;
         }
     }
@@ -596,6 +603,9 @@ static void spdc_selection_modified(Inkscape::Selection *sel, guint /*flags*/, F
 
 static void spdc_attach_selection(FreehandBase *dc, Inkscape::Selection */*sel*/)
 {
+    if (SP_IS_PENCIL_CONTEXT(dc) && dc->sa && dc->input_has_pressure) {
+        return;
+    }
     // We reset white and forget white/start/end anchors
     spdc_reset_white(dc);
     dc->sa = NULL;
@@ -718,7 +728,6 @@ void spdc_concat_colors_and_flush(FreehandBase *dc, gboolean forceclosed)
         c->unref();
         return;
     }
-
     // Step A - test, whether we ended on green anchor
     if ( (forceclosed && 
          (!dc->sa || (dc->sa && dc->sa->curve->is_empty()))) || 
@@ -732,7 +741,6 @@ void spdc_concat_colors_and_flush(FreehandBase *dc, gboolean forceclosed)
         c->unref();
         return;
     }
-
     // Step B - both start and end anchored to same curve
     if ( dc->sa && dc->ea
          && ( dc->sa->curve == dc->ea->curve )
@@ -741,38 +749,21 @@ void spdc_concat_colors_and_flush(FreehandBase *dc, gboolean forceclosed)
     {
         // We hit bot start and end of single curve, closing paths
         dc->desktop->messageStack()->flash(Inkscape::NORMAL_MESSAGE, _("Closing path."));
-        if (dc->sa->start && !(dc->sa->curve->is_closed()) ) {
-            c = reverse_then_unref(c);
-        }
-        if(prefs->getInt(tool_name(dc) + "/freehand-mode", 0) == 1 || 
-            prefs->getInt(tool_name(dc) + "/freehand-mode", 0) == 2){
-            dc->overwrite_curve->append_continuous(c, 0.0625);
-            c->unref();
-            dc->overwrite_curve->closepath_current();
-            if(dc->sa){
-                dc->white_curves.erase(std::find(dc->white_curves.begin(),dc->white_curves.end(), dc->sa->curve));
-                dc->white_curves.push_back(dc->overwrite_curve);
-            }
-        }else{
-            dc->sa->curve->append_continuous(c, 0.0625);
-            c->unref();
-            dc->sa->curve->closepath_current();
+        dc->sa_overwrited->append_continuous(c, 0.0625);
+        c->unref();
+        dc->sa_overwrited->closepath_current();
+        if(dc->sa){
+            dc->white_curves.erase(std::find(dc->white_curves.begin(),dc->white_curves.end(), dc->sa->curve));
+            dc->white_curves.push_back(dc->sa_overwrited);
         }
+        
         spdc_flush_white(dc, NULL);
         return;
     }
-
     // Step C - test start
     if (dc->sa) {
-        SPCurve *s = dc->sa->curve;
-        dc->white_curves.erase(std::find(dc->white_curves.begin(),dc->white_curves.end(), s));
-        if(prefs->getInt(tool_name(dc) + "/freehand-mode", 0) == 1 || 
-            prefs->getInt(tool_name(dc) + "/freehand-mode", 0) == 2){
-                s = dc->overwrite_curve;
-        }
-        if (dc->sa->start) {
-            s = reverse_then_unref(s);
-        }
+        dc->white_curves.erase(std::find(dc->white_curves.begin(),dc->white_curves.end(), dc->sa->curve));
+        SPCurve *s = dc->sa_overwrited;
         s->append_continuous(c, 0.0625);
         c->unref();
         c = s;
@@ -804,7 +795,6 @@ void spdc_concat_colors_and_flush(FreehandBase *dc, gboolean forceclosed)
         c->append_continuous(e, 0.0625);
         e->unref();
     }
-
     if (forceclosed) 
     {
         dc->desktop->messageStack()->flash(Inkscape::NORMAL_MESSAGE, _("Path is closed."));
@@ -869,7 +859,7 @@ static void spdc_flush_white(FreehandBase *dc, SPCurve *gc)
             Inkscape::Preferences *prefs = Inkscape::Preferences::get();
             SPItem *item = SP_ITEM(desktop->currentLayer()->appendChildRepr(repr));
             //Bend needs the transforms applied after, Other effects best before
-            (dc, item, c, true);
+            spdc_check_for_and_apply_waiting_LPE(dc, item, c, true);
             Inkscape::GC::release(repr);
             item->transform = SP_ITEM(desktop->currentLayer())->i2doc_affine().inverse();
             item->updateRepr();
@@ -879,8 +869,17 @@ static void spdc_flush_white(FreehandBase *dc, SPCurve *gc)
             if(previous_shape_type == BEND_CLIPBOARD){
                 repr->parent()->removeChild(repr);
             }
+        } else if (SP_IS_PENCIL_CONTEXT(dc)) {
+            if (dc->input_has_pressure) {
+                spdc_check_for_and_apply_waiting_LPE(dc,  dc->white_item, c, false);
+//                Inkscape::Preferences *prefs = Inkscape::Preferences::get();
+//                shapeType shape = (shapeType)prefs->getInt(tool_name(dc) + "/shape", 0);
+//                if (shape == NONE) {
+//                    std::vector<Geom::Point> points;
+//                    spdc_apply_powerstroke_shape(points, dc, dc->white_item);
+//                }
+            }
         }
-
         DocumentUndo::done(doc, SP_IS_PEN_CONTEXT(dc)? SP_VERB_CONTEXT_PEN : SP_VERB_CONTEXT_PENCIL,
                          _("Draw path"));
 
@@ -949,7 +948,11 @@ static void spdc_free_colors(FreehandBase *dc)
     if (dc->blue_curve) {
         dc->blue_curve = dc->blue_curve->unref();
     }
-
+    
+    // Overwrite start anchor curve
+    if (dc->sa_overwrited) {
+        dc->sa_overwrited = dc->sa_overwrited->unref();
+    }
     // Green
     for (auto i : dc->green_bpaths)
         sp_canvas_item_destroy(i);
diff --git a/src/ui/tools/freehand-base.h b/src/ui/tools/freehand-base.h
index 02d0a9982..4a14cf8d3 100644
--- a/src/ui/tools/freehand-base.h
+++ b/src/ui/tools/freehand-base.h
@@ -76,11 +76,8 @@ public:
     std::list<SPCurve *> white_curves;
     std::vector<SPDrawAnchor*> white_anchors;
 
-    // Alternative curve to use on continuing the exisiting curve in case of
-    // bspline or spirolive, because using anchor curves gives random memory
-    // bugs as reported by su_v when running this code on macOS (as well as
-    // making the code hard to understand).
-    SPCurve *overwrite_curve;
+    // Temporary modiffied curve when start anchor
+    SPCurve *sa_overwrited;
 
     // Start anchor
     SPDrawAnchor *sa;
diff --git a/src/ui/tools/pen-tool.cpp b/src/ui/tools/pen-tool.cpp
index 16cdf63b5..f42af8bb6 100644
--- a/src/ui/tools/pen-tool.cpp
+++ b/src/ui/tools/pen-tool.cpp
@@ -422,8 +422,9 @@ bool PenTool::_handleButtonPress(GdkEventButton const &bevent) {
                     case PenTool::STOP:
                         // This is allowed, if we just canceled curve
                     case PenTool::POINT:
+                        this->setPolylineMode();
+                        this->_bsplineSpiroColor();
                         if (this->npoints == 0) {
-                            this->_bsplineSpiroColor();
                             Geom::Point p;
                             if ((bevent.state & GDK_CONTROL_MASK) && (this->polylines_only || this->polylines_paraxial)) {
                                 p = event_dt;
@@ -444,7 +445,13 @@ bool PenTool::_handleButtonPress(GdkEventButton const &bevent) {
                             // Set start anchor
 
                             this->sa = anchor;
-                            if(anchor){
+                            if (anchor) {
+                                //Put the start overwrite curve always on the same direction
+                                if (anchor->start) {
+                                    this->sa_overwrited = this->sa->curve->create_reverse();
+                                } else {
+                                    this->sa_overwrited = this->sa->curve->copy();
+                                }
                                 this->_bsplineSpiroStartAnchor(bevent.state & GDK_SHIFT_MASK);
                             }
                             if (anchor && (!this->hasWaitingLPE()|| this->bspline || this->spiro)) {
@@ -473,7 +480,6 @@ bool PenTool::_handleButtonPress(GdkEventButton const &bevent) {
                             }
                             this->_setInitialPoint(p);
                         } else {
-
                             // Set end anchor
                             this->ea = anchor;
                             Geom::Point p;
@@ -734,27 +740,30 @@ bool PenTool::_handleButtonRelease(GdkEventButton const &revent) {
             case PenTool::MODE_CLICK:
                 switch (this->state) {
                     case PenTool::POINT:
-                        if ( this->npoints == 0 ) {
-                            // Start new thread only with button release
-                            this->_bsplineSpiroColor();
-                            if (anchor) {
-                                p = anchor->dp;
-                            }
-                            this->sa = anchor;
-                            // continue the existing curve
-                            if (anchor) {
-                                if(this->bspline || this->spiro){
-                                    this->_bsplineSpiroStartAnchor(revent.state & GDK_SHIFT_MASK);;
-                                }
-                            }
-                            this->_setInitialPoint(p);
-                        } else {
+//                        if ( this->npoints == 0 ) {
+//                            // Start new thread only with button release
+//                            this->_bsplineSpiroColor();
+//                            if (anchor) {
+//                                p = anchor->dp;
+//                                this->sa = anchor;
+//                                //Put the start overwrite curve always on the same direction
+//                                if (anchor->start) {
+//                                    this->sa_overwrited = this->sa->curve->create_reverse();
+//                                } else {
+//                                    this->sa_overwrited = this->sa->curve->copy();
+//                                }
+//                                if(this->bspline || this->spiro){
+//                                    this->_bsplineSpiroStartAnchor(revent.state & GDK_SHIFT_MASK);;
+//                                }
+//                            }
+//                            this->_setInitialPoint(p);
+//                        } else {
                             // Set end anchor here
                             this->ea = anchor;
                             if (anchor) {
                                 p = anchor->dp;
                             }
-                        }
+                        //}
                         this->state = PenTool::CONTROL;
                         break;
                     case PenTool::CONTROL:
@@ -1264,6 +1273,8 @@ void PenTool::_resetColors() {
     }
     this->sa = NULL;
     this->ea = NULL;
+    this->sa_overwrited->reset();
+
     this->npoints = 0;
     this->red_curve_is_valid = false;
 }
@@ -1415,11 +1426,7 @@ void PenTool::_bsplineSpiroStartAnchor(bool shift)
         this->spiro = false;
     }
     if(!this->spiro && !this->bspline){
-        SPCurve *tmp_curve = this->sa->curve->copy();
-        if (this->sa->start) {
-            tmp_curve  = tmp_curve ->create_reverse();
-        }
-        this->overwrite_curve = tmp_curve ;
+        _bsplineSpiroColor();
         return;
     }
     if(shift){
@@ -1433,14 +1440,10 @@ void PenTool::_bsplineSpiroStartAnchorOn()
 {
     using Geom::X;
     using Geom::Y;
-    SPCurve *tmp_curve = this->sa->curve->copy();
-    if (this->sa->start) {
-        tmp_curve  = tmp_curve ->create_reverse();
-    }
-    Geom::CubicBezier const * cubic = dynamic_cast<Geom::CubicBezier const*>(&*tmp_curve ->last_segment());
+    Geom::CubicBezier const * cubic = dynamic_cast<Geom::CubicBezier const*>(&*this->sa_overwrited ->last_segment());
     SPCurve *last_segment = new SPCurve();
-    Geom::Point point_a = tmp_curve->last_segment()->initialPoint();
-    Geom::Point point_d = *tmp_curve->last_point();
+    Geom::Point point_a = this->sa_overwrited->last_segment()->initialPoint();
+    Geom::Point point_d = *this->sa_overwrited->last_point();
     Geom::Point point_c = point_d + (1./3)*(point_a - point_d);
     point_c = Geom::Point(point_c[X] + HANDLE_CUBIC_GAP, point_c[Y] + HANDLE_CUBIC_GAP);
     if(cubic){
@@ -1450,43 +1453,34 @@ void PenTool::_bsplineSpiroStartAnchorOn()
         last_segment->moveto(point_a);
         last_segment->curveto(point_a,point_c,point_d);
     }
-    if( tmp_curve ->get_segment_count() == 1){
-        tmp_curve  = last_segment;
+    if( this->sa_overwrited->get_segment_count() == 1){
+        this->sa_overwrited  = last_segment->copy();
     }else{
         //we eliminate the last segment
-        tmp_curve ->backspace();
+        this->sa_overwrited->backspace();
         //and we add it again with the recreation
-        tmp_curve ->append_continuous(last_segment, 0.0625);
+        this->sa_overwrited->append_continuous(last_segment, 0.0625);
     }
-    if (this->sa->start) {
-        tmp_curve  = tmp_curve ->create_reverse();
-    }
-    this->overwrite_curve = tmp_curve ;
+    last_segment->unref();
 }
 
 void PenTool::_bsplineSpiroStartAnchorOff()
 {
-    SPCurve *tmp_curve  = this->sa->curve->copy();
-    if(this->sa->start)
-        tmp_curve  = tmp_curve ->create_reverse();
-    Geom::CubicBezier const * cubic = dynamic_cast<Geom::CubicBezier const*>(&*tmp_curve ->last_segment());
+    Geom::CubicBezier const * cubic = dynamic_cast<Geom::CubicBezier const*>(&*this->sa_overwrited->last_segment());
     if(cubic){
         SPCurve *last_segment = new SPCurve();
         last_segment->moveto((*cubic)[0]);
         last_segment->curveto((*cubic)[1],(*cubic)[3],(*cubic)[3]);
-        if( tmp_curve ->get_segment_count() == 1){
-            tmp_curve  = last_segment;
+        if( this->sa_overwrited->get_segment_count() == 1){
+            this->sa_overwrited = last_segment->copy();
         }else{
             //we eliminate the last segment
-            tmp_curve ->backspace();
+            this->sa_overwrited->backspace();
             //and we add it again with the recreation
-            tmp_curve ->append_continuous(last_segment, 0.0625);
+            this->sa_overwrited->append_continuous(last_segment, 0.0625);
         }
+        last_segment->unref();
     }
-    if (this->sa->start) {
-        tmp_curve  = tmp_curve ->create_reverse();
-    }
-    this->overwrite_curve = tmp_curve;
 }
 
 void PenTool::_bsplineSpiroMotion(guint const state){
@@ -1510,10 +1504,7 @@ void PenTool::_bsplineSpiroMotion(guint const state){
     } else if (!this->green_curve->is_unset()){
         tmp_curve  = this->green_curve->copy();
     } else {
-        tmp_curve  = this->overwrite_curve->copy();
-        if(this->sa->start) {
-            tmp_curve  = tmp_curve ->create_reverse();
-        }
+        tmp_curve  = this->sa_overwrited->copy();
     }
     if ((state & GDK_MOD1_MASK ) && previous != Geom::Point(0,0)) { //ALT drag
         this->p[0] = this->p[0] + (this->p[3] - previous);
@@ -1550,11 +1541,7 @@ void PenTool::_bsplineSpiroMotion(guint const state){
             }
             cubic = dynamic_cast<Geom::CubicBezier const*>(&*tmp_curve ->last_segment());
             if (this->sa && this->green_curve->is_unset()) {
-                if(this->sa->start) {
-                    this->overwrite_curve = tmp_curve->copy()->create_reverse();
-                } else {
-                    this->overwrite_curve = tmp_curve->copy();
-                }
+                this->sa_overwrited = tmp_curve->copy();
             }
             if (!this->green_bpaths.empty()) {
                 this->green_curve = tmp_curve->copy();
@@ -1627,19 +1614,13 @@ void PenTool::_bsplineSpiroEndAnchorOn()
     SPCurve *tmp_curve;
     SPCurve *last_segment = new SPCurve();
     Geom::Point point_c(0,0);
-    bool reverse = false;
     if( this->green_anchor && this->green_anchor->active ){
         tmp_curve  = this->green_curve->create_reverse();
         if(this->green_curve->get_segment_count()==0){
             return;
         }
-        reverse = true;
     } else if(this->sa){
-        tmp_curve  = this->overwrite_curve->copy();
-        if(!this->sa->start){
-            tmp_curve  = tmp_curve ->create_reverse();
-            reverse = true;
-        }
+        tmp_curve  = this->sa_overwrited->copy()->create_reverse();
     }else{
         return;
     }
@@ -1665,17 +1646,16 @@ void PenTool::_bsplineSpiroEndAnchorOn()
         //and we add it again with the recreation
         tmp_curve ->append_continuous(last_segment, 0.0625);
     }
-    if (reverse) {
-        tmp_curve  = tmp_curve ->create_reverse();
-    }
+    tmp_curve  = tmp_curve ->create_reverse();
     if( this->green_anchor && this->green_anchor->active )
     {
         this->green_curve->reset();
-        this->green_curve = tmp_curve ;
+        this->green_curve = tmp_curve->copy();
     }else{
-        this->overwrite_curve->reset();
-        this->overwrite_curve = tmp_curve ;
+        this->sa_overwrited->reset();
+        this->sa_overwrited = tmp_curve->copy();
     }
+    tmp_curve->unref();
 }
 
 void PenTool::_bsplineSpiroEndAnchorOff()
@@ -1683,20 +1663,14 @@ void PenTool::_bsplineSpiroEndAnchorOff()
 
     SPCurve *tmp_curve;
     SPCurve *last_segment = new SPCurve();
-    bool reverse = false;
     this->p[2] = this->p[3];
     if( this->green_anchor && this->green_anchor->active ){
         tmp_curve  = this->green_curve->create_reverse();
         if(this->green_curve->get_segment_count()==0){
             return;
         }
-        reverse = true;
     } else if(this->sa){
-        tmp_curve  = this->overwrite_curve->copy();
-        if(!this->sa->start){
-            tmp_curve  = tmp_curve ->create_reverse();
-            reverse = true;
-        }
+        tmp_curve  = this->sa_overwrited->copy()->create_reverse();
     }else{
         return;
     }
@@ -1716,17 +1690,17 @@ void PenTool::_bsplineSpiroEndAnchorOff()
         //and we add it again with the recreation
         tmp_curve ->append_continuous(last_segment, 0.0625);
     }
-    if (reverse) {
-        tmp_curve  = tmp_curve ->create_reverse();
-    }
+    tmp_curve  = tmp_curve ->create_reverse();
+
     if( this->green_anchor && this->green_anchor->active )
     {
         this->green_curve->reset();
-        this->green_curve = tmp_curve ;
+        this->green_curve = tmp_curve->copy();
     }else{
-        this->overwrite_curve->reset();
-        this->overwrite_curve = tmp_curve ;
+        this->sa_overwrited->reset();
+        this->sa_overwrited = tmp_curve->copy();
     }
+    tmp_curve->unref();
 }
 
 //prepares the curves for its transformation into BSpline curve.
@@ -1740,10 +1714,7 @@ void PenTool::_bsplineSpiroBuild()
     SPCurve *curve = new SPCurve();
     //If we continuate the existing curve we add it at the start
     if(this->sa && !this->sa->curve->is_unset()){
-        curve = this->overwrite_curve->copy();
-        if (this->sa->start) {
-            curve = curve->create_reverse();
-        }
+        curve = this->sa_overwrited->copy();
     }
 
     if (!this->green_curve->is_unset()){
@@ -2038,7 +2009,6 @@ void PenTool::_finish(gboolean const closed) {
     // cancelate line without a created segment
     this->red_curve->reset();
     spdc_concat_colors_and_flush(this, closed);
-    this->overwrite_curve = NULL;
     this->sa = NULL;
     this->ea = NULL;
 
diff --git a/src/ui/tools/pencil-tool.cpp b/src/ui/tools/pencil-tool.cpp
index 1e1650d44..1d0646cc2 100644
--- a/src/ui/tools/pencil-tool.cpp
+++ b/src/ui/tools/pencil-tool.cpp
@@ -211,7 +211,12 @@ bool PencilTool::_handleButtonPress(GdkEventButton const &bevent) {
                 }
                 if (anchor) {
                     p = anchor->dp;
-                    this->overwrite_curve = anchor->curve;
+                    //Put the start overwrite curve always on the same direction
+                    if (anchor->start) {
+                        this->sa_overwrited =  anchor->curve->create_reverse();
+                    } else {
+                        this->sa_overwrited =  anchor->curve->copy();
+                    }
                     desktop->messageStack()->flash(Inkscape::NORMAL_MESSAGE, _("Continuing selected path"));
                 } else {
                     m.setup(desktop, true);
@@ -282,18 +287,28 @@ bool PencilTool::_handleMotionNotify(GdkEventMotion const &mevent) {
     // (indicating they intend to move the object, not click), then always process the
     // motion notify coordinates as given (no snapping back to origin)
     if (input_has_pressure && pencil_within_tolerance) {
-        p = desktop->w2d(pencil_drag_origin_w);
         anchor = spdc_test_inside(this, pencil_drag_origin_w);
+        if (anchor) {
+            this->sa = anchor;
+            //Put the start overwrite curve always on the same direction
+            if (anchor->start) {
+                this->sa_overwrited = this->sa->curve->create_reverse();
+            } else {
+                this->sa_overwrited = this->sa->curve->copy();
+            }
+            p = anchor->dp;
+            this->_setStartpoint(p);
+            desktop->messageStack()->flash(Inkscape::NORMAL_MESSAGE, _("Continuing selected path"));
+        }
     }
+    if (input_has_pressure) {
+        this->state = SP_PENCIL_CONTEXT_FREEHAND;
+    } 
     pencil_within_tolerance = false;
 
     switch (this->state) {
         case SP_PENCIL_CONTEXT_ADDLINE:
             /* Set red endpoint */
-            if (input_has_pressure) {
-                this->state = SP_PENCIL_CONTEXT_FREEHAND;
-                return false;
-            } 
             if (anchor) {
                 p = anchor->dp;
             } else {
@@ -317,7 +332,6 @@ bool PencilTool::_handleMotionNotify(GdkEventMotion const &mevent) {
                     this->green_anchor = sp_draw_anchor_new(this, this->green_curve, TRUE, this->p[0]);
                 }
                 if (anchor) {
-                    std::cout << "aaaaaaaaaaaaaaaaaaaaaaaaaa" << std::endl;
                     p = anchor->dp;
                 }
                 if ( this->npoints != 0) { // buttonpress may have happened before we entered draw context!
@@ -745,6 +759,36 @@ PencilTool::removePowerStrokePreview()
         _powerpreview = NULL;
     }
 }
+
+void PencilTool::_startAnchorToCurve() {
+    if (!this->sa_overwrited->is_unset()) {
+        Geom::Point A(0,0);
+        Geom::Point B(0,0);
+        Geom::Point C(0,0);
+        Geom::Point D(0,0);
+        Geom::CubicBezier const *cubic = dynamic_cast<Geom::CubicBezier const *>( this->sa_overwrited->last_segment() );
+        //We obtain the last segment 4 points in the previous curve 
+        if ( cubic && !this->green_curve->is_unset()){
+            A = (*cubic)[0];
+            B = (*cubic)[1];
+            C = *this->green_curve->last_point() + (1./3.)*(this->green_curve->last_segment()->initialPoint() - *this->green_curve->last_point());
+            D = (*cubic)[3];
+            SPCurve *previous = new SPCurve();
+            previous->moveto(A);
+            previous->curveto(B, C, D);
+            if ( this->sa_overwrited->get_segment_count() == 1) {
+                this->sa_overwrited = previous->copy();
+            } else {
+                //we eliminate the last segment
+                this->sa_overwrited->backspace();
+                //and we add it again with the recreation
+                this->sa_overwrited->append_continuous(previous->copy(), 0.0625);
+            }
+            previous->unref();
+        }
+    }
+}
+
 void 
 PencilTool::addPowerStrokePencil(SPCurve * c) 
 {
@@ -759,7 +803,7 @@ PencilTool::addPowerStrokePencil(SPCurve * c)
         min = max;
     }
     bool live = false;
-    SPCurve * curve;
+    SPCurve * curve = new SPCurve();
     if (sa) {
         Effect* lpe = SP_LPE_ITEM(white_item)->getCurrentLPE();
         LPEPowerStroke* ps = static_cast<LPEPowerStroke*>(lpe);
@@ -770,6 +814,7 @@ PencilTool::addPowerStrokePencil(SPCurve * c)
                 this->points = ps->offset_points.data();
             }
         }
+        //_startAnchorToCurve();
     }
     if(!c) {
         live = true;
@@ -785,40 +830,25 @@ PencilTool::addPowerStrokePencil(SPCurve * c)
         stroreps.clear();
         strorewps.clear();
         prefs->setDouble("/tools/freehand/pencil/tolerance", tol);
-        if (sa) {
-            curve = sa->curve;
-            if(prefs->getInt("/tools/freehand/pencil/freehand-mode", 0) == 1 || 
-               prefs->getInt("/tools/freehand/pencil/freehand-mode", 0) == 2)
-            {
-                curve = overwrite_curve;
-            }
-            if (sa->start) {
-                SPCurve *ret = curve->create_reverse();
-                curve->unref();
-                curve = ret->copy();
-                ret->unref();
-            }
-            if (!green_curve->is_empty()) {
-                if (curve->is_empty()) {
-                    curve = green_curve->copy();
-                } else {
-                    green_curve->move_endpoints(curve->first_path()->finalPoint(), green_curve->first_path()->finalPoint());
-                    curve->append_continuous( green_curve, 0.0625);
-                }
+        if (sa && sa->curve) {
+            curve = sa_overwrited->copy();
+            if (!green_curve->is_unset()) {
+                curve->append_continuous( green_curve, 0.0625);
                 if (!red_curve->is_empty()) {
                     curve->append_continuous( red_curve, 0.0625);
                 }
             }
         } else {
-            if (!green_curve->is_empty()) {
+            if (!green_curve->is_unset()) {
                 curve = green_curve->copy();
                 if (!red_curve->is_empty()) {
                     curve->append_continuous( red_curve, 0.0625);
                 }
-            } else {
-                curve = NULL;
             }
         }
+        if (curve->is_empty()) {
+            curve = NULL;
+        }
         red_curve = previous_red->copy();
         green_curve = previous_green->copy();
         previous_red->unref();
@@ -844,28 +874,24 @@ PencilTool::addPowerStrokePencil(SPCurve * c)
     bool start     = true;
     auto pressure  = this->wps.begin();
     size_t counter = 0;
-    Geom::Point position = *this->ps.begin();
-    if (!live) {
-        position *= transformCoordinate.inverse();
-    }
-    double pos = Geom::nearest_time(position, path);
     for (auto point = this->ps.begin(); point != this->ps.end(); ++point, ++pressure) {
         counter++;
         double pressure_shrunk = (*pressure * (max - min)) + min;
        //We need half width for power stroke
         pressure_computed = pressure_shrunk * dezoomify_factor/2.0;
         //remove start pressure gap
+        Geom::Point position = *point;
+        if (!live) {
+            position *= transformCoordinate.inverse();
+        }
+        double pos = Geom::nearest_time(position, path);
         if (start) {
             start = false;
             this->points.push_back(Geom::Point(pos + 0.01, pressure_computed));
             previous_pressure = pressure_shrunk;
             continue;
         }
-        position = *point;
-        if (!live) {
-            position *= transformCoordinate.inverse();
-        }
-        pos = Geom::nearest_time(position, path);
+        
         if (pos < 1e6 && std::abs(previous_pressure - pressure_shrunk) > gap_pressure && pos < path.size() - 1) {
             previous_pressure = pressure_shrunk;
             this->points.push_back(Geom::Point(pos, pressure_computed));
@@ -959,27 +985,27 @@ void PencilTool::_interpolate(bool realize) {
 
 
 
-        if (realize && this->ps.size() > 3) {
-            Geom::Point start_point = *this->ps.begin();
-            while ( this->ps.size() > 6 && Geom::distance(*(this->ps.begin()+1), start_point) < smoothlenght) {
-                this->ps.erase(this->ps.begin() + 1);
-                this->wps.erase(this->wps.begin() + 1);
-            }
-        }
-        //Smooth last segments
-        if (realize && this->ps.size() > 3) {
-            Geom::Point last_point = *this->ps.end();
-            bool erased = false;
-            while ( this->ps.size() > 6 && Geom::distance(*this->ps.end(), last_point) < smoothlenght) {
-                this->ps.pop_back();
-                this->wps.pop_back();
-                erased = true;
-            }
-            if (erased) {
-                this->wps.push_back(this->wps[this->wps.size()-1]);
-                this->ps.push_back(last_point);
-            }
-        }
+//        if (realize && this->ps.size() > 3) {
+//            Geom::Point start_point = *this->ps.begin();
+//            while ( this->ps.size() > 6 && Geom::distance(*(this->ps.begin()+1), start_point) < smoothlenght) {
+//                this->ps.erase(this->ps.begin() + 1);
+//                this->wps.erase(this->wps.begin() + 1);
+//            }
+//        }
+//        //Smooth last segments
+//        if (realize && this->ps.size() > 3) {
+//            Geom::Point last_point = *this->ps.end();
+//            bool erased = false;
+//            while ( this->ps.size() > 6 && Geom::distance(*this->ps.end(), last_point) < smoothlenght) {
+//                this->ps.pop_back();
+//                this->wps.pop_back();
+//                erased = true;
+//            }
+//            if (erased) {
+//                this->wps.push_back(this->wps[this->wps.size()-1]);
+//                this->ps.push_back(last_point);
+//            }
+//        }
     }
 
     double tolerance_sq = 0.02 * square(this->desktop->w2d().descrim() * tol) * exp(0.2 * tol - 2);
@@ -1002,7 +1028,7 @@ void PencilTool::_interpolate(bool realize) {
     if (n_segs > 0) {
         /* Fit and draw and reset state */
 
-        this->green_curve->moveto(b[0]);
+        this->green_curve->moveto(this->ps[0]);
         Inkscape::Preferences *prefs = Inkscape::Preferences::get();
         guint mode = prefs->getInt("/tools/freehand/pencil/freehand-mode", 0);
         for (int c = 0; c < n_segs; c++) {
@@ -1014,14 +1040,14 @@ void PencilTool::_interpolate(bool realize) {
                 point_at2 = Geom::Point(point_at2[X] + HANDLE_CUBIC_GAP, point_at2[Y] + HANDLE_CUBIC_GAP);
                 this->green_curve->curveto(point_at1,point_at2,b[4*c+3]);
             } else {
-                //force retracted handle at end if power stroke
-                if (c == n_segs - 1 && input_has_pressure) {
-                    this->green_curve->curveto(b[4 * c + 1], b[4 * c + 3], b[4 * c + 3]);
-                } else if (c == 0 && input_has_pressure) {
-                    this->green_curve->curveto(b[4 * c], b[4 * c + 2], b[4 * c + 3]);
-                } else {
+//                //force retracted handle at end if power stroke
+//                if (c == n_segs - 1 && input_has_pressure) {
+//                    this->green_curve->curveto(b[4 * c + 1], b[4 * c + 3], b[4 * c + 3]);
+//                } else if (c == 0 && input_has_pressure && !this->sa) {
+//                    this->green_curve->curveto(b[4 * c], b[4 * c + 2], b[4 * c + 3]);
+//                } else {
                     this->green_curve->curveto(b[4 * c + 1], b[4 * c + 2], b[4 * c + 3]);
-                }
+               // }
             }
         }
         if (!input_has_pressure) {
diff --git a/src/ui/tools/pencil-tool.h b/src/ui/tools/pencil-tool.h
index e15289ad1..98a9b547d 100644
--- a/src/ui/tools/pencil-tool.h
+++ b/src/ui/tools/pencil-tool.h
@@ -70,6 +70,7 @@ private:
     bool _handleKeyPress(GdkEventKey const &event);
     bool _handleKeyRelease(GdkEventKey const &event);
     void _setStartpoint(Geom::Point const &p);
+    void _startAnchorToCurve();
     void _powerStrokePreview(Geom::Path const path, std::vector<Geom::Point> points, bool write);
     SPShape *_powerpreview;
     SPShape *_powerpreviewtail;