00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 #include <float.h>
00043 #include <math.h>
00044 #include <algorithm>
00045
00046 #include <pcl/surface/poisson/factor.h>
00047
00048
00049 namespace pcl {
00050 namespace poisson {
00051
00052
00054
00056 template<int Degree>
00057 Polynomial<Degree>::Polynomial(void){memset(coefficients,0,sizeof(double)*(Degree+1));}
00058 template<int Degree>
00059 template<int Degree2>
00060 Polynomial<Degree>::Polynomial(const Polynomial<Degree2>& P){
00061 memset(coefficients,0,sizeof(double)*(Degree+1));
00062 for(int i=0;i<=Degree && i<=Degree2;i++){coefficients[i]=P.coefficients[i];}
00063 }
00064
00065
00066 template<int Degree>
00067 template<int Degree2>
00068 Polynomial<Degree>& Polynomial<Degree>::operator = (const Polynomial<Degree2> &p){
00069 int d=Degree<Degree2?Degree:Degree2;
00070 memset(coefficients,0,sizeof(double)*(Degree+1));
00071 memcpy(coefficients,p.coefficients,sizeof(double)*(d+1));
00072 return *this;
00073 }
00074
00075 template<int Degree>
00076 Polynomial<Degree-1> Polynomial<Degree>::derivative(void) const{
00077 Polynomial<Degree-1> p;
00078 for(int i=0;i<Degree;i++){p.coefficients[i]=coefficients[i+1]*(i+1);}
00079 return p;
00080 }
00081
00082 template<int Degree>
00083 Polynomial<Degree+1> Polynomial<Degree>::integral(void) const{
00084 Polynomial<Degree+1> p;
00085 p.coefficients[0]=0;
00086 for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);}
00087 return p;
00088 }
00089 template<int Degree>
00090 double Polynomial<Degree>::operator() (const double& t) const{
00091 double temp=1;
00092 double v=0;
00093 for(int i=0;i<=Degree;i++){
00094 v+=temp*coefficients[i];
00095 temp*=t;
00096 }
00097 return v;
00098 }
00099 template<int Degree>
00100 double Polynomial<Degree>::integral(const double& tMin,const double& tMax) const{
00101 double v=0;
00102 double t1,t2;
00103 t1=tMin;
00104 t2=tMax;
00105 for(int i=0;i<=Degree;i++){
00106 v+=coefficients[i]*(t2-t1)/(i+1);
00107 if(t1!=-DBL_MAX && t1!=DBL_MAX){t1*=tMin;}
00108 if(t2!=-DBL_MAX && t2!=DBL_MAX){t2*=tMax;}
00109 }
00110 return v;
00111 }
00112 template<int Degree>
00113 int Polynomial<Degree>::operator == (const Polynomial& p) const{
00114 for(int i=0;i<=Degree;i++){if(coefficients[i]!=p.coefficients[i]){return 0;}}
00115 return 1;
00116 }
00117 template<int Degree>
00118 int Polynomial<Degree>::operator != (const Polynomial& p) const{
00119 for(int i=0;i<=Degree;i++){if(coefficients[i]==p.coefficients[i]){return 0;}}
00120 return 1;
00121 }
00122 template<int Degree>
00123 int Polynomial<Degree>::isZero(void) const{
00124 for(int i=0;i<=Degree;i++){if(coefficients[i]!=0){return 0;}}
00125 return 1;
00126 }
00127 template<int Degree>
00128 void Polynomial<Degree>::setZero(void){memset(coefficients,0,sizeof(double)*(Degree+1));}
00129
00130 template<int Degree>
00131 Polynomial<Degree>& Polynomial<Degree>::addScaled(const Polynomial& p,const double& s){
00132 for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i]*s;}
00133 return *this;
00134 }
00135 template<int Degree>
00136 Polynomial<Degree>& Polynomial<Degree>::operator += (const Polynomial<Degree>& p){
00137 for(int i=0;i<=Degree;i++){coefficients[i]+=p.coefficients[i];}
00138 return *this;
00139 }
00140 template<int Degree>
00141 Polynomial<Degree>& Polynomial<Degree>::operator -= (const Polynomial<Degree>& p){
00142 for(int i=0;i<=Degree;i++){coefficients[i]-=p.coefficients[i];}
00143 return *this;
00144 }
00145 template<int Degree>
00146 Polynomial<Degree> Polynomial<Degree>::operator + (const Polynomial<Degree>& p) const{
00147 Polynomial q;
00148 for(int i=0;i<=Degree;i++){q.coefficients[i]=(coefficients[i]+p.coefficients[i]);}
00149 return q;
00150 }
00151 template<int Degree>
00152 Polynomial<Degree> Polynomial<Degree>::operator - (const Polynomial<Degree>& p) const{
00153 Polynomial q;
00154 for(int i=0;i<=Degree;i++) {q.coefficients[i]=coefficients[i]-p.coefficients[i];}
00155 return q;
00156 }
00157 template<int Degree>
00158 void Polynomial<Degree>::Scale(const Polynomial& p,const double& w,Polynomial& q){
00159 for(int i=0;i<=Degree;i++){q.coefficients[i]=p.coefficients[i]*w;}
00160 }
00161 template<int Degree>
00162 void Polynomial<Degree>::AddScaled(const Polynomial& p1,const double& w1,const Polynomial& p2,const double& w2,Polynomial& q){
00163 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i]*w2;}
00164 }
00165 template<int Degree>
00166 void Polynomial<Degree>::AddScaled(const Polynomial& p1,const double& w1,const Polynomial& p2,Polynomial& q){
00167 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]*w1+p2.coefficients[i];}
00168 }
00169 template<int Degree>
00170 void Polynomial<Degree>::AddScaled(const Polynomial& p1,const Polynomial& p2,const double& w2,Polynomial& q){
00171 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]+p2.coefficients[i]*w2;}
00172 }
00173
00174 template<int Degree>
00175 void Polynomial<Degree>::Subtract(const Polynomial &p1,const Polynomial& p2,Polynomial& q){
00176 for(int i=0;i<=Degree;i++){q.coefficients[i]=p1.coefficients[i]-p2.coefficients[i];}
00177 }
00178 template<int Degree>
00179 void Polynomial<Degree>::Negate(const Polynomial& in,Polynomial& out){
00180 out=in;
00181 for(int i=0;i<=Degree;i++){out.coefficients[i]=-out.coefficients[i];}
00182 }
00183
00184 template<int Degree>
00185 Polynomial<Degree> Polynomial<Degree>::operator - (void) const{
00186 Polynomial q=*this;
00187 for(int i=0;i<=Degree;i++){q.coefficients[i]=-q.coefficients[i];}
00188 return q;
00189 }
00190 template<int Degree>
00191 template<int Degree2>
00192 Polynomial<Degree+Degree2> Polynomial<Degree>::operator * (const Polynomial<Degree2>& p) const{
00193 Polynomial<Degree+Degree2> q;
00194 for(int i=0;i<=Degree;i++){for(int j=0;j<=Degree2;j++){q.coefficients[i+j]+=coefficients[i]*p.coefficients[j];}}
00195 return q;
00196 }
00197
00198 template<int Degree>
00199 Polynomial<Degree>& Polynomial<Degree>::operator += (const double& s){
00200 coefficients[0]+=s;
00201 return *this;
00202 }
00203 template<int Degree>
00204 Polynomial<Degree>& Polynomial<Degree>::operator -= (const double& s){
00205 coefficients[0]-=s;
00206 return *this;
00207 }
00208 template<int Degree>
00209 Polynomial<Degree>& Polynomial<Degree>::operator *= (const double& s){
00210 for(int i=0;i<=Degree;i++){coefficients[i]*=s;}
00211 return *this;
00212 }
00213 template<int Degree>
00214 Polynomial<Degree>& Polynomial<Degree>::operator /= (const double& s){
00215 for(int i=0;i<=Degree;i++){coefficients[i]/=s;}
00216 return *this;
00217 }
00218 template<int Degree>
00219 Polynomial<Degree> Polynomial<Degree>::operator + (const double& s) const{
00220 Polynomial<Degree> q=*this;
00221 q.coefficients[0]+=s;
00222 return q;
00223 }
00224 template<int Degree>
00225 Polynomial<Degree> Polynomial<Degree>::operator - (const double& s) const{
00226 Polynomial q=*this;
00227 q.coefficients[0]-=s;
00228 return q;
00229 }
00230 template<int Degree>
00231 Polynomial<Degree> Polynomial<Degree>::operator * (const double& s) const{
00232 Polynomial q;
00233 for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]*s;}
00234 return q;
00235 }
00236 template<int Degree>
00237 Polynomial<Degree> Polynomial<Degree>::operator / (const double& s) const{
00238 Polynomial q(this->degree());
00239 for(int i=0;i<=Degree;i++){q.coefficients[i]=coefficients[i]/s;}
00240 return q;
00241 }
00242 template<int Degree>
00243 Polynomial<Degree> Polynomial<Degree>::scale(const double& s) const{
00244 Polynomial q=*this;
00245 double s2=1.0;
00246 for(int i=0;i<=Degree;i++){
00247 q.coefficients[i]*=s2;
00248 s2/=s;
00249 }
00250 return q;
00251 }
00252 template<int Degree>
00253 Polynomial<Degree> Polynomial<Degree>::shift(const double& t) const{
00254 Polynomial<Degree> q;
00255 for(int i=0;i<=Degree;i++){
00256 double temp=1;
00257 for(int j=i;j>=0;j--){
00258 q.coefficients[j]+=coefficients[i]*temp;
00259 temp*=-t*j;
00260 temp/=(i-j+1);
00261 }
00262 }
00263 return q;
00264 }
00265 template<int Degree>
00266 void Polynomial<Degree>::printnl(void) const{
00267 for(int j=0;j<=Degree;j++){
00268 printf("%6.4f x^%d ",coefficients[j],j);
00269 if(j<Degree && coefficients[j+1]>=0){printf("+");}
00270 }
00271 printf("\n");
00272 }
00273 template<int Degree>
00274 void Polynomial<Degree>::getSolutions(const double& c,std::vector<double>& roots,const double& EPS) const {
00275 double r[4][2];
00276 int rCount=0;
00277 roots.clear();
00278 switch(Degree){
00279 case 1:
00280 rCount=Factor(coefficients[1],coefficients[0]-c,r,EPS);
00281 break;
00282 case 2:
00283 rCount=Factor(coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
00284 break;
00285 case 3:
00286 rCount=Factor(coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
00287 break;
00288
00289
00290
00291 default:
00292 printf("Can't solve polynomial of degree: %d\n",Degree);
00293 }
00294 for(int i=0;i<rCount;i++){
00295 if(fabs(r[i][1])<=EPS){
00296 roots.push_back(r[i][0]);
00297
00298 }
00299 }
00300 }
00301
00302
00303 }
00304 }