Point Cloud Library (PCL)  1.9.1-dev
polynomial.hpp
1 /*
2 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 Redistributions of source code must retain the above copyright notice, this list of
9 conditions and the following disclaimer. Redistributions in binary form must reproduce
10 the above copyright notice, this list of conditions and the following disclaimer
11 in the documentation and/or other materials provided with the distribution.
12 
13 Neither the name of the Johns Hopkins University nor the names of its contributors
14 may be used to endorse or promote products derived from this software without specific
15 prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 DAMAGE.
27 */
28 
29 #include <float.h>
30 #include <math.h>
31 #include <algorithm>
32 #include "factor.h"
33 
34 ////////////////
35 // Polynomial //
36 ////////////////
37 
38 namespace pcl
39 {
40  namespace poisson
41  {
42 
43 
44  template<int Degree>
45  Polynomial<Degree>::Polynomial(void){memset(coefficients,0,sizeof(double)*(Degree+1));}
46  template<int Degree>
47  template<int Degree2>
49  memset(coefficients,0,sizeof(double)*(Degree+1));
50  for(int i=0;i<=Degree && i<=Degree2;i++){coefficients[i]=P.coefficients[i];}
51  }
52 
53 
54  template<int Degree>
55  template<int Degree2>
57  int d=Degree<Degree2?Degree:Degree2;
58  memset(coefficients,0,sizeof(double)*(Degree+1));
59  memcpy(coefficients,p.coefficients,sizeof(double)*(d+1));
60  return *this;
61  }
62 
63  template<int Degree>
65  Polynomial<Degree-1> p;
66  for(int i=0;i<Degree;i++){p.coefficients[i]=coefficients[i+1]*(i+1);}
67  return p;
68  }
69 
70  template<int Degree>
73  p.coefficients[0]=0;
74  for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);}
75  return p;
76  }
77  template<> double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; }
78  template<> double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; }
79  template<> double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; }
80  template<int Degree>
81  double Polynomial<Degree>::operator() ( double t ) const{
82  double v=coefficients[Degree];
83  for( int d=Degree-1 ; d>=0 ; d-- ) v = v*t + coefficients[d];
84  return v;
85  }
86  template<int Degree>
87  double Polynomial<Degree>::integral( double tMin , double tMax ) const
88  {
89  double v=0;
90  double t1,t2;
91  t1=tMin;
92  t2=tMax;
93  for(int i=0;i<=Degree;i++){
94  v+=coefficients[i]*(t2-t1)/(i+1);
95  if(t1!=-DBL_MAX && t1!=DBL_MAX){t1*=tMin;}
96  if(t2!=-DBL_MAX && t2!=DBL_MAX){t2*=tMax;}
97  }
98  return v;
99  }
100  template<int Degree>
102  for(int i=0;i<=Degree;i++){if(coefficients[i]!=p.coefficients[i]){return 0;}}
103  return 1;
104  }
105  template<int Degree>
107  for(int i=0;i<=Degree;i++){if(coefficients[i]==p.coefficients[i]){return 0;}}
108  return 1;
109  }
110  template<int Degree>
111  int Polynomial<Degree>::isZero(void) const{
112  for(int i=0;i<=Degree;i++){if(coefficients[i]!=0){return 0;}}
113  return 1;
114  }
115  template<int Degree>
116  void Polynomial<Degree>::setZero(void){memset(coefficients,0,sizeof(double)*(Degree+1));}
117 
118  template<int Degree>
120  for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i]*s;}
121  return *this;
122  }
123  template<int Degree>
125  for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i];}
126  return *this;
127  }
128  template<int Degree>
130  for(int i=0;i<=Degree;i++){coefficients[i]-=p.coefficients[i];}
131  return *this;
132  }
133  template<int Degree>
135  Polynomial q;
136  for(int i=0;i<=Degree;i++){q.coefficients[i]=(coefficients[i]+p.coefficients[i]);}
137  return q;
138  }
139  template<int Degree>
141  Polynomial q;
142  for(int i=0;i<=Degree;i++) {q.coefficients[i]=coefficients[i]-p.coefficients[i];}
143  return q;
144  }
145  template<int Degree>
146  void Polynomial<Degree>::Scale(const Polynomial& p,double w,Polynomial& q){
147  for(int i=0;i<=Degree;i++){q.coefficients[i]=p.coefficients[i]*w;}
148  }
149  template<int Degree>
150  void Polynomial<Degree>::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,double w2,Polynomial& q){
151  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i]*w2;}
152  }
153  template<int Degree>
154  void Polynomial<Degree>::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,Polynomial& q){
155  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i];}
156  }
157  template<int Degree>
158  void Polynomial<Degree>::AddScaled(const Polynomial& p1,const Polynomial& p2,double w2,Polynomial& q){
159  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]+p2.coefficients[i]*w2;}
160  }
161 
162  template<int Degree>
164  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]-p2.coefficients[i];}
165  }
166  template<int Degree>
168  out=in;
169  for(int i=0;i<=Degree;i++){out.coefficients[i]=-out.coefficients[i];}
170  }
171 
172  template<int Degree>
174  Polynomial q=*this;
175  for(int i=0;i<=Degree;i++){q.coefficients[i]=-q.coefficients[i];}
176  return q;
177  }
178  template<int Degree>
179  template<int Degree2>
182  for(int i=0;i<=Degree;i++){for(int j=0;j<=Degree2;j++){q.coefficients[i+j]+=coefficients[i]*p.coefficients[j];}}
183  return q;
184  }
185 
186  template<int Degree>
188  {
189  coefficients[0]+=s;
190  return *this;
191  }
192  template<int Degree>
194  {
195  coefficients[0]-=s;
196  return *this;
197  }
198  template<int Degree>
200  {
201  for(int i=0;i<=Degree;i++){coefficients[i]*=s;}
202  return *this;
203  }
204  template<int Degree>
206  {
207  for(int i=0;i<=Degree;i++){coefficients[i]/=s;}
208  return *this;
209  }
210  template<int Degree>
212  {
213  Polynomial<Degree> q=*this;
214  q.coefficients[0]+=s;
215  return q;
216  }
217  template<int Degree>
219  {
220  Polynomial q=*this;
221  q.coefficients[0]-=s;
222  return q;
223  }
224  template<int Degree>
226  {
227  Polynomial q;
228  for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]*s;}
229  return q;
230  }
231  template<int Degree>
233  {
234  Polynomial q;
235  for( int i=0 ; i<=Degree ; i++ ) q.coefficients[i] = coefficients[i]/s;
236  return q;
237  }
238  template<int Degree>
240  {
241  Polynomial q=*this;
242  double s2=1.0;
243  for(int i=0;i<=Degree;i++){
244  q.coefficients[i]*=s2;
245  s2/=s;
246  }
247  return q;
248  }
249  template<int Degree>
251  {
253  for(int i=0;i<=Degree;i++){
254  double temp=1;
255  for(int j=i;j>=0;j--){
256  q.coefficients[j]+=coefficients[i]*temp;
257  temp*=-t*j;
258  temp/=(i-j+1);
259  }
260  }
261  return q;
262  }
263  template<int Degree>
264  void Polynomial<Degree>::printnl(void) const{
265  for(int j=0;j<=Degree;j++){
266  printf("%6.4f x^%d ",coefficients[j],j);
267  if(j<Degree && coefficients[j+1]>=0){printf("+");}
268  }
269  printf("\n");
270  }
271  template<int Degree>
272  void Polynomial<Degree>::getSolutions(double c,std::vector<double>& roots,double EPS) const
273  {
274  double r[4][2];
275  int rCount=0;
276  roots.clear();
277  switch(Degree){
278  case 1:
279  rCount=Factor(coefficients[1],coefficients[0]-c,r,EPS);
280  break;
281  case 2:
282  rCount=Factor(coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
283  break;
284  case 3:
285  rCount=Factor(coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
286  break;
287  // case 4:
288  // rCount=Factor(coefficients[4],coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
289  // break;
290  default:
291  printf("Can't solve polynomial of degree: %d\n",Degree);
292  }
293  for(int i=0;i<rCount;i++){
294  if(fabs(r[i][1])<=EPS){
295  roots.push_back(r[i][0]);
296  }
297  }
298  }
299  template< >
301  {
302  Polynomial p;
303  p.coefficients[0] = 1.;
304  return p;
305  }
306  template< int Degree >
308  {
309  Polynomial p;
310  if( i>0 )
311  {
313  p -= _p;
314  p.coefficients[0] += _p(1);
315  }
316  if( i<Degree )
317  {
319  p += _p;
320  }
321  return p;
322  }
323 
324  }
325 }
PCL_EXPORTS int Factor(double a1, double a0, double roots[1][2], double EPS)
Polynomial< Degree+Degree2 > operator*(const Polynomial< Degree2 > &p) const
Definition: polynomial.hpp:180
Polynomial & operator/=(double s)
Definition: polynomial.hpp:205
double coefficients[Degree+1]
Definition: polynomial.h:42
double operator()(double t) const
Definition: polynomial.hpp:81
double integral(double tMin, double tMax) const
Definition: polynomial.hpp:87
Polynomial & operator*=(double s)
Definition: polynomial.hpp:199
Polynomial operator/(double s) const
Definition: polynomial.hpp:232
void printnl(void) const
Definition: polynomial.hpp:264
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void getSolutions(double c, std::vector< double > &roots, double EPS) const
Definition: polynomial.hpp:272
Polynomial operator+(const Polynomial &p) const
Definition: polynomial.hpp:134
Polynomial & operator+=(const Polynomial &p)
Definition: polynomial.hpp:124
static void Negate(const Polynomial &in, Polynomial &out)
Definition: polynomial.hpp:167
static void Scale(const Polynomial &p, double w, Polynomial &q)
Definition: polynomial.hpp:146
int operator!=(const Polynomial &p) const
Definition: polynomial.hpp:106
Polynomial & operator=(const Polynomial< Degree2 > &p)
int isZero(void) const
Definition: polynomial.hpp:111
static void Subtract(const Polynomial &p1, const Polynomial &p2, Polynomial &q)
Definition: polynomial.hpp:163
int operator==(const Polynomial &p) const
Definition: polynomial.hpp:101
Polynomial scale(double s) const
Definition: polynomial.hpp:239
static void AddScaled(const Polynomial &p1, double w1, const Polynomial &p2, double w2, Polynomial &q)
Definition: polynomial.hpp:150
Polynomial< Degree+1 > integral(void) const
Definition: polynomial.hpp:71
Polynomial & addScaled(const Polynomial &p, double scale)
Definition: polynomial.hpp:119
Polynomial operator-(void) const
Definition: polynomial.hpp:173
Polynomial & operator-=(const Polynomial &p)
Definition: polynomial.hpp:129
Polynomial< Degree-1 > derivative(void) const
Definition: polynomial.hpp:64
Polynomial shift(double t) const
Definition: polynomial.hpp:250
static Polynomial BSplineComponent(int i)
Definition: polynomial.hpp:307