2619da594757d755f3751332db91529bc7e0f241
1 /*
2 * sbasis.cpp - S-power basis function class + supporting classes
3 *
4 * Authors:
5 * Nathan Hurst <njh@mail.csse.monash.edu.au>
6 * Michael Sloan <mgsloan@gmail.com>
7 *
8 * Copyright (C) 2006-2007 authors
9 *
10 * This library is free software; you can redistribute it and/or
11 * modify it either under the terms of the GNU Lesser General Public
12 * License version 2.1 as published by the Free Software Foundation
13 * (the "LGPL") or, at your option, under the terms of the Mozilla
14 * Public License Version 1.1 (the "MPL"). If you do not alter this
15 * notice, a recipient may use your version of this file under either
16 * the MPL or the LGPL.
17 *
18 * You should have received a copy of the LGPL along with this library
19 * in the file COPYING-LGPL-2.1; if not, write to the Free Software
20 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21 * You should have received a copy of the MPL along with this library
22 * in the file COPYING-MPL-1.1
23 *
24 * The contents of this file are subject to the Mozilla Public License
25 * Version 1.1 (the "License"); you may not use this file except in
26 * compliance with the License. You may obtain a copy of the License at
27 * http://www.mozilla.org/MPL/
28 *
29 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
30 * OF ANY KIND, either express or implied. See the LGPL or the MPL for
31 * the specific language governing rights and limitations.
32 */
34 #include <cmath>
36 #include <2geom/sbasis.h>
37 #include <2geom/isnan.h>
39 namespace Geom{
41 /** bound the error from term truncation
42 \param tail first term to chop
43 \returns the largest possible error this truncation could give
44 */
45 double SBasis::tailError(unsigned tail) const {
46 Interval bs = bounds_fast(*this, tail);
47 return std::max(fabs(bs.min()),fabs(bs.max()));
48 }
50 /** test all coefficients are finite
51 */
52 bool SBasis::isFinite() const {
53 for(unsigned i = 0; i < size(); i++) {
54 if(!(*this)[i].isFinite())
55 return false;
56 }
57 return true;
58 }
60 /** Compute the value and the first n derivatives
61 \param t position to evaluate
62 \param n number of derivatives (not counting value)
63 \returns a vector with the value and the n derivative evaluations
65 There is an elegant way to compute the value and n derivatives for a polynomial using a variant of horner's rule. Someone will someday work out how for sbasis.
66 */
67 std::vector<double> SBasis::valueAndDerivatives(double t, unsigned n) const {
68 std::vector<double> ret(n);
69 ret.push_back(valueAt(t));
70 SBasis tmp = *this;
71 for(unsigned i = 0; i < n; i++) {
72 tmp.derive();
73 ret[i+1] = tmp.valueAt(t);
74 }
75 return ret;
76 }
79 /** Compute the pointwise sum of a and b (Exact)
80 \param a,b sbasis functions
81 \returns sbasis equal to a+b
83 */
84 SBasis operator+(const SBasis& a, const SBasis& b) {
85 SBasis result;
86 const unsigned out_size = std::max(a.size(), b.size());
87 const unsigned min_size = std::min(a.size(), b.size());
88 result.reserve(out_size);
90 for(unsigned i = 0; i < min_size; i++) {
91 result.push_back(a[i] + b[i]);
92 }
93 for(unsigned i = min_size; i < a.size(); i++)
94 result.push_back(a[i]);
95 for(unsigned i = min_size; i < b.size(); i++)
96 result.push_back(b[i]);
98 assert(result.size() == out_size);
99 return result;
100 }
102 /** Compute the pointwise difference of a and b (Exact)
103 \param a,b sbasis functions
104 \returns sbasis equal to a-b
106 */
107 SBasis operator-(const SBasis& a, const SBasis& b) {
108 SBasis result;
109 const unsigned out_size = std::max(a.size(), b.size());
110 const unsigned min_size = std::min(a.size(), b.size());
111 result.reserve(out_size);
113 for(unsigned i = 0; i < min_size; i++) {
114 result.push_back(a[i] - b[i]);
115 }
116 for(unsigned i = min_size; i < a.size(); i++)
117 result.push_back(a[i]);
118 for(unsigned i = min_size; i < b.size(); i++)
119 result.push_back(-b[i]);
121 assert(result.size() == out_size);
122 return result;
123 }
125 /** Compute the pointwise sum of a and b and store in a (Exact)
126 \param a,b sbasis functions
127 \returns sbasis equal to a+b
129 */
130 SBasis& operator+=(SBasis& a, const SBasis& b) {
131 const unsigned out_size = std::max(a.size(), b.size());
132 const unsigned min_size = std::min(a.size(), b.size());
133 a.reserve(out_size);
135 for(unsigned i = 0; i < min_size; i++)
136 a[i] += b[i];
137 for(unsigned i = min_size; i < b.size(); i++)
138 a.push_back(b[i]);
140 assert(a.size() == out_size);
141 return a;
142 }
144 /** Compute the pointwise difference of a and b and store in a (Exact)
145 \param a,b sbasis functions
146 \returns sbasis equal to a-b
148 */
149 SBasis& operator-=(SBasis& a, const SBasis& b) {
150 const unsigned out_size = std::max(a.size(), b.size());
151 const unsigned min_size = std::min(a.size(), b.size());
152 a.reserve(out_size);
154 for(unsigned i = 0; i < min_size; i++)
155 a[i] -= b[i];
156 for(unsigned i = min_size; i < b.size(); i++)
157 a.push_back(-b[i]);
159 assert(a.size() == out_size);
160 return a;
161 }
163 /** Compute the pointwise product of a and b (Exact)
164 \param a,b sbasis functions
165 \returns sbasis equal to a*b
167 */
168 SBasis operator*(SBasis const &a, double k) {
169 SBasis c;
170 c.reserve(a.size());
171 for(unsigned i = 0; i < a.size(); i++)
172 c.push_back(a[i] * k);
173 return c;
174 }
176 /** Compute the pointwise product of a and b and store the value in a (Exact)
177 \param a,b sbasis functions
178 \returns sbasis equal to a*b
180 */
181 SBasis& operator*=(SBasis& a, double b) {
182 if (a.isZero()) return a;
183 if (b == 0)
184 a.clear();
185 else
186 for(unsigned i = 0; i < a.size(); i++)
187 a[i] *= b;
188 return a;
189 }
191 /** multiply a by x^sh in place (Exact)
192 \param a sbasis function
193 \param sh power
194 \returns a
196 */
197 SBasis shift(SBasis const &a, int sh) {
198 SBasis c = a;
199 if(sh > 0) {
200 c.insert(c.begin(), sh, Linear(0,0));
201 } else {
202 //TODO: truncate
203 }
204 return c;
205 }
207 /** multiply a by x^sh (Exact)
208 \param a linear function
209 \param sh power
210 \returns a* x^sh
212 */
213 SBasis shift(Linear const &a, int sh) {
214 SBasis c;
215 if(sh > 0) {
216 c.insert(c.begin(), sh, Linear(0,0));
217 c.push_back(a);
218 }
219 return c;
220 }
222 #if 0
223 SBasis multiply(SBasis const &a, SBasis const &b) {
224 // c = {a0*b0 - shift(1, a.Tri*b.Tri), a1*b1 - shift(1, a.Tri*b.Tri)}
226 // shift(1, a.Tri*b.Tri)
227 SBasis c;
228 if(a.isZero() || b.isZero())
229 return c;
230 c.resize(a.size() + b.size(), Linear(0,0));
231 for(unsigned j = 0; j < b.size(); j++) {
232 for(unsigned i = j; i < a.size()+j; i++) {
233 double tri = Tri(b[j])*Tri(a[i-j]);
234 c[i+1/*shift*/] += Linear(Hat(-tri));
235 }
236 }
237 for(unsigned j = 0; j < b.size(); j++) {
238 for(unsigned i = j; i < a.size()+j; i++) {
239 for(unsigned dim = 0; dim < 2; dim++)
240 c[i][dim] += b[j][dim]*a[i-j][dim];
241 }
242 }
243 c.normalize();
244 //assert(!(0 == c.back()[0] && 0 == c.back()[1]));
245 return c;
246 }
247 #else
249 /** Compute the pointwise product of a and b adding c (Exact)
250 \param a,b,c sbasis functions
251 \returns sbasis equal to a*b+c
253 The added term is almost free
254 */
255 SBasis multiply_add(SBasis const &a, SBasis const &b, SBasis c) {
256 if(a.isZero() || b.isZero())
257 return c;
258 c.resize(a.size() + b.size(), Linear(0,0));
259 for(unsigned j = 0; j < b.size(); j++) {
260 for(unsigned i = j; i < a.size()+j; i++) {
261 double tri = Tri(b[j])*Tri(a[i-j]);
262 c[i+1/*shift*/] += Linear(Hat(-tri));
263 }
264 }
265 for(unsigned j = 0; j < b.size(); j++) {
266 for(unsigned i = j; i < a.size()+j; i++) {
267 for(unsigned dim = 0; dim < 2; dim++)
268 c[i][dim] += b[j][dim]*a[i-j][dim];
269 }
270 }
271 c.normalize();
272 //assert(!(0 == c.back()[0] && 0 == c.back()[1]));
273 return c;
274 }
276 /** Compute the pointwise product of a and b (Exact)
277 \param a,b sbasis functions
278 \returns sbasis equal to a*b
280 */
281 SBasis multiply(SBasis const &a, SBasis const &b) {
282 SBasis c;
283 if(a.isZero() || b.isZero())
284 return c;
285 return multiply_add(a, b, c);
286 }
287 #endif
288 /** Compute the integral of a (Exact)
289 \param a sbasis functions
290 \returns sbasis integral(a)
292 */
293 SBasis integral(SBasis const &c) {
294 SBasis a;
295 a.resize(c.size() + 1, Linear(0,0));
296 a[0] = Linear(0,0);
298 for(unsigned k = 1; k < c.size() + 1; k++) {
299 double ahat = -Tri(c[k-1])/(2*k);
300 a[k] = Hat(ahat);
301 }
302 double aTri = 0;
303 for(int k = c.size()-1; k >= 0; k--) {
304 aTri = (Hat(c[k]).d + (k+1)*aTri/2)/(2*k+1);
305 a[k][0] -= aTri/2;
306 a[k][1] += aTri/2;
307 }
308 a.normalize();
309 return a;
310 }
312 /** Compute the derivative of a (Exact)
313 \param a sbasis functions
314 \returns sbasis da/dt
316 */
317 SBasis derivative(SBasis const &a) {
318 SBasis c;
319 c.resize(a.size(), Linear(0,0));
320 if(a.isZero())
321 return c;
323 for(unsigned k = 0; k < a.size()-1; k++) {
324 double d = (2*k+1)*(a[k][1] - a[k][0]);
326 c[k][0] = d + (k+1)*a[k+1][0];
327 c[k][1] = d - (k+1)*a[k+1][1];
328 }
329 int k = a.size()-1;
330 double d = (2*k+1)*(a[k][1] - a[k][0]);
331 if(d == 0)
332 c.pop_back();
333 else {
334 c[k][0] = d;
335 c[k][1] = d;
336 }
338 return c;
339 }
341 /** Compute the derivative of this inplace (Exact)
343 */
344 void SBasis::derive() { // in place version
345 if(isZero()) return;
346 for(unsigned k = 0; k < size()-1; k++) {
347 double d = (2*k+1)*((*this)[k][1] - (*this)[k][0]);
349 (*this)[k][0] = d + (k+1)*(*this)[k+1][0];
350 (*this)[k][1] = d - (k+1)*(*this)[k+1][1];
351 }
352 int k = size()-1;
353 double d = (2*k+1)*((*this)[k][1] - (*this)[k][0]);
354 if(d == 0)
355 pop_back();
356 else {
357 (*this)[k][0] = d;
358 (*this)[k][1] = d;
359 }
360 }
362 /** Compute the sqrt of a
363 \param a sbasis functions
364 \returns sbasis \f[ \sqrt{a} \f]
366 It is recommended to use the piecewise version unless you have good reason.
367 TODO: convert int k to unsigned k, and remove cast
368 */
369 SBasis sqrt(SBasis const &a, int k) {
370 SBasis c;
371 if(a.isZero() || k == 0)
372 return c;
373 c.resize(k, Linear(0,0));
374 c[0] = Linear(std::sqrt(a[0][0]), std::sqrt(a[0][1]));
375 SBasis r = a - multiply(c, c); // remainder
377 for(unsigned i = 1; i <= (unsigned)k and i<r.size(); i++) {
378 Linear ci(r[i][0]/(2*c[0][0]), r[i][1]/(2*c[0][1]));
379 SBasis cisi = shift(ci, i);
380 r -= multiply(shift((c*2 + cisi), i), SBasis(ci));
381 r.truncate(k+1);
382 c += cisi;
383 if(r.tailError(i) == 0) // if exact
384 break;
385 }
387 return c;
388 }
390 /** Compute the recpirocal of a
391 \param a sbasis functions
392 \returns sbasis 1/a
394 It is recommended to use the piecewise version unless you have good reason.
395 */
396 SBasis reciprocal(Linear const &a, int k) {
397 SBasis c;
398 assert(!a.isZero());
399 c.resize(k, Linear(0,0));
400 double r_s0 = (Tri(a)*Tri(a))/(-a[0]*a[1]);
401 double r_s0k = 1;
402 for(unsigned i = 0; i < (unsigned)k; i++) {
403 c[i] = Linear(r_s0k/a[0], r_s0k/a[1]);
404 r_s0k *= r_s0;
405 }
406 return c;
407 }
409 /** Compute a / b to k terms
410 \param a,b sbasis functions
411 \returns sbasis a/b
413 It is recommended to use the piecewise version unless you have good reason.
414 */
415 SBasis divide(SBasis const &a, SBasis const &b, int k) {
416 SBasis c;
417 assert(!a.isZero());
418 SBasis r = a; // remainder
420 k++;
421 r.resize(k, Linear(0,0));
422 c.resize(k, Linear(0,0));
424 for(unsigned i = 0; i < (unsigned)k; i++) {
425 Linear ci(r[i][0]/b[0][0], r[i][1]/b[0][1]); //H0
426 c[i] += ci;
427 r -= shift(multiply(ci,b), i);
428 r.truncate(k+1);
429 if(r.tailError(i) == 0) // if exact
430 break;
431 }
433 return c;
434 }
436 /** Compute a composed with b
437 \param a,b sbasis functions
438 \returns sbasis a(b(t))
440 return a0 + s(a1 + s(a2 +... where s = (1-u)u; ak =(1 - u)a^0_k + ua^1_k
441 */
442 SBasis compose(SBasis const &a, SBasis const &b) {
443 SBasis s = multiply((SBasis(Linear(1,1))-b), b);
444 SBasis r;
446 for(int i = a.size()-1; i >= 0; i--) {
447 r = multiply_add(r, s, SBasis(Linear(Hat(a[i][0]))) - b*a[i][0] + b*a[i][1]);
448 }
449 return r;
450 }
452 /** Compute a composed with b to k terms
453 \param a,b sbasis functions
454 \returns sbasis a(b(t))
456 return a0 + s(a1 + s(a2 +... where s = (1-u)u; ak =(1 - u)a^0_k + ua^1_k
457 */
458 SBasis compose(SBasis const &a, SBasis const &b, unsigned k) {
459 SBasis s = multiply((SBasis(Linear(1,1))-b), b);
460 SBasis r;
462 for(int i = a.size()-1; i >= 0; i--) {
463 r = multiply_add(r, s, SBasis(Linear(Hat(a[i][0]))) - b*a[i][0] + b*a[i][1]);
464 }
465 r.truncate(k);
466 return r;
467 }
469 /*
470 Inversion algorithm. The notation is certainly very misleading. The
471 pseudocode should say:
473 c(v) := 0
474 r(u) := r_0(u) := u
475 for i:=0 to k do
476 c_i(v) := H_0(r_i(u)/(t_1)^i; u)
477 c(v) := c(v) + c_i(v)*t^i
478 r(u) := r(u) ? c_i(u)*(t(u))^i
479 endfor
480 */
482 //#define DEBUG_INVERSION 1
484 /** find the function a^-1 such that a^-1 composed with a to k terms is the identity function
485 \param a sbasis function
486 \returns sbasis a^-1 s.t. a^-1(a(t)) = 1
488 The function must have 'unit range'("a00 = 0 and a01 = 1") and be monotonic.
489 */
490 SBasis inverse(SBasis a, int k) {
491 assert(a.size() > 0);
492 double a0 = a[0][0];
493 if(a0 != 0) {
494 a -= a0;
495 }
496 double a1 = a[0][1];
497 assert(a1 != 0); // not invertable.
499 if(a1 != 1) {
500 a /= a1;
501 }
502 SBasis c; // c(v) := 0
503 if(a.size() >= 2 && k == 2) {
504 c.push_back(Linear(0,1));
505 Linear t1(1+a[1][0], 1-a[1][1]); // t_1
506 c.push_back(Linear(-a[1][0]/t1[0], -a[1][1]/t1[1]));
507 } else if(a.size() >= 2) { // non linear
508 SBasis r = Linear(0,1); // r(u) := r_0(u) := u
509 Linear t1(1./(1+a[1][0]), 1./(1-a[1][1])); // 1./t_1
510 Linear one(1,1);
511 Linear t1i = one; // t_1^0
512 SBasis one_minus_a = SBasis(one) - a;
513 SBasis t = multiply(one_minus_a, a); // t(u)
514 SBasis ti(one); // t(u)^0
515 #ifdef DEBUG_INVERSION
516 std::cout << "a=" << a << std::endl;
517 std::cout << "1-a=" << one_minus_a << std::endl;
518 std::cout << "t1=" << t1 << std::endl;
519 //assert(t1 == t[1]);
520 #endif
522 c.resize(k+1, Linear(0,0));
523 for(unsigned i = 0; i < (unsigned)k; i++) { // for i:=0 to k do
524 #ifdef DEBUG_INVERSION
525 std::cout << "-------" << i << ": ---------" <<std::endl;
526 std::cout << "r=" << r << std::endl
527 << "c=" << c << std::endl
528 << "ti=" << ti << std::endl
529 << std::endl;
530 #endif
531 if(r.size() <= i) // ensure enough space in the remainder, probably not needed
532 r.resize(i+1, Linear(0,0));
533 Linear ci(r[i][0]*t1i[0], r[i][1]*t1i[1]); // c_i(v) := H_0(r_i(u)/(t_1)^i; u)
534 #ifdef DEBUG_INVERSION
535 std::cout << "t1i=" << t1i << std::endl;
536 std::cout << "ci=" << ci << std::endl;
537 #endif
538 for(int dim = 0; dim < 2; dim++) // t1^-i *= 1./t1
539 t1i[dim] *= t1[dim];
540 c[i] = ci; // c(v) := c(v) + c_i(v)*t^i
541 // change from v to u parameterisation
542 SBasis civ = one_minus_a*ci[0] + a*ci[1];
543 // r(u) := r(u) - c_i(u)*(t(u))^i
544 // We can truncate this to the number of final terms, as no following terms can
545 // contribute to the result.
546 r -= multiply(civ,ti);
547 r.truncate(k);
548 if(r.tailError(i) == 0)
549 break; // yay!
550 ti = multiply(ti,t);
551 }
552 #ifdef DEBUG_INVERSION
553 std::cout << "##########################" << std::endl;
554 #endif
555 } else
556 c = Linear(0,1); // linear
557 c -= a0; // invert the offset
558 c /= a1; // invert the slope
559 return c;
560 }
562 /** Compute the sine of a to k terms
563 \param b linear function
564 \returns sbasis sin(a)
566 It is recommended to use the piecewise version unless you have good reason.
567 */
568 SBasis sin(Linear b, int k) {
569 SBasis s = Linear(std::sin(b[0]), std::sin(b[1]));
570 Tri tr(s[0]);
571 double t2 = Tri(b);
572 s.push_back(Linear(std::cos(b[0])*t2 - tr, -std::cos(b[1])*t2 + tr));
574 t2 *= t2;
575 for(int i = 0; i < k; i++) {
576 Linear bo(4*(i+1)*s[i+1][0] - 2*s[i+1][1],
577 -2*s[i+1][0] + 4*(i+1)*s[i+1][1]);
578 bo -= s[i]*(t2/(i+1));
581 s.push_back(bo/double(i+2));
582 }
584 return s;
585 }
587 /** Compute the cosine of a
588 \param b linear function
589 \returns sbasis cos(a)
591 It is recommended to use the piecewise version unless you have good reason.
592 */
593 SBasis cos(Linear bo, int k) {
594 return sin(Linear(bo[0] + M_PI/2,
595 bo[1] + M_PI/2),
596 k);
597 }
599 /** compute fog^-1.
600 \param f,g sbasis functions
601 \returns sbasis f(g^-1(t)).
603 ("zero" = double comparison threshold. *!*we might divide by "zero"*!*)
604 TODO: compute order according to tol?
605 TODO: requires g(0)=0 & g(1)=1 atm... adaptation to other cases should be obvious!
606 */
607 SBasis compose_inverse(SBasis const &f, SBasis const &g, unsigned order, double zero){
608 SBasis result; //result
609 SBasis r=f; //remainder
610 SBasis Pk=Linear(1)-g,Qk=g,sg=Pk*Qk;
611 Pk.truncate(order);
612 Qk.truncate(order);
613 Pk.resize(order,Linear(0.));
614 Qk.resize(order,Linear(0.));
615 r.resize(order,Linear(0.));
617 int vs= valuation(sg,zero);
619 for (unsigned k=0; k<order; k+=vs){
620 double p10 = Pk.at(k)[0];// we have to solve the linear system:
621 double p01 = Pk.at(k)[1];//
622 double q10 = Qk.at(k)[0];// p10*a + q10*b = r10
623 double q01 = Qk.at(k)[1];// &
624 double r10 = r.at(k)[0];// p01*a + q01*b = r01
625 double r01 = r.at(k)[1];//
626 double a,b;
627 double det = p10*q01-p01*q10;
629 //TODO: handle det~0!!
630 if (fabs(det)<zero){
631 det = zero;
632 a=b=0;
633 }else{
634 a=( q01*r10-q10*r01)/det;
635 b=(-p01*r10+p10*r01)/det;
636 }
637 result.push_back(Linear(a,b));
638 r=r-Pk*a-Qk*b;
640 Pk=Pk*sg;
641 Qk=Qk*sg;
642 Pk.truncate(order);
643 Qk.truncate(order);
644 r.truncate(order);
645 }
646 result.normalize();
647 return result;
648 }
650 }
652 /*
653 Local Variables:
654 mode:c++
655 c-file-style:"stroustrup"
656 c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
657 indent-tabs-mode:nil
658 fill-column:99
659 End:
660 */
661 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :