1 #include <2geom/sbasis-geometric.h>
2 #include <2geom/sbasis.h>
3 #include <2geom/sbasis-math.h>
4 //#include <2geom/solver.h>
5 #include <2geom/sbasis-geometric.h>
7 /** Geometric operators on D2<SBasis> (1D->2D).
8 * Copyright 2007 JF Barraud
9 * Copyright 2007 N Hurst
10 *
11 * The functions defined in this header related to 2d geometric operations such as arc length,
12 * unit_vector, curvature, and centroid. Most are built on top of unit_vector, which takes an
13 * arbitrary D2 and returns a D2 with unit length with the same direction.
14 *
15 * Todo/think about:
16 * arclength D2 -> sbasis (giving arclength function)
17 * does uniform_speed return natural parameterisation?
18 * integrate sb2d code from normal-bundle
19 * angle(md<2>) -> sbasis (gives angle from vector - discontinuous?)
20 * osculating circle center?
21 *
22 **/
24 //namespace Geom{
25 using namespace Geom;
26 using namespace std;
28 //Some utils first.
29 //TODO: remove this!!
30 static vector<double>
31 vect_intersect(vector<double> const &a, vector<double> const &b, double tol=0.){
32 vector<double> inter;
33 unsigned i=0,j=0;
34 while ( i<a.size() && j<b.size() ){
35 if (fabs(a[i]-b[j])<tol){
36 inter.push_back(a[i]);
37 i+=1;
38 j+=1;
39 }else if (a[i]<b[j]){
40 i+=1;
41 }else if (a[i]>b[j]){
42 j+=1;
43 }
44 }
45 return inter;
46 }
48 static SBasis divide_by_sk(SBasis const &a, int k) {
49 assert( k<(int)a.size());
50 if(k < 0) return shift(a,-k);
51 SBasis c;
52 c.insert(c.begin(), a.begin()+k, a.end());
53 return c;
54 }
56 static SBasis divide_by_t0k(SBasis const &a, int k) {
57 if(k < 0) {
58 SBasis c = Linear(0,1);
59 for (int i=2; i<=-k; i++){
60 c*=c;
61 }
62 c*=a;
63 return(c);
64 }else{
65 SBasis c = Linear(1,0);
66 for (int i=2; i<=k; i++){
67 c*=c;
68 }
69 c*=a;
70 return(divide_by_sk(c,k));
71 }
72 }
74 static SBasis divide_by_t1k(SBasis const &a, int k) {
75 if(k < 0) {
76 SBasis c = Linear(1,0);
77 for (int i=2; i<=-k; i++){
78 c*=c;
79 }
80 c*=a;
81 return(c);
82 }else{
83 SBasis c = Linear(0,1);
84 for (int i=2; i<=k; i++){
85 c*=c;
86 }
87 c*=a;
88 return(divide_by_sk(c,k));
89 }
90 }
92 static D2<SBasis> RescaleForNonVanishingEnds(D2<SBasis> const &MM, double ZERO=1.e-4){
93 D2<SBasis> M = MM;
94 //TODO: divide by all the s at once!!!
95 while (fabs(M[0].at0())<ZERO &&
96 fabs(M[1].at0())<ZERO &&
97 fabs(M[0].at1())<ZERO &&
98 fabs(M[1].at1())<ZERO){
99 M[0] = divide_by_sk(M[0],1);
100 M[1] = divide_by_sk(M[1],1);
101 }
102 while (fabs(M[0].at0())<ZERO && fabs(M[1].at0())<ZERO){
103 M[0] = divide_by_t0k(M[0],1);
104 M[1] = divide_by_t0k(M[1],1);
105 }
106 while (fabs(M[0].at1())<ZERO && fabs(M[1].at1())<ZERO){
107 M[0] = divide_by_t1k(M[0],1);
108 M[1] = divide_by_t1k(M[1],1);
109 }
110 return M;
111 }
113 //=================================================================
114 //TODO: what's this for?!?!
115 Piecewise<D2<SBasis> >
116 Geom::cutAtRoots(Piecewise<D2<SBasis> > const &M, double ZERO){
117 vector<double> rts;
118 for (unsigned i=0; i<M.size(); i++){
119 vector<double> seg_rts = roots((M.segs[i])[0]);
120 seg_rts = vect_intersect(seg_rts, roots((M.segs[i])[1]), ZERO);
121 Linear mapToDom = Linear(M.cuts[i],M.cuts[i+1]);
122 for (unsigned r=0; r<seg_rts.size(); r++){
123 seg_rts[r]= mapToDom(seg_rts[r]);
124 }
125 rts.insert(rts.end(),seg_rts.begin(),seg_rts.end());
126 }
127 return partition(M,rts);
128 }
130 Piecewise<SBasis>
131 Geom::atan2(Piecewise<D2<SBasis> > const &vect, double tol, unsigned order){
132 Piecewise<SBasis> result;
133 Piecewise<D2<SBasis> > v = cutAtRoots(vect);
134 result.cuts.push_back(v.cuts.front());
135 for (unsigned i=0; i<v.size(); i++){
137 D2<SBasis> vi = RescaleForNonVanishingEnds(v.segs[i]);
138 SBasis x=vi[0], y=vi[1];
139 Piecewise<SBasis> angle;
140 angle = divide (x*derivative(y)-y*derivative(x), x*x+y*y, tol, order);
142 //TODO: I don't understand this - sign.
143 angle = integral(-angle);
144 Point vi0 = vi.at0();
145 angle += -std::atan2(vi0[1],vi0[0]) - angle[0].at0();
146 //TODO: deal with 2*pi jumps form one seg to the other...
147 //TODO: not exact at t=1 because of the integral.
148 //TODO: force continuity?
150 angle.setDomain(Interval(v.cuts[i],v.cuts[i+1]));
151 result.concat(angle);
152 }
153 return result;
154 }
155 Piecewise<SBasis>
156 Geom::atan2(D2<SBasis> const &vect, double tol, unsigned order){
157 return atan2(Piecewise<D2<SBasis> >(vect),tol,order);
158 }
160 //tan2 is the pseudo-inverse of atan2. It takes an angle and returns a unit_vector that points in the direction of angle.
161 D2<Piecewise<SBasis> >
162 Geom::tan2(SBasis const &angle, double tol, unsigned order){
163 return tan2(Piecewise<SBasis>(angle), tol, order);
164 }
166 D2<Piecewise<SBasis> >
167 Geom::tan2(Piecewise<SBasis> const &angle, double tol, unsigned order){
168 return D2<Piecewise<SBasis> >(cos(angle, tol, order), sin(angle, tol, order));
169 }
171 //unitVector(x,y) is computed as (b,-a) where a and b are solutions of:
172 // ax+by=0 (eqn1) and a^2+b^2=1 (eqn2)
173 Piecewise<D2<SBasis> >
174 Geom::unitVector(D2<SBasis> const &V_in, double tol, unsigned order){
175 D2<SBasis> V = RescaleForNonVanishingEnds(V_in);
176 if (V[0].empty() && V[1].empty())
177 return Piecewise<D2<SBasis> >(D2<SBasis>(Linear(1),SBasis()));
178 SBasis x = V[0], y = V[1], a, b;
179 SBasis r_eqn1, r_eqn2;
181 Point v0 = unit_vector(V.at0());
182 Point v1 = unit_vector(V.at1());
183 a.push_back(Linear(-v0[1],-v1[1]));
184 b.push_back(Linear( v0[0], v1[0]));
186 r_eqn1 = -(a*x+b*y);
187 r_eqn2 = Linear(1.)-(a*a+b*b);
189 for (unsigned k=1; k<=order; k++){
190 double r0 = (k<r_eqn1.size())? r_eqn1.at(k).at0() : 0;
191 double r1 = (k<r_eqn1.size())? r_eqn1.at(k).at1() : 0;
192 double rr0 = (k<r_eqn2.size())? r_eqn2.at(k).at0() : 0;
193 double rr1 = (k<r_eqn2.size())? r_eqn2.at(k).at1() : 0;
194 double a0,a1,b0,b1;// coeffs in a[k] and b[k]
196 //the equations to solve at this point are:
197 // a0*x(0)+b0*y(0)=r0 & 2*a0*a(0)+2*b0*b(0)=rr0
198 //and
199 // a1*x(1)+b1*y(1)=r1 & 2*a1*a(1)+2*b1*b(1)=rr1
200 a0 = r0/dot(v0,V(0))*v0[0]-rr0/2*v0[1];
201 b0 = r0/dot(v0,V(0))*v0[1]+rr0/2*v0[0];
202 a1 = r1/dot(v1,V(1))*v1[0]-rr1/2*v1[1];
203 b1 = r1/dot(v1,V(1))*v1[1]+rr1/2*v1[0];
205 a.push_back(Linear(a0,a1));
206 b.push_back(Linear(b0,b1));
207 //TODO: use "incremental" rather than explicit formulas.
208 r_eqn1 = -(a*x+b*y);
209 r_eqn2 = Linear(1)-(a*a+b*b);
210 }
212 //our candidate is:
213 D2<SBasis> unitV;
214 unitV[0] = b;
215 unitV[1] = -a;
217 //is it good?
218 double rel_tol = std::max(1.,std::max(V_in[0].tailError(0),V_in[1].tailError(0)))*tol;
220 if (r_eqn1.tailError(order)>rel_tol || r_eqn2.tailError(order)>tol){
221 //if not: subdivide and concat results.
222 Piecewise<D2<SBasis> > unitV0, unitV1;
223 unitV0 = unitVector(compose(V,Linear(0,.5)),tol,order);
224 unitV1 = unitVector(compose(V,Linear(.5,1)),tol,order);
225 unitV0.setDomain(Interval(0.,.5));
226 unitV1.setDomain(Interval(.5,1.));
227 unitV0.concat(unitV1);
228 return(unitV0);
229 }else{
230 //if yes: return it as pw.
231 Piecewise<D2<SBasis> > result;
232 result=(Piecewise<D2<SBasis> >)unitV;
233 return result;
234 }
235 }
237 Piecewise<D2<SBasis> >
238 Geom::unitVector(Piecewise<D2<SBasis> > const &V, double tol, unsigned order){
239 Piecewise<D2<SBasis> > result;
240 Piecewise<D2<SBasis> > VV = cutAtRoots(V);
241 result.cuts.push_back(VV.cuts.front());
242 for (unsigned i=0; i<VV.size(); i++){
243 Piecewise<D2<SBasis> > unit_seg;
244 unit_seg = unitVector(VV.segs[i],tol, order);
245 unit_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
246 result.concat(unit_seg);
247 }
248 return result;
249 }
251 Piecewise<SBasis>
252 Geom::arcLengthSb(Piecewise<D2<SBasis> > const &M, double tol){
253 Piecewise<D2<SBasis> > dM = derivative(M);
254 Piecewise<SBasis> dMlength = sqrt(dot(dM,dM),tol,3);
255 Piecewise<SBasis> length = integral(dMlength);
256 length-=length.segs.front().at0();
257 return length;
258 }
259 Piecewise<SBasis>
260 Geom::arcLengthSb(D2<SBasis> const &M, double tol){
261 return arcLengthSb(Piecewise<D2<SBasis> >(M), tol);
262 }
264 double
265 Geom::length(D2<SBasis> const &M,
266 double tol){
267 Piecewise<SBasis> length = arcLengthSb(M, tol);
268 return length.segs.back().at1();
269 }
270 double
271 Geom::length(Piecewise<D2<SBasis> > const &M,
272 double tol){
273 Piecewise<SBasis> length = arcLengthSb(M, tol);
274 return length.segs.back().at1();
275 }
278 // incomplete.
279 Piecewise<SBasis>
280 Geom::curvature(D2<SBasis> const &M, double tol) {
281 D2<SBasis> dM=derivative(M);
282 Piecewise<SBasis> result;
283 Piecewise<D2<SBasis> > unitv = unitVector(dM,tol);
284 Piecewise<SBasis> dMlength = dot(Piecewise<D2<SBasis> >(dM),unitv);
285 Piecewise<SBasis> k = cross(derivative(unitv),unitv);
286 k = divide(k,dMlength,tol,3);
287 return(k);
288 }
290 Piecewise<SBasis>
291 Geom::curvature(Piecewise<D2<SBasis> > const &V, double tol){
292 Piecewise<SBasis> result;
293 Piecewise<D2<SBasis> > VV = cutAtRoots(V);
294 result.cuts.push_back(VV.cuts.front());
295 for (unsigned i=0; i<VV.size(); i++){
296 Piecewise<SBasis> curv_seg;
297 curv_seg = curvature(VV.segs[i],tol);
298 curv_seg.setDomain(Interval(VV.cuts[i],VV.cuts[i+1]));
299 result.concat(curv_seg);
300 }
301 return result;
302 }
304 //=================================================================
306 Piecewise<D2<SBasis> >
307 Geom::arc_length_parametrization(D2<SBasis> const &M,
308 unsigned order,
309 double tol){
310 Piecewise<D2<SBasis> > u;
311 u.push_cut(0);
313 Piecewise<SBasis> s = arcLengthSb(Piecewise<D2<SBasis> >(M),tol);
314 for (unsigned i=0; i < s.size();i++){
315 double t0=s.cuts[i],t1=s.cuts[i+1];
316 D2<SBasis> sub_M = compose(M,Linear(t0,t1));
317 D2<SBasis> sub_u;
318 for (unsigned dim=0;dim<2;dim++){
319 SBasis sub_s = s.segs[i];
320 sub_s-=sub_s.at0();
321 sub_s/=sub_s.at1();
322 sub_u[dim]=compose_inverse(sub_M[dim],sub_s, order, tol);
323 }
324 u.push(sub_u,s(t1));
325 }
326 return u;
327 }
329 Piecewise<D2<SBasis> >
330 Geom::arc_length_parametrization(Piecewise<D2<SBasis> > const &M,
331 unsigned order,
332 double tol){
333 Piecewise<D2<SBasis> > result;
334 for (unsigned i=0; i<M.size(); i++ ){
335 Piecewise<D2<SBasis> > uniform_seg=arc_length_parametrization(M[i],order,tol);
336 result.concat(uniform_seg);
337 }
338 return(result);
339 }
341 /** centroid using sbasis integration.
342 * This approach uses green's theorem to compute the area and centroid using integrals. For curved
343 * shapes this is much faster than converting to polyline.
345 * Returned values:
346 0 for normal execution;
347 2 if area is zero, meaning centroid is meaningless.
349 * Copyright Nathan Hurst 2006
350 */
352 unsigned Geom::centroid(Piecewise<D2<SBasis> > const &p, Point& centroid, double &area) {
353 Point centroid_tmp(0,0);
354 double atmp = 0;
355 for(unsigned i = 0; i < p.size(); i++) {
356 SBasis curl = dot(p[i], rot90(derivative(p[i])));
357 SBasis A = integral(curl);
358 D2<SBasis> C = integral(multiply(curl, p[i]));
359 atmp += A.at1() - A.at0();
360 centroid_tmp += C.at1()- C.at0(); // first moment.
361 }
362 // join ends
363 centroid_tmp *= 2;
364 Point final = p[p.size()-1].at1(), initial = p[0].at0();
365 const double ai = cross(final, initial);
366 atmp += ai;
367 centroid_tmp += (final + initial)*ai; // first moment.
369 area = atmp / 2;
370 if (atmp != 0) {
371 centroid = centroid_tmp / (3 * atmp);
372 return 0;
373 }
374 return 2;
375 }
377 //}; // namespace
380 /*
381 Local Variables:
382 mode:c++
383 c-file-style:"stroustrup"
384 c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
385 indent-tabs-mode:nil
386 fill-column:99
387 End:
388 */
389 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :