1
by Jasper van de Gronde
Initial commit |
1 |
#include <2geom/sbasis-2d.h> |
2 |
#include <2geom/sbasis-geometric.h> |
|
3 |
||
4 |
namespace Geom{ |
|
5 |
||
6 |
SBasis extract_u(SBasis2d const &a, double u) { |
|
7 |
SBasis sb(a.vs, Linear()); |
|
8 |
double s = u*(1-u); |
|
9 |
||
10 |
for(unsigned vi = 0; vi < a.vs; vi++) { |
|
11 |
double sk = 1; |
|
12 |
Linear bo(0,0); |
|
13 |
for(unsigned ui = 0; ui < a.us; ui++) { |
|
14 |
bo += (extract_u(a.index(ui, vi), u))*sk; |
|
15 |
sk *= s; |
|
16 |
}
|
|
17 |
sb[vi] = bo; |
|
18 |
}
|
|
19 |
||
20 |
return sb; |
|
21 |
}
|
|
22 |
||
23 |
SBasis extract_v(SBasis2d const &a, double v) { |
|
24 |
SBasis sb(a.us, Linear()); |
|
25 |
double s = v*(1-v); |
|
26 |
||
27 |
for(unsigned ui = 0; ui < a.us; ui++) { |
|
28 |
double sk = 1; |
|
29 |
Linear bo(0,0); |
|
30 |
for(unsigned vi = 0; vi < a.vs; vi++) { |
|
31 |
bo += (extract_v(a.index(ui, vi), v))*sk; |
|
32 |
sk *= s; |
|
33 |
}
|
|
34 |
sb[ui] = bo; |
|
35 |
}
|
|
36 |
||
37 |
return sb; |
|
38 |
}
|
|
39 |
||
40 |
SBasis compose(Linear2d const &a, D2<SBasis> const &p) { |
|
41 |
D2<SBasis> omp(-p[X] + 1, -p[Y] + 1); |
|
42 |
return multiply(omp[0], omp[1])*a[0] + |
|
43 |
multiply(p[0], omp[1])*a[1] + |
|
44 |
multiply(omp[0], p[1])*a[2] + |
|
45 |
multiply(p[0], p[1])*a[3]; |
|
46 |
}
|
|
47 |
||
48 |
SBasis
|
|
49 |
compose(SBasis2d const &fg, D2<SBasis> const &p) { |
|
50 |
SBasis B; |
|
51 |
SBasis s[2]; |
|
52 |
SBasis ss[2]; |
|
53 |
for(unsigned dim = 0; dim < 2; dim++) |
|
54 |
s[dim] = p[dim]*(Linear(1) - p[dim]); |
|
55 |
ss[1] = Linear(1); |
|
56 |
for(unsigned vi = 0; vi < fg.vs; vi++) { |
|
57 |
ss[0] = ss[1]; |
|
58 |
for(unsigned ui = 0; ui < fg.us; ui++) { |
|
59 |
unsigned i = ui + vi*fg.us; |
|
60 |
B += ss[0]*compose(fg[i], p); |
|
61 |
ss[0] *= s[0]; |
|
62 |
}
|
|
63 |
ss[1] *= s[1]; |
|
64 |
}
|
|
65 |
return B; |
|
66 |
}
|
|
67 |
||
68 |
D2<SBasis> |
|
69 |
compose_each(D2<SBasis2d> const &fg, D2<SBasis> const &p) { |
|
70 |
return D2<SBasis>(compose(fg[X], p), compose(fg[Y], p)); |
|
71 |
}
|
|
72 |
||
73 |
SBasis2d partial_derivative(SBasis2d const &f, int dim) { |
|
74 |
SBasis2d result; |
|
75 |
for(unsigned i = 0; i < f.size(); i++) { |
|
76 |
result.push_back(Linear2d(0,0,0,0)); |
|
77 |
}
|
|
78 |
result.us = f.us; |
|
79 |
result.vs = f.vs; |
|
80 |
||
81 |
for(unsigned i = 0; i < f.us; i++) { |
|
82 |
for(unsigned j = 0; j < f.vs; j++) { |
|
83 |
Linear2d lin = f.index(i,j); |
|
84 |
Linear2d dlin(lin[1+dim]-lin[0], lin[1+2*dim]-lin[dim], lin[3-dim]-lin[2*(1-dim)], lin[3]-lin[2-dim]); |
|
85 |
result[i+j*result.us] += dlin; |
|
86 |
unsigned di = dim?j:i; |
|
87 |
if (di>=1){ |
|
88 |
float motpi = dim?-1:1; |
|
89 |
Linear2d ds_lin_low( lin[0], -motpi*lin[1], motpi*lin[2], -lin[3] ); |
|
90 |
result[(i+dim-1)+(j-dim)*result.us] += di*ds_lin_low; |
|
91 |
||
92 |
Linear2d ds_lin_hi( lin[1+dim]-lin[0], lin[1+2*dim]-lin[dim], lin[3]-lin[2-dim], lin[3-dim]-lin[2-dim] ); |
|
93 |
result[i+j*result.us] += di*ds_lin_hi; |
|
94 |
}
|
|
95 |
}
|
|
96 |
}
|
|
97 |
return result; |
|
98 |
}
|
|
99 |
||
100 |
/**
|
|
101 |
* Finds a path which traces the 0 contour of f, traversing from A to B as a single d2<sbasis>.
|
|
102 |
* degmax specifies the degree (degree = 2*degmax-1, so a degmax of 2 generates a cubic fit).
|
|
103 |
* The algorithm is based on dividing out derivatives at each end point and does not use the curvature for fitting.
|
|
104 |
* It is less accurate than sb2d_cubic_solve, although this may be fixed in the future.
|
|
105 |
*/
|
|
106 |
D2<SBasis> |
|
107 |
sb2dsolve(SBasis2d const &f, Geom::Point const &A, Geom::Point const &B, unsigned degmax){ |
|
108 |
//g_warning("check f(A)= %f = f(B) = %f =0!", f.apply(A[X],A[Y]), f.apply(B[X],B[Y]));
|
|
109 |
||
110 |
SBasis2d dfdu = partial_derivative(f, 0); |
|
111 |
SBasis2d dfdv = partial_derivative(f, 1); |
|
112 |
Geom::Point dfA(dfdu.apply(A[X],A[Y]),dfdv.apply(A[X],A[Y])); |
|
113 |
Geom::Point dfB(dfdu.apply(B[X],B[Y]),dfdv.apply(B[X],B[Y])); |
|
114 |
Geom::Point nA = dfA/(dfA[X]*dfA[X]+dfA[Y]*dfA[Y]); |
|
115 |
Geom::Point nB = dfB/(dfB[X]*dfB[X]+dfB[Y]*dfB[Y]); |
|
116 |
||
117 |
D2<SBasis>result(SBasis(degmax, Linear()), SBasis(degmax, Linear())); |
|
118 |
double fact_k=1; |
|
119 |
double sign = 1.; |
|
120 |
for(int dim = 0; dim < 2; dim++) |
|
121 |
result[dim][0] = Linear(A[dim],B[dim]); |
|
122 |
for(unsigned k=1; k<degmax; k++){ |
|
123 |
// these two lines make the solutions worse!
|
|
124 |
//fact_k *= k;
|
|
125 |
//sign = -sign;
|
|
126 |
SBasis f_on_curve = compose(f,result); |
|
127 |
Linear reste = f_on_curve[k]; |
|
128 |
double ax = -reste[0]/fact_k*nA[X]; |
|
129 |
double ay = -reste[0]/fact_k*nA[Y]; |
|
130 |
double bx = -sign*reste[1]/fact_k*nB[X]; |
|
131 |
double by = -sign*reste[1]/fact_k*nB[Y]; |
|
132 |
||
133 |
result[X][k] = Linear(ax,bx); |
|
134 |
result[Y][k] = Linear(ay,by); |
|
135 |
//sign *= 3;
|
|
136 |
}
|
|
137 |
return result; |
|
138 |
}
|
|
139 |
||
140 |
/**
|
|
141 |
* Finds a path which traces the 0 contour of f, traversing from A to B as a single cubic d2<sbasis>.
|
|
142 |
* The algorithm is based on matching direction and curvature at each end point.
|
|
143 |
*/
|
|
144 |
//TODO: handle the case when B is "behind" A for the natural orientation of the level set.
|
|
145 |
//TODO: more generally, there might be up to 4 solutions. Choose the best one!
|
|
146 |
D2<SBasis> |
|
147 |
sb2d_cubic_solve(SBasis2d const &f, Geom::Point const &A, Geom::Point const &B){ |
|
148 |
D2<SBasis>result;//(Linear(A[X],B[X]),Linear(A[Y],B[Y])); |
|
149 |
//g_warning("check 0 = %f = %f!", f.apply(A[X],A[Y]), f.apply(B[X],B[Y]));
|
|
150 |
||
151 |
SBasis2d f_u = partial_derivative(f , 0); |
|
152 |
SBasis2d f_v = partial_derivative(f , 1); |
|
153 |
SBasis2d f_uu = partial_derivative(f_u, 0); |
|
154 |
SBasis2d f_uv = partial_derivative(f_v, 0); |
|
155 |
SBasis2d f_vv = partial_derivative(f_v, 1); |
|
156 |
||
157 |
Geom::Point dfA(f_u.apply(A[X],A[Y]),f_v.apply(A[X],A[Y])); |
|
158 |
Geom::Point dfB(f_u.apply(B[X],B[Y]),f_v.apply(B[X],B[Y])); |
|
159 |
||
160 |
Geom::Point V0 = rot90(dfA); |
|
161 |
Geom::Point V1 = rot90(dfB); |
|
162 |
||
163 |
double D2fVV0 = f_uu.apply(A[X],A[Y])*V0[X]*V0[X]+ |
|
164 |
2*f_uv.apply(A[X],A[Y])*V0[X]*V0[Y]+ |
|
165 |
f_vv.apply(A[X],A[Y])*V0[Y]*V0[Y]; |
|
166 |
double D2fVV1 = f_uu.apply(B[X],B[Y])*V1[X]*V1[X]+ |
|
167 |
2*f_uv.apply(B[X],B[Y])*V1[X]*V1[Y]+ |
|
168 |
f_vv.apply(B[X],B[Y])*V1[Y]*V1[Y]; |
|
169 |
||
170 |
std::vector<D2<SBasis> > candidates = cubics_fitting_curvature(A,B,V0,V1,D2fVV0,D2fVV1); |
|
171 |
if (candidates.size()==0) { |
|
172 |
return D2<SBasis>(Linear(A[X],B[X]),Linear(A[Y],B[Y])); |
|
173 |
}
|
|
174 |
//TODO: I'm sure std algorithm could do that for me...
|
|
175 |
double error = -1; |
|
176 |
unsigned best = 0; |
|
177 |
for (unsigned i=0; i<candidates.size(); i++){ |
|
178 |
Interval bounds = *bounds_fast(compose(f,candidates[i])); |
|
179 |
double new_error = (fabs(bounds.max())>fabs(bounds.min()) ? fabs(bounds.max()) : fabs(bounds.min()) ); |
|
180 |
if ( new_error < error || error < 0 ){ |
|
181 |
error = new_error; |
|
182 |
best = i; |
|
183 |
}
|
|
184 |
}
|
|
185 |
return candidates[best]; |
|
186 |
}
|
|
187 |
||
188 |
||
189 |
||
190 |
||
191 |
};
|
|
192 |
||
193 |
/*
|
|
194 |
Local Variables:
|
|
195 |
mode:c++
|
|
196 |
c-file-style:"stroustrup"
|
|
197 |
c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
|
|
198 |
indent-tabs-mode:nil
|
|
199 |
fill-column:99
|
|
200 |
End:
|
|
201 |
*/
|
|
202 |
// vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :
|