1 files changed, 132 insertions, 0 deletions
diff --git a/src/toys/differential-constraint.cpp b/src/toys/differential-constraint.cpp
new file mode 100644
index 0000000..501eb76
--- /dev/null
+++ b/src/toys/differential-constraint.cpp
@@ -0,0 +1,132 @@
+/** Differential constraint solver hack
+ * based on idea from Michael Glissner
+ * (njh)
+ */
+#include <2geom/d2.h>
+#include <2geom/sbasis.h>
+#include <2geom/bezier-to-sbasis.h>
+
+#include <toys/path-cairo.h>
+#include <toys/toy-framework.h>
+
+using std::vector;
+using namespace Geom;
+
+unsigned total_pieces_sub;
+unsigned total_pieces_inc;
+
+#include <stdio.h>
+#include <gsl/gsl_errno.h>
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_odeiv.h>
+
+vector<Geom::Point> *handlesptr = NULL;
+
+const unsigned order = 6;
+
+class SBez: public Toy {
+    static int
+    func (double /*t*/, const double y[], double f[],
+          void */*params*/)
+    {
+        //double mu = *(double *)params;
+        D2<SBasis> B = handles_to_sbasis(handlesptr->begin(), order);
+        D2<SBasis> dB = derivative(B);
+        Geom::Point tan = dB(y[0]);//Geom::unit_vector();
+        tan /= dot(tan,tan);
+        Geom::Point yp = B(y[0]);
+        double dtau = -dot(tan, yp - (*handlesptr)[order+1]);
+        f[0] = dtau;
+        
+        return GSL_SUCCESS;
+    }
+     
+    static int
+    jac (double /*t*/, const double y[], double *dfdy, 
+         double dfdt[], void *params)
+    {
+        double mu = *(double *)params;
+        gsl_matrix_view dfdy_mat 
+            = gsl_matrix_view_array (dfdy, 2, 2);
+        gsl_matrix * m = &dfdy_mat.matrix; 
+        gsl_matrix_set (m, 0, 0, 0.0);
+        gsl_matrix_set (m, 0, 1, 1.0);
+        gsl_matrix_set (m, 1, 0, -2.0*mu*y[0]*y[1] - 1.0);
+        gsl_matrix_set (m, 1, 1, -mu*(y[0]*y[0] - 1.0));
+        dfdt[0] = 0.0;
+        dfdt[1] = 0.0;
+        return GSL_SUCCESS;
+    }
+     
+    double y[2];
+
+    virtual void draw(cairo_t *cr, std::ostringstream *notify, int width, int height, bool save, std::ostringstream *timer_stream) {
+        handlesptr = &handles;
+        cairo_set_line_width (cr, 0.5);
+    
+        D2<SBasis> B = handles_to_sbasis(handles.begin(), order);
+        cairo_d2_sb(cr, B);
+    
+        const gsl_odeiv_step_type * T 
+            = gsl_odeiv_step_rk8pd;
+     
+        gsl_odeiv_step * s 
+            = gsl_odeiv_step_alloc (T, 1);
+        gsl_odeiv_control * c 
+            = gsl_odeiv_control_y_new (1e-6, 0.0);
+        gsl_odeiv_evolve * e 
+            = gsl_odeiv_evolve_alloc (1);
+     
+        double mu = 10;
+        gsl_odeiv_system sys = {func, jac, 1, &mu};
+     
+        static double t = 0.0;
+        double t1 = t + 1;
+        double h = 1e-6;
+     
+        while (t < t1)
+        {
+            int status = gsl_odeiv_evolve_apply (e, c, s,
+                                                 &sys, 
+                                                 &t, t1,
+                                                 &h, y);
+     
+            if (status != GSL_SUCCESS)
+                break;
+        
+            //printf ("%.5e %.5e %.5e\n", t, y[0], y[1]);
+        }
+    
+        draw_cross(cr, B(y[0]));
+     
+        gsl_odeiv_evolve_free (e);
+        gsl_odeiv_control_free (c);
+        gsl_odeiv_step_free (s);
+
+        Toy::draw(cr, notify, width, height, save,timer_stream);
+    }
+public:
+    SBez() {
+        y[0] = 0;
+        for(unsigned i = 0; i <= order+1; i++) {
+            handles.push_back(Geom::Point(uniform()*400, uniform()*400));
+        }
+    }
+};
+
+int main(int argc, char **argv) {
+    init(argc, argv, new SBez());
+
+    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: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:fileencoding=utf-8:textwidth=99 :