polynomial.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho,
00007  *                      Johns Hopkins University
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of Willow Garage, Inc. nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  * $Id: polynomial.hpp 5389 2012-03-28 20:33:17Z jspricke $
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     // Polynomial //
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           //    case 4:
00289           //            rCount=Factor(coefficients[4],coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
00290           //            break;
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           //printf("%d] %f\t%f\n",i,r[i][0],(*this)(r[i][0])-c);
00298         }
00299       }
00300     }
00301 
00302 
00303   }
00304 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:21