Point Cloud Library (PCL)  1.14.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 "factor.h"
30 
31 #include <float.h>
32 #include <math.h>
33 
34 #include <cstdio>
35 #include <cstring>
36 
37 ////////////////
38 // Polynomial //
39 ////////////////
40 
41 namespace pcl
42 {
43  namespace poisson
44  {
45 
46 
47  template<int Degree>
48  Polynomial<Degree>::Polynomial(void){memset(coefficients,0,sizeof(double)*(Degree+1));}
49  template<int Degree>
50  template<int Degree2>
52  memset(coefficients,0,sizeof(double)*(Degree+1));
53  for(int i=0;i<=Degree && i<=Degree2;i++){coefficients[i]=P.coefficients[i];}
54  }
55 
56 
57  template<int Degree>
58  template<int Degree2>
60  int d=Degree<Degree2?Degree:Degree2;
61  memset(coefficients,0,sizeof(double)*(Degree+1));
62  memcpy(coefficients,p.coefficients,sizeof(double)*(d+1));
63  return *this;
64  }
65 
66  template<int Degree>
68  Polynomial<Degree-1> p;
69  for(int i=0;i<Degree;i++){p.coefficients[i]=coefficients[i+1]*(i+1);}
70  return p;
71  }
72 
73  template<int Degree>
76  p.coefficients[0]=0;
77  for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);}
78  return p;
79  }
80  template<> inline double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; }
81  template<> inline double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; }
82  template<> inline double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; }
83  template<int Degree>
84  double Polynomial<Degree>::operator() ( double t ) const{
85  double v=coefficients[Degree];
86  for( int d=Degree-1 ; d>=0 ; d-- ) v = v*t + coefficients[d];
87  return v;
88  }
89  template<int Degree>
90  double Polynomial<Degree>::integral( double tMin , double tMax ) const
91  {
92  double v=0;
93  double t1,t2;
94  t1=tMin;
95  t2=tMax;
96  for(int i=0;i<=Degree;i++){
97  v+=coefficients[i]*(t2-t1)/(i+1);
98  if(t1!=-DBL_MAX && t1!=DBL_MAX){t1*=tMin;}
99  if(t2!=-DBL_MAX && t2!=DBL_MAX){t2*=tMax;}
100  }
101  return v;
102  }
103  template<int Degree>
105  for(int i=0;i<=Degree;i++){if(coefficients[i]!=p.coefficients[i]){return 0;}}
106  return 1;
107  }
108  template<int Degree>
110  for(int i=0;i<=Degree;i++){if(coefficients[i]==p.coefficients[i]){return 0;}}
111  return 1;
112  }
113  template<int Degree>
114  int Polynomial<Degree>::isZero(void) const{
115  for(int i=0;i<=Degree;i++){if(coefficients[i]!=0){return 0;}}
116  return 1;
117  }
118  template<int Degree>
119  void Polynomial<Degree>::setZero(void){memset(coefficients,0,sizeof(double)*(Degree+1));}
120 
121  template<int Degree>
123  for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i]*s;}
124  return *this;
125  }
126  template<int Degree>
128  for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i];}
129  return *this;
130  }
131  template<int Degree>
133  for(int i=0;i<=Degree;i++){coefficients[i]-=p.coefficients[i];}
134  return *this;
135  }
136  template<int Degree>
138  Polynomial q;
139  for(int i=0;i<=Degree;i++){q.coefficients[i]=(coefficients[i]+p.coefficients[i]);}
140  return q;
141  }
142  template<int Degree>
144  Polynomial q;
145  for(int i=0;i<=Degree;i++) {q.coefficients[i]=coefficients[i]-p.coefficients[i];}
146  return q;
147  }
148  template<int Degree>
149  void Polynomial<Degree>::Scale(const Polynomial& p,double w,Polynomial& q){
150  for(int i=0;i<=Degree;i++){q.coefficients[i]=p.coefficients[i]*w;}
151  }
152  template<int Degree>
153  void Polynomial<Degree>::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,double w2,Polynomial& q){
154  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i]*w2;}
155  }
156  template<int Degree>
157  void Polynomial<Degree>::AddScaled(const Polynomial& p1,double w1,const Polynomial& p2,Polynomial& q){
158  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i];}
159  }
160  template<int Degree>
161  void Polynomial<Degree>::AddScaled(const Polynomial& p1,const Polynomial& p2,double w2,Polynomial& q){
162  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]+p2.coefficients[i]*w2;}
163  }
164 
165  template<int Degree>
167  for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]-p2.coefficients[i];}
168  }
169  template<int Degree>
171  out=in;
172  for(int i=0;i<=Degree;i++){out.coefficients[i]=-out.coefficients[i];}
173  }
174 
175  template<int Degree>
177  Polynomial q=*this;
178  for(int i=0;i<=Degree;i++){q.coefficients[i]=-q.coefficients[i];}
179  return q;
180  }
181  template<int Degree>
182  template<int Degree2>
185  for(int i=0;i<=Degree;i++){for(int j=0;j<=Degree2;j++){q.coefficients[i+j]+=coefficients[i]*p.coefficients[j];}}
186  return q;
187  }
188 
189  template<int Degree>
191  {
192  coefficients[0]+=s;
193  return *this;
194  }
195  template<int Degree>
197  {
198  coefficients[0]-=s;
199  return *this;
200  }
201  template<int Degree>
203  {
204  for(int i=0;i<=Degree;i++){coefficients[i]*=s;}
205  return *this;
206  }
207  template<int Degree>
209  {
210  for(int i=0;i<=Degree;i++){coefficients[i]/=s;}
211  return *this;
212  }
213  template<int Degree>
215  {
216  Polynomial<Degree> q=*this;
217  q.coefficients[0]+=s;
218  return q;
219  }
220  template<int Degree>
222  {
223  Polynomial q=*this;
224  q.coefficients[0]-=s;
225  return q;
226  }
227  template<int Degree>
229  {
230  Polynomial q;
231  for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]*s;}
232  return q;
233  }
234  template<int Degree>
236  {
237  Polynomial q;
238  for( int i=0 ; i<=Degree ; i++ ) q.coefficients[i] = coefficients[i]/s;
239  return q;
240  }
241  template<int Degree>
243  {
244  Polynomial q=*this;
245  double s2=1.0;
246  for(int i=0;i<=Degree;i++){
247  q.coefficients[i]*=s2;
248  s2/=s;
249  }
250  return q;
251  }
252  template<int Degree>
254  {
256  for(int i=0;i<=Degree;i++){
257  double temp=1;
258  for(int j=i;j>=0;j--){
259  q.coefficients[j]+=coefficients[i]*temp;
260  temp*=-t*j;
261  temp/=(i-j+1);
262  }
263  }
264  return q;
265  }
266  template<int Degree>
267  void Polynomial<Degree>::printnl(void) const{
268  for(int j=0;j<=Degree;j++){
269  printf("%6.4f x^%d ",coefficients[j],j);
270  if(j<Degree && coefficients[j+1]>=0){printf("+");}
271  }
272  printf("\n");
273  }
274  template<int Degree>
275  void Polynomial<Degree>::getSolutions(double c,std::vector<double>& roots,double EPS) const
276  {
277  double r[4][2];
278  int rCount=0;
279  roots.clear();
280  switch(Degree){
281  case 1:
282  rCount=Factor(coefficients[1],coefficients[0]-c,r,EPS);
283  break;
284  case 2:
285  rCount=Factor(coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
286  break;
287  case 3:
288  rCount=Factor(coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
289  break;
290  // case 4:
291  // rCount=Factor(coefficients[4],coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
292  // break;
293  default:
294  printf("Can't solve polynomial of degree: %d\n",Degree);
295  }
296  for(int i=0;i<rCount;i++){
297  if(fabs(r[i][1])<=EPS){
298  roots.push_back(r[i][0]);
299  }
300  }
301  }
302  template< > inline
304  {
305  Polynomial p;
306  p.coefficients[0] = 1.;
307  return p;
308  }
309  template< int Degree > inline
311  {
312  Polynomial p;
313  if( i>0 )
314  {
316  p -= _p;
317  p.coefficients[0] += _p(1);
318  }
319  if( i<Degree )
320  {
322  p += _p;
323  }
324  return p;
325  }
326 
327  }
328 }
Polynomial & operator+=(const Polynomial &p)
Definition: polynomial.hpp:127
static void Negate(const Polynomial &in, Polynomial &out)
Definition: polynomial.hpp:170
Polynomial scale(double s) const
Definition: polynomial.hpp:242
Polynomial< Degree+1 > integral(void) const
Definition: polynomial.hpp:74
Polynomial shift(double t) const
Definition: polynomial.hpp:253
void getSolutions(double c, std::vector< double > &roots, double EPS) const
Definition: polynomial.hpp:275
double operator()(double t) const
Definition: polynomial.hpp:84
int operator!=(const Polynomial &p) const
Definition: polynomial.hpp:109
static void AddScaled(const Polynomial &p1, double w1, const Polynomial &p2, double w2, Polynomial &q)
Definition: polynomial.hpp:153
int isZero(void) const
Definition: polynomial.hpp:114
static void Scale(const Polynomial &p, double w, Polynomial &q)
Definition: polynomial.hpp:149
Polynomial operator-(void) const
Definition: polynomial.hpp:176
Polynomial< Degree-1 > derivative(void) const
Definition: polynomial.hpp:67
Polynomial & operator/=(double s)
Definition: polynomial.hpp:208
Polynomial & operator=(const Polynomial< Degree2 > &p)
void printnl(void) const
Definition: polynomial.hpp:267
Polynomial & operator*=(double s)
Definition: polynomial.hpp:202
Polynomial & operator-=(const Polynomial &p)
Definition: polynomial.hpp:132
Polynomial operator/(double s) const
Definition: polynomial.hpp:235
double coefficients[Degree+1]
Definition: polynomial.h:42
int operator==(const Polynomial &p) const
Definition: polynomial.hpp:104
static void Subtract(const Polynomial &p1, const Polynomial &p2, Polynomial &q)
Definition: polynomial.hpp:166
static Polynomial BSplineComponent(int i)
Definition: polynomial.hpp:310
Polynomial & addScaled(const Polynomial &p, double scale)
Definition: polynomial.hpp:122
double integral(double tMin, double tMax) const
Definition: polynomial.hpp:90
Polynomial operator+(const Polynomial &p) const
Definition: polynomial.hpp:137
Polynomial< Degree+Degree2 > operator*(const Polynomial< Degree2 > &p) const
Definition: polynomial.hpp:183
PCL_EXPORTS int Factor(double a1, double a0, double roots[1][2], double EPS)