Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
polynomial.hpp
1/*
2Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without modification,
6are permitted provided that the following conditions are met:
7
8Redistributions of source code must retain the above copyright notice, this list of
9conditions and the following disclaimer. Redistributions in binary form must reproduce
10the above copyright notice, this list of conditions and the following disclaimer
11in the documentation and/or other materials provided with the distribution.
12
13Neither the name of the Johns Hopkins University nor the names of its contributors
14may be used to endorse or promote products derived from this software without specific
15prior written permission.
16
17THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26DAMAGE.
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
41namespace 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>
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>
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>
139 for(int i=0;i<=Degree;i++){q.coefficients[i]=(coefficients[i]+p.coefficients[i]);}
140 return q;
141 }
142 template<int Degree>
145 for(int i=0;i<=Degree;i++) {q.coefficients[i]=coefficients[i]-p.coefficients[i];}
146 return q;
147 }
148 template<int Degree>
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>
158 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i];}
159 }
160 template<int Degree>
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 {
231 for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]*s;}
232 return q;
233 }
234 template<int Degree>
236 {
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>
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}
Iterator class for point clouds with or without given indices.
Polynomial & operator=(const Polynomial< Degree2 > &p)
Polynomial & operator+=(const Polynomial &p)
static void Negate(const Polynomial &in, Polynomial &out)
Polynomial scale(double s) const
Polynomial< Degree+1 > integral(void) const
Polynomial shift(double t) const
void getSolutions(double c, std::vector< double > &roots, double EPS) const
double operator()(double t) const
int operator!=(const Polynomial &p) const
static void AddScaled(const Polynomial &p1, double w1, const Polynomial &p2, double w2, Polynomial &q)
static void Scale(const Polynomial &p, double w, Polynomial &q)
Polynomial operator-(void) const
Polynomial< Degree-1 > derivative(void) const
Polynomial & operator/=(double s)
void printnl(void) const
Polynomial & operator*=(double s)
Polynomial & operator-=(const Polynomial &p)
Polynomial operator/(double s) const
double coefficients[Degree+1]
Definition polynomial.h:42
int operator==(const Polynomial &p) const
static void Subtract(const Polynomial &p1, const Polynomial &p2, Polynomial &q)
static Polynomial BSplineComponent(int i)
Polynomial & addScaled(const Polynomial &p, double scale)
Polynomial operator+(const Polynomial &p) const
Polynomial< Degree+Degree2 > operator*(const Polynomial< Degree2 > &p) const
PCL_EXPORTS int Factor(double a1, double a0, double roots[1][2], double EPS)