summaryrefslogtreecommitdiffstats
path: root/src/3rdparty/2geom/src/toys/differential-constraint.cpp
blob: 501eb76718b0c74a3dd5db466c08dd7e92eabd5e (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
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 :