1 files changed, 142 insertions, 0 deletions
diff --git a/src/toys/curvature-curve.cpp b/src/toys/curvature-curve.cpp
new file mode 100644
index 0000000..8f8f7bc
--- /dev/null
+++ b/src/toys/curvature-curve.cpp
@@ -0,0 +1,142 @@
+#include <2geom/d2.h>
+#include <2geom/sbasis.h>
+#include <2geom/sbasis-2d.h>
+#include <2geom/bezier-to-sbasis.h>
+#include <2geom/sbasis-geometric.h>
+
+#include <toys/path-cairo.h>
+#include <toys/toy-framework-2.h>
+
+#include <time.h>
+using std::vector;
+using namespace Geom;
+using namespace std;
+
+//-----------------------------------------------
+
+class CurvatureTester: public Toy {
+    PointSetHandle curve_handle;
+    Path current_curve;
+    void draw(cairo_t *cr, std::ostringstream *notify, int width, int height, bool save, std::ostringstream *timer_stream) override {
+        cairo_set_line_width (cr, 1);
+        current_curve = Path();
+        
+        for(int base_i = 0; base_i < int(curve_handle.pts.size()/2) - 1; base_i++) {
+            for(int i = 0; i < 2; i++) {
+                Geom::Point center=curve_handle.pts[1+2*i+base_i*2];
+                Geom::Point normal=center- curve_handle.pts[2*i+base_i*2];
+                double radius = Geom::L2(normal);
+                *notify<<"K="<<radius<<std::endl;
+                if (fabs(radius)>1e-4){
+                    
+                    double ang = atan2(-normal);
+                    cairo_arc(cr, center[0], center[1],fabs(radius), ang-0.3, ang+0.3);
+                    cairo_set_source_rgba (cr, 0.75, 0.89, 1., 1);
+                    cairo_stroke(cr);
+                    
+                    
+                    //draw_handle(cr, center);
+                }else{
+                }
+                {
+                cairo_save(cr);
+                double dashes[2] = {10, 10};
+                cairo_set_dash(cr, dashes, 2, 0);
+                cairo_move_to(cr, center);
+                cairo_line_to(cr, center-normal);
+                cairo_stroke(cr);
+                cairo_restore(cr);
+                }
+            }
+            cairo_set_source_rgba (cr, 0., 0, 0., 1);
+                Geom::Point A = curve_handle.pts[0+base_i*2];
+                Geom::Point B = curve_handle.pts[2+base_i*2];
+            D2<SBasis> best_c = D2<SBasis>(SBasis(Linear(A[X],B[X])),SBasis(Linear(A[Y],B[Y])));
+            double error = -1;
+            for(int i = 0; i < 16; i++) {
+                Geom::Point dA = curve_handle.pts[1+base_i*2]-A;
+                Geom::Point dB = curve_handle.pts[3+base_i*2]-B;
+                std::vector<D2<SBasis> > candidates = 
+                    cubics_fitting_curvature(curve_handle.pts[0+base_i*2],curve_handle.pts[2+base_i*2],
+                                             (i&2)?rot90(dA):-rot90(dA),
+                                             (i&1)?rot90(dB):-rot90(dB),
+                                             ((i&4)?-1:1)*L2sq(dA), ((i&8)?-1:1)*L2sq(dB));
+	  
+                if (candidates.empty()) {
+                } else {
+                    //TODO: I'm sure std algorithm could do that for me...
+                    unsigned best = 0;
+                    for (unsigned i=0; i<candidates.size(); i++){
+                        Piecewise<SBasis> K = arcLengthSb(candidates[i]);
+        
+                        double l = Geom::length(candidates[i]);
+                        //double l = K.segs.back().at1();//Geom::length(candidates[i]);
+                        //printf("l = %g\n", l);
+                        if ( l < error || error < 0 ){
+                            error = l;
+                            best = i;
+                            best_c = candidates[best];
+                        }
+                    }
+                }
+            }
+            if(error >= 0) {
+                //cairo_d2_sb(cr, best_c);
+                current_curve.append(best_c);
+            }
+        }
+        
+        cairo_path(cr, current_curve);
+        cairo_stroke(cr);
+        Toy::draw(cr, notify, width, height, save,timer_stream);
+    }        
+  
+    void canvas_click(Geom::Point at, int button) override {
+        std::cout << "clicked at:" << at << " with button " << button << std::endl;
+        if(button == 1) {
+            double dist;
+            double t = current_curve.nearestTime(at, &dist).asFlatTime();
+            if(dist > 5) {
+                curve_handle.push_back(at);
+                curve_handle.push_back(at+Point(100,100));
+            } else {
+                // split around t
+                Piecewise<D2<SBasis> > pw =  current_curve.toPwSb();
+                std::vector<Point > vnd = pw.valueAndDerivatives(t, 2);
+                Point on_curve = current_curve(t);
+                Point normal = rot90(vnd[1]);
+                Piecewise<SBasis > K = curvature(pw);
+                Point ps[2] = {on_curve, on_curve+unit_vector(normal)/K(t)};
+                curve_handle.pts.insert(curve_handle.pts.begin()+2*(int(t)+1), ps, ps+2);
+            }
+        }
+    }
+
+public:
+    CurvatureTester(){
+        if(handles.empty()) {
+            handles.push_back(&curve_handle);
+            for(unsigned i = 0; i < 4; i++)
+                curve_handle.push_back(150+uniform()*300,150+uniform()*300);
+        }
+    }
+};
+
+int main(int argc, char **argv) {
+    std::cout << "testing unit_normal(multidim_sbasis) based offset." << std::endl;
+    init(argc, argv, new CurvatureTester);
+    return 0;
+}
+
+/*
+  Local Variables:
+  mode:c++
+  c-file-style:"stroustrup"
+  c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
+  indent-tabs-mode:nil
+  fill-column:99
+  End:
+*/
+//vim:expandtab:shiftwidth = 4:tabstop = 8:softtabstop = 4:encoding = utf-8:textwidth = 99 :
+
+