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+1);
69 ret[0] = valueAt(t);
70 SBasis tmp = *this;
71 for(unsigned i = 1; i < n+1; i++) {
72 tmp.derive();
73 ret[i] = 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 const unsigned out_size = std::max(a.size(), b.size());
86 const unsigned min_size = std::min(a.size(), b.size());
87 SBasis result(out_size, Linear());
89 for(unsigned i = 0; i < min_size; i++) {
90 result[i] = a[i] + b[i];
91 }
92 for(unsigned i = min_size; i < a.size(); i++)
93 result[i] = a[i];
94 for(unsigned i = min_size; i < b.size(); i++)
95 result[i] = b[i];
97 assert(result.size() == out_size);
98 return result;
99 }
101 /** Compute the pointwise difference of a and b (Exact)
102 \param a,b sbasis functions
103 \returns sbasis equal to a-b
105 */
106 SBasis operator-(const SBasis& a, const SBasis& b) {
107 const unsigned out_size = std::max(a.size(), b.size());
108 const unsigned min_size = std::min(a.size(), b.size());
109 SBasis result(out_size, Linear());
111 for(unsigned i = 0; i < min_size; i++) {
112 result[i] = a[i] - b[i];
113 }
114 for(unsigned i = min_size; i < a.size(); i++)
115 result[i] = a[i];
116 for(unsigned i = min_size; i < b.size(); i++)
117 result[i] = -b[i];
119 assert(result.size() == out_size);
120 return result;
121 }
123 /** Compute the pointwise sum of a and b and store in a (Exact)
124 \param a,b sbasis functions
125 \returns sbasis equal to a+b
127 */
128 SBasis& operator+=(SBasis& a, const SBasis& b) {
129 const unsigned out_size = std::max(a.size(), b.size());
130 const unsigned min_size = std::min(a.size(), b.size());
131 a.resize(out_size);
133 for(unsigned i = 0; i < min_size; i++)
134 a[i] += b[i];
135 for(unsigned i = min_size; i < b.size(); i++)
136 a[i] = b[i];
138 assert(a.size() == out_size);
139 return a;
140 }
142 /** Compute the pointwise difference of a and b and store in a (Exact)
143 \param a,b sbasis functions
144 \returns sbasis equal to a-b
146 */
147 SBasis& operator-=(SBasis& a, const SBasis& b) {
148 const unsigned out_size = std::max(a.size(), b.size());
149 const unsigned min_size = std::min(a.size(), b.size());
150 a.resize(out_size);
152 for(unsigned i = 0; i < min_size; i++)
153 a[i] -= b[i];
154 for(unsigned i = min_size; i < b.size(); i++)
155 a[i] = -b[i];
157 assert(a.size() == out_size);
158 return a;
159 }
161 /** Compute the pointwise product of a and b (Exact)
162 \param a,b sbasis functions
163 \returns sbasis equal to a*b
165 */
166 SBasis operator*(SBasis const &a, double k) {
167 SBasis c(a.size(), Linear());
168 for(unsigned i = 0; i < a.size(); i++)
169 c[i] = a[i] * k;
170 return c;
171 }
173 /** Compute the pointwise product of a and b and store the value in a (Exact)
174 \param a,b sbasis functions
175 \returns sbasis equal to a*b
177 */
178 SBasis& operator*=(SBasis& a, double b) {
179 if (a.isZero()) return a;
180 if (b == 0)
181 a.clear();
182 else
183 for(unsigned i = 0; i < a.size(); i++)
184 a[i] *= b;
185 return a;
186 }
188 /** multiply a by x^sh in place (Exact)
189 \param a sbasis function
190 \param sh power
191 \returns a
193 */
194 SBasis shift(SBasis const &a, int sh) {
195 size_t n = a.size()+sh;
196 SBasis c(n, Linear());
197 size_t m = std::max(0, sh);
199 for(int i = 0; i < sh; i++)
200 c[i] = Linear(0,0);
201 for(size_t i = m, j = 0; i < n; i++, j++)
202 c[i] = a[j];
203 return c;
204 }
206 /** multiply a by x^sh (Exact)
207 \param a linear function
208 \param sh power
209 \returns a* x^sh
211 */
212 SBasis shift(Linear const &a, int sh) {
213 size_t n = 1+sh;
214 SBasis c(n, Linear());
216 for(int i = 0; i < sh; i++)
217 c[i] = Linear(0,0);
218 if(sh >= 0)
219 c[sh] = a;
220 return c;
221 }
223 #if 0
224 SBasis multiply(SBasis const &a, SBasis const &b) {
225 // c = {a0*b0 - shift(1, a.Tri*b.Tri), a1*b1 - shift(1, a.Tri*b.Tri)}
227 // shift(1, a.Tri*b.Tri)
228 SBasis c;
229 if(a.isZero() || b.isZero())
230 return c;
231 c.resize(a.size() + b.size(), Linear(0,0));
232 for(unsigned j = 0; j < b.size(); j++) {
233 for(unsigned i = j; i < a.size()+j; i++) {
234 double tri = b[j].tri()*a[i-j].tri();
235 c[i+1/*shift*/] += Linear(-tri);
236 }
237 }
238 for(unsigned j = 0; j < b.size(); j++) {
239 for(unsigned i = j; i < a.size()+j; i++) {
240 for(unsigned dim = 0; dim < 2; dim++)
241 c[i][dim] += b[j][dim]*a[i-j][dim];
242 }
243 }
244 c.normalize();
245 //assert(!(0 == c.back()[0] && 0 == c.back()[1]));
246 return c;
247 }
248 #else
250 /** Compute the pointwise product of a and b adding c (Exact)
251 \param a,b,c sbasis functions
252 \returns sbasis equal to a*b+c
254 The added term is almost free
255 */
256 SBasis multiply_add(SBasis const &a, SBasis const &b, SBasis c) {
257 if(a.isZero() || b.isZero())
258 return c;
259 c.resize(a.size() + b.size(), Linear(0,0));
260 for(unsigned j = 0; j < b.size(); j++) {
261 for(unsigned i = j; i < a.size()+j; i++) {
262 double tri = b[j].tri()*a[i-j].tri();
263 c[i+1/*shift*/] += Linear(-tri);
264 }
265 }
266 for(unsigned j = 0; j < b.size(); j++) {
267 for(unsigned i = j; i < a.size()+j; i++) {
268 for(unsigned dim = 0; dim < 2; dim++)
269 c[i][dim] += b[j][dim]*a[i-j][dim];
270 }
271 }
272 c.normalize();
273 //assert(!(0 == c.back()[0] && 0 == c.back()[1]));
274 return c;
275 }
277 /** Compute the pointwise product of a and b (Exact)
278 \param a,b sbasis functions
279 \returns sbasis equal to a*b
281 */
282 SBasis multiply(SBasis const &a, SBasis const &b) {
283 SBasis c;
284 if(a.isZero() || b.isZero())
285 return c;
286 return multiply_add(a, b, c);
287 }
288 #endif
289 /** Compute the integral of a (Exact)
290 \param a sbasis functions
291 \returns sbasis integral(a)
293 */
294 SBasis integral(SBasis const &c) {
295 SBasis a;
296 a.resize(c.size() + 1, Linear(0,0));
297 a[0] = Linear(0,0);
299 for(unsigned k = 1; k < c.size() + 1; k++) {
300 double ahat = -c[k-1].tri()/(2*k);
301 a[k][0] = a[k][1] = ahat;
302 }
303 double aTri = 0;
304 for(int k = c.size()-1; k >= 0; k--) {
305 aTri = (c[k].hat() + (k+1)*aTri/2)/(2*k+1);
306 a[k][0] -= aTri/2;
307 a[k][1] += aTri/2;
308 }
309 a.normalize();
310 return a;
311 }
313 /** Compute the derivative of a (Exact)
314 \param a sbasis functions
315 \returns sbasis da/dt
317 */
318 SBasis derivative(SBasis const &a) {
319 SBasis c;
320 c.resize(a.size(), Linear(0,0));
321 if(a.isZero())
322 return c;
324 for(unsigned k = 0; k < a.size()-1; k++) {
325 double d = (2*k+1)*(a[k][1] - a[k][0]);
327 c[k][0] = d + (k+1)*a[k+1][0];
328 c[k][1] = d - (k+1)*a[k+1][1];
329 }
330 int k = a.size()-1;
331 double d = (2*k+1)*(a[k][1] - a[k][0]);
332 if(d == 0)
333 c.pop_back();
334 else {
335 c[k][0] = d;
336 c[k][1] = d;
337 }
339 return c;
340 }
342 /** Compute the derivative of this inplace (Exact)
344 */
345 void SBasis::derive() { // in place version
346 if(isZero()) return;
347 for(unsigned k = 0; k < size()-1; k++) {
348 double d = (2*k+1)*((*this)[k][1] - (*this)[k][0]);
350 (*this)[k][0] = d + (k+1)*(*this)[k+1][0];
351 (*this)[k][1] = d - (k+1)*(*this)[k+1][1];
352 }
353 int k = size()-1;
354 double d = (2*k+1)*((*this)[k][1] - (*this)[k][0]);
355 if(d == 0)
356 pop_back();
357 else {
358 (*this)[k][0] = d;
359 (*this)[k][1] = d;
360 }
361 }
363 /** Compute the sqrt of a
364 \param a sbasis functions
365 \returns sbasis \f[ \sqrt{a} \f]
367 It is recommended to use the piecewise version unless you have good reason.
368 TODO: convert int k to unsigned k, and remove cast
369 */
370 SBasis sqrt(SBasis const &a, int k) {
371 SBasis c;
372 if(a.isZero() || k == 0)
373 return c;
374 c.resize(k, Linear(0,0));
375 c[0] = Linear(std::sqrt(a[0][0]), std::sqrt(a[0][1]));
376 SBasis r = a - multiply(c, c); // remainder
378 for(unsigned i = 1; i <= (unsigned)k and i<r.size(); i++) {
379 Linear ci(r[i][0]/(2*c[0][0]), r[i][1]/(2*c[0][1]));
380 SBasis cisi = shift(ci, i);
381 r -= multiply(shift((c*2 + cisi), i), SBasis(ci));
382 r.truncate(k+1);
383 c += cisi;
384 if(r.tailError(i) == 0) // if exact
385 break;
386 }
388 return c;
389 }
391 /** Compute the recpirocal of a
392 \param a sbasis functions
393 \returns sbasis 1/a
395 It is recommended to use the piecewise version unless you have good reason.
396 */
397 SBasis reciprocal(Linear const &a, int k) {
398 SBasis c;
399 assert(!a.isZero());
400 c.resize(k, Linear(0,0));
401 double r_s0 = (a.tri()*a.tri())/(-a[0]*a[1]);
402 double r_s0k = 1;
403 for(unsigned i = 0; i < (unsigned)k; i++) {
404 c[i] = Linear(r_s0k/a[0], r_s0k/a[1]);
405 r_s0k *= r_s0;
406 }
407 return c;
408 }
410 /** Compute a / b to k terms
411 \param a,b sbasis functions
412 \returns sbasis a/b
414 It is recommended to use the piecewise version unless you have good reason.
415 */
416 SBasis divide(SBasis const &a, SBasis const &b, int k) {
417 SBasis c;
418 assert(!a.isZero());
419 SBasis r = a; // remainder
421 k++;
422 r.resize(k, Linear(0,0));
423 c.resize(k, Linear(0,0));
425 for(unsigned i = 0; i < (unsigned)k; i++) {
426 Linear ci(r[i][0]/b[0][0], r[i][1]/b[0][1]); //H0
427 c[i] += ci;
428 r -= shift(multiply(ci,b), i);
429 r.truncate(k+1);
430 if(r.tailError(i) == 0) // if exact
431 break;
432 }
434 return c;
435 }
437 /** Compute a composed with b
438 \param a,b sbasis functions
439 \returns sbasis a(b(t))
441 return a0 + s(a1 + s(a2 +... where s = (1-u)u; ak =(1 - u)a^0_k + ua^1_k
442 */
443 SBasis compose(SBasis const &a, SBasis const &b) {
444 SBasis s = multiply((SBasis(Linear(1,1))-b), b);
445 SBasis r;
447 for(int i = a.size()-1; i >= 0; i--) {
448 r = multiply_add(r, s, SBasis(Linear(a[i][0])) - b*a[i][0] + b*a[i][1]);
449 }
450 return r;
451 }
453 /** Compute a composed with b to k terms
454 \param a,b sbasis functions
455 \returns sbasis a(b(t))
457 return a0 + s(a1 + s(a2 +... where s = (1-u)u; ak =(1 - u)a^0_k + ua^1_k
458 */
459 SBasis compose(SBasis const &a, SBasis const &b, unsigned k) {
460 SBasis s = multiply((SBasis(Linear(1,1))-b), b);
461 SBasis r;
463 for(int i = a.size()-1; i >= 0; i--) {
464 r = multiply_add(r, s, SBasis(Linear(a[i][0])) - b*a[i][0] + b*a[i][1]);
465 }
466 r.truncate(k);
467 return r;
468 }
470 /*
471 Inversion algorithm. The notation is certainly very misleading. The
472 pseudocode should say:
474 c(v) := 0
475 r(u) := r_0(u) := u
476 for i:=0 to k do
477 c_i(v) := H_0(r_i(u)/(t_1)^i; u)
478 c(v) := c(v) + c_i(v)*t^i
479 r(u) := r(u) ? c_i(u)*(t(u))^i
480 endfor
481 */
483 //#define DEBUG_INVERSION 1
485 /** find the function a^-1 such that a^-1 composed with a to k terms is the identity function
486 \param a sbasis function
487 \returns sbasis a^-1 s.t. a^-1(a(t)) = 1
489 The function must have 'unit range'("a00 = 0 and a01 = 1") and be monotonic.
490 */
491 SBasis inverse(SBasis a, int k) {
492 assert(a.size() > 0);
493 double a0 = a[0][0];
494 if(a0 != 0) {
495 a -= a0;
496 }
497 double a1 = a[0][1];
498 assert(a1 != 0); // not invertable.
500 if(a1 != 1) {
501 a /= a1;
502 }
503 SBasis c(k, Linear()); // c(v) := 0
504 if(a.size() >= 2 && k == 2) {
505 c[0] = Linear(0,1);
506 Linear t1(1+a[1][0], 1-a[1][1]); // t_1
507 c[1] = Linear(-a[1][0]/t1[0], -a[1][1]/t1[1]);
508 } else if(a.size() >= 2) { // non linear
509 SBasis r = Linear(0,1); // r(u) := r_0(u) := u
510 Linear t1(1./(1+a[1][0]), 1./(1-a[1][1])); // 1./t_1
511 Linear one(1,1);
512 Linear t1i = one; // t_1^0
513 SBasis one_minus_a = SBasis(one) - a;
514 SBasis t = multiply(one_minus_a, a); // t(u)
515 SBasis ti(one); // t(u)^0
516 #ifdef DEBUG_INVERSION
517 std::cout << "a=" << a << std::endl;
518 std::cout << "1-a=" << one_minus_a << std::endl;
519 std::cout << "t1=" << t1 << std::endl;
520 //assert(t1 == t[1]);
521 #endif
523 //c.resize(k+1, Linear(0,0));
524 for(unsigned i = 0; i < (unsigned)k; i++) { // for i:=0 to k do
525 #ifdef DEBUG_INVERSION
526 std::cout << "-------" << i << ": ---------" <<std::endl;
527 std::cout << "r=" << r << std::endl
528 << "c=" << c << std::endl
529 << "ti=" << ti << std::endl
530 << std::endl;
531 #endif
532 if(r.size() <= i) // ensure enough space in the remainder, probably not needed
533 r.resize(i+1, Linear(0,0));
534 Linear ci(r[i][0]*t1i[0], r[i][1]*t1i[1]); // c_i(v) := H_0(r_i(u)/(t_1)^i; u)
535 #ifdef DEBUG_INVERSION
536 std::cout << "t1i=" << t1i << std::endl;
537 std::cout << "ci=" << ci << std::endl;
538 #endif
539 for(int dim = 0; dim < 2; dim++) // t1^-i *= 1./t1
540 t1i[dim] *= t1[dim];
541 c[i] = ci; // c(v) := c(v) + c_i(v)*t^i
542 // change from v to u parameterisation
543 SBasis civ = one_minus_a*ci[0] + a*ci[1];
544 // r(u) := r(u) - c_i(u)*(t(u))^i
545 // We can truncate this to the number of final terms, as no following terms can
546 // contribute to the result.
547 r -= multiply(civ,ti);
548 r.truncate(k);
549 if(r.tailError(i) == 0)
550 break; // yay!
551 ti = multiply(ti,t);
552 }
553 #ifdef DEBUG_INVERSION
554 std::cout << "##########################" << std::endl;
555 #endif
556 } else
557 c = Linear(0,1); // linear
558 c -= a0; // invert the offset
559 c /= a1; // invert the slope
560 return c;
561 }
563 /** Compute the sine of a to k terms
564 \param b linear function
565 \returns sbasis sin(a)
567 It is recommended to use the piecewise version unless you have good reason.
568 */
569 SBasis sin(Linear b, int k) {
570 SBasis s(k+2, Linear());
571 s[0] = Linear(std::sin(b[0]), std::sin(b[1]));
572 double tr = s[0].tri();
573 double t2 = b.tri();
574 s[1] = Linear(std::cos(b[0])*t2 - tr, -std::cos(b[1])*t2 + tr);
576 t2 *= t2;
577 for(int i = 0; i < k; i++) {
578 Linear bo(4*(i+1)*s[i+1][0] - 2*s[i+1][1],
579 -2*s[i+1][0] + 4*(i+1)*s[i+1][1]);
580 bo -= s[i]*(t2/(i+1));
583 s[i+2] = bo/double(i+2);
584 }
586 return s;
587 }
589 /** Compute the cosine of a
590 \param b linear function
591 \returns sbasis cos(a)
593 It is recommended to use the piecewise version unless you have good reason.
594 */
595 SBasis cos(Linear bo, int k) {
596 return sin(Linear(bo[0] + M_PI/2,
597 bo[1] + M_PI/2),
598 k);
599 }
601 /** compute fog^-1.
602 \param f,g sbasis functions
603 \returns sbasis f(g^-1(t)).
605 ("zero" = double comparison threshold. *!*we might divide by "zero"*!*)
606 TODO: compute order according to tol?
607 TODO: requires g(0)=0 & g(1)=1 atm... adaptation to other cases should be obvious!
608 */
609 SBasis compose_inverse(SBasis const &f, SBasis const &g, unsigned order, double zero){
610 SBasis result(order, Linear()); //result
611 SBasis r=f; //remainder
612 SBasis Pk=Linear(1)-g,Qk=g,sg=Pk*Qk;
613 Pk.truncate(order);
614 Qk.truncate(order);
615 Pk.resize(order,Linear(0.));
616 Qk.resize(order,Linear(0.));
617 r.resize(order,Linear(0.));
619 int vs= valuation(sg,zero);
621 for (unsigned k=0; k<order; k+=vs){
622 double p10 = Pk.at(k)[0];// we have to solve the linear system:
623 double p01 = Pk.at(k)[1];//
624 double q10 = Qk.at(k)[0];// p10*a + q10*b = r10
625 double q01 = Qk.at(k)[1];// &
626 double r10 = r.at(k)[0];// p01*a + q01*b = r01
627 double r01 = r.at(k)[1];//
628 double a,b;
629 double det = p10*q01-p01*q10;
631 //TODO: handle det~0!!
632 if (fabs(det)<zero){
633 det = zero;
634 a=b=0;
635 }else{
636 a=( q01*r10-q10*r01)/det;
637 b=(-p01*r10+p10*r01)/det;
638 }
639 result[k] = Linear(a,b);
640 r=r-Pk*a-Qk*b;
642 Pk=Pk*sg;
643 Qk=Qk*sg;
644 Pk.truncate(order);
645 Qk.truncate(order);
646 r.truncate(order);
647 }
648 result.normalize();
649 return result;
650 }
652 }
654 /*
655 Local Variables:
656 mode:c++
657 c-file-style:"stroustrup"
658 c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
659 indent-tabs-mode:nil
660 fill-column:99
661 End:
662 */
663 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :