00001
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005
00006
00007
00008
00009
00010
00011
00012 #include "vnl_real_npolynomial.h"
00013 #include <vcl_cassert.h>
00014 #include <vcl_iostream.h>
00015 #include <vcl_sstream.h>
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 vnl_real_npolynomial::vnl_real_npolynomial(const vnl_vector<double>& c, const vnl_matrix<unsigned int>& p)
00032 : coeffs_(c)
00033 , polyn_(p)
00034 , nvar_(p.cols())
00035 , nterms_(p.rows())
00036 , ideg_(p.max_value())
00037 {
00038 assert(c.size() == p.rows());
00039 simplify();
00040 }
00041
00042
00043
00044 void vnl_real_npolynomial::simplify()
00045 {
00046 for (unsigned int row1=0; row1<nterms_; ++row1)
00047 for (unsigned int row2=row1+1; row2<nterms_; ++row2) {
00048 unsigned int col=0;
00049 while (col<nvar_ && polyn_(row1,col) == polyn_(row2,col)) ++col;
00050 if (col < nvar_) continue;
00051 coeffs_(row1) += coeffs_(row2); coeffs_(row2) = 0;
00052 }
00053 while (nterms_ > 0 && coeffs_(nterms_-1)==0) --nterms_;
00054 for (unsigned int row=0; row<nterms_; ++row)
00055 if (coeffs_(row) == 0) {
00056 --nterms_;
00057 coeffs_(row) = coeffs_(nterms_);
00058 for (unsigned int i=0; i<nvar_; ++i)
00059 polyn_(row,i) = polyn_(nterms_,i);
00060 }
00061
00062 if (nterms_ < polyn_.rows())
00063 this->set(coeffs_.extract(nterms_), polyn_.extract(nterms_,nvar_));
00064 }
00065
00066 double vnl_real_npolynomial::eval(const vnl_matrix<double>& xn)
00067 {
00068 double s=0;
00069 for (unsigned int i=0; i<nterms_; i++){
00070 double t=coeffs_(i);
00071 for (unsigned int j=0; j<nvar_; j++)
00072 t*=xn(j,polyn_(i,j));
00073 s+=t;
00074 }
00075 return s;
00076 }
00077
00078 double vnl_real_npolynomial::eval(const vnl_vector<double>& x)
00079 {
00080 vnl_matrix<double> xn(nvar_,ideg_+1);
00081
00082 for (unsigned int j=0; j<nvar_; j++){
00083 xn(j,0)=1;
00084 for (unsigned int i=1; i<ideg_+1; i++)
00085 xn(j,i)=xn(j,i-1)*x(j);
00086 }
00087 return eval(xn);
00088 }
00089
00090 double vnl_real_npolynomial::deval(const vnl_vector<double>& x, unsigned int var)
00091 {
00092 return deriv(var).eval(x);
00093 }
00094
00095 vnl_vector<double> vnl_real_npolynomial::deval(const vnl_vector<double>& x)
00096 {
00097 vnl_vector<double> dx(nvar_);
00098
00099 for (unsigned int j=0; j<nvar_; j++) {
00100 dx(j) = deriv(j).eval(x);
00101 }
00102 return dx;
00103 }
00104
00105 vnl_real_npolynomial vnl_real_npolynomial::deriv(unsigned int unk)
00106 {
00107 vnl_vector<double> c(nterms_);
00108 vnl_matrix<unsigned int> p = polyn_;
00109
00110 for (unsigned int i = 0; i < nterms_; i++) {
00111 if (polyn_(i,unk) > 0) {
00112 p(i,unk) = p(i,unk) - 1;
00113 c(i) = coeffs_(i)*polyn_(i,unk);
00114 }
00115 else {
00116 c(i) = coeffs_(i);
00117 }
00118 }
00119
00120 return vnl_real_npolynomial(c,p);
00121 }
00122
00123 void vnl_real_npolynomial::set(const vnl_vector<double>& c, const vnl_matrix<unsigned int>& p)
00124 {
00125 coeffs_= c;
00126 polyn_ = p;
00127 nvar_ = p.cols();
00128 nterms_ = p.rows();
00129 ideg_ = p.max_value();
00130 }
00131
00132 unsigned int vnl_real_npolynomial::degree() const
00133 {
00134 unsigned int d=0;
00135 for (unsigned int i=0; i<nterms_; i++)
00136 {
00137 unsigned int dt=0;
00138 for (unsigned int j=0; j<nvar_; j++)
00139 dt+=polyn_(i,j);
00140 if (dt>d) d=dt;
00141 }
00142 return d;
00143 }
00144
00145 vcl_vector<unsigned int> vnl_real_npolynomial::degrees() const
00146 {
00147 vcl_vector<unsigned int> d(nvar_);
00148 for (unsigned int j=0; j<nvar_; ++j)
00149 {
00150 d[j]=0;
00151 for (unsigned int i=0; i<nterms_; ++i)
00152 if (polyn_(i,j)>d[j]) d[j]=polyn_(i,j);
00153 }
00154 return d;
00155 }
00156
00157 vnl_real_npolynomial vnl_real_npolynomial::operator-() const
00158 {
00159 vnl_vector<double> coef(nterms_);
00160 for (unsigned int i=0; i<nterms_; ++i) coef(i) = - coeffs_(i);
00161
00162 vnl_matrix<unsigned int> poly = polyn_;
00163
00164 return vnl_real_npolynomial(coef, poly);
00165 }
00166
00167 vnl_real_npolynomial vnl_real_npolynomial::operator+(vnl_real_npolynomial const& P) const
00168 {
00169 assert(nvar_ == P.nvar_);
00170
00171 vnl_vector<double> coef(nterms_+P.nterms_);
00172 unsigned int i = 0; for (; i<nterms_; ++i) coef(i) = coeffs_(i);
00173 for (unsigned int j=0; j<P.nterms_; ++i,++j) coef(i) = P.coeffs_(j);
00174
00175 vnl_matrix<unsigned int> poly(nterms_+P.nterms_,nvar_);
00176 for (i=0; i<nterms_; ++i)
00177 for (unsigned int k=0; k<nvar_; ++k)
00178 poly(i,k) = polyn_(i,k);
00179 for (unsigned int j=0; j<P.nterms_; ++i,++j)
00180 for (unsigned int k=0; k<nvar_; ++k)
00181 poly(i,k) = P.polyn_(j,k);
00182
00183 return vnl_real_npolynomial(coef, poly);
00184 }
00185
00186 vnl_real_npolynomial vnl_real_npolynomial::operator+(double P) const
00187 {
00188 vnl_vector<double> coef(nterms_+1);
00189 for (unsigned int i=0; i<nterms_; ++i)
00190 coef(i) = coeffs_(i);
00191 coef(nterms_) = P;
00192
00193 vnl_matrix<unsigned int> poly(nterms_+1,nvar_);
00194 for (unsigned int i=0; i<nterms_; ++i)
00195 for (unsigned int k=0; k<nvar_; ++k)
00196 poly(i,k) = polyn_(i,k);
00197 for (unsigned int k=0; k<nvar_; ++k)
00198 poly(nterms_,k) = 0;
00199
00200 return vnl_real_npolynomial(coef, poly);
00201 }
00202
00203 vnl_real_npolynomial vnl_real_npolynomial::operator-(vnl_real_npolynomial const& P) const
00204 {
00205 assert(nvar_ == P.nvar_);
00206
00207 vnl_vector<double> coef(nterms_+P.nterms_);
00208 unsigned int i = 0; for (; i<nterms_; ++i) coef(i) = coeffs_(i);
00209 for (unsigned int j=0; j<P.nterms_; ++i,++j) coef(i) = - P.coeffs_(j);
00210
00211 vnl_matrix<unsigned int> poly(nterms_+P.nterms_,nvar_);
00212 for (i=0; i<nterms_; ++i)
00213 for (unsigned int k=0; k<nvar_; ++k)
00214 poly(i,k) = polyn_(i,k);
00215 for (unsigned int j=0; j<P.nterms_; ++i,++j)
00216 for (unsigned int k=0; k<nvar_; ++k)
00217 poly(i,k) = P.polyn_(j,k);
00218
00219 return vnl_real_npolynomial(coef, poly);
00220 }
00221
00222 vnl_real_npolynomial vnl_real_npolynomial::operator*(vnl_real_npolynomial const& P) const
00223 {
00224 assert(nvar_ == P.nvar_);
00225
00226 vnl_vector<double> coef(nterms_*P.nterms_);
00227 unsigned int k = 0;
00228 for (unsigned int i=0; i<nterms_; ++i)
00229 for (unsigned int j=0; j<P.nterms_; ++j,++k)
00230 coef(k) = coeffs_(i) * P.coeffs_(j);
00231
00232 vnl_matrix<unsigned int> poly(nterms_*P.nterms_,nvar_);
00233 k = 0;
00234 for (unsigned int i=0; i<nterms_; ++i)
00235 for (unsigned int j=0; j<P.nterms_; ++j,++k)
00236 for (unsigned int l=0; l<nvar_; ++l)
00237 poly(k,l) = polyn_(i,l) + P.polyn_(j,l);
00238
00239 return vnl_real_npolynomial(coef, poly);
00240 }
00241
00242 vnl_real_npolynomial vnl_real_npolynomial::operator*(double P) const
00243 {
00244 vnl_vector<double> coef(nterms_);
00245 for (unsigned int i=0; i<nterms_; ++i)
00246 coef(i) = coeffs_(i) * P;
00247
00248 vnl_matrix<unsigned int> poly = polyn_;
00249
00250 return vnl_real_npolynomial(coef, poly);
00251 }
00252
00253 vcl_ostream& operator<<(vcl_ostream& os, vnl_real_npolynomial const& P)
00254 {
00255 return os << P.asString() << vcl_endl;
00256 }
00257 vnl_real_npolynomial& vnl_real_npolynomial::operator+=(vnl_real_npolynomial const& rhs){
00258 *this = (*this) + rhs;
00259 return *this;
00260 }
00261 vnl_real_npolynomial& vnl_real_npolynomial::operator-=(vnl_real_npolynomial const& rhs){
00262 *this = (*this) - rhs;
00263 return *this;
00264 }
00265 vnl_real_npolynomial& vnl_real_npolynomial::operator*=(vnl_real_npolynomial const& rhs){
00266 *this = (*this) * rhs;
00267 return *this;
00268 }
00269 vcl_string vnl_real_npolynomial::asString() const
00270 {
00271 vcl_ostringstream os;
00272 if (nvar_ <= 3)
00273 for (unsigned int i=0; i<nterms_; ++i)
00274 {
00275 if (i>0) os << ' ';
00276 if (i>0 && coeffs_(i) >= 0) os << "+ ";
00277 double abs_coef = coeffs_(i);
00278 if (coeffs_(i) < 0) { abs_coef = -abs_coef; os << "- "; }
00279 if (abs_coef != 1) os << abs_coef << ' ';
00280 unsigned int totaldeg = 0;
00281 if (nvar_ > 0 && polyn_(i,0) > 0) { os << 'X'; totaldeg += polyn_(i,0); }
00282 if (nvar_ > 0 && polyn_(i,0) > 1) os << '^' << polyn_(i,0) << ' ';
00283 if (nvar_ > 1 && polyn_(i,1) > 0) { os << 'Y'; totaldeg += polyn_(i,1); }
00284 if (nvar_ > 1 && polyn_(i,1) > 1) os << '^' << polyn_(i,1) << ' ';
00285 if (nvar_ > 2 && polyn_(i,2) > 0) { os << 'Z'; totaldeg += polyn_(i,2); }
00286 if (nvar_ > 2 && polyn_(i,2) > 1) os << '^' << polyn_(i,2) << ' ';
00287 if (totaldeg == 0 && abs_coef == 1) os << abs_coef;
00288 }
00289 else
00290 for (unsigned int i=0; i<nterms_; ++i)
00291 {
00292 if (i>0) os << ' ';
00293 if (i>0 && coeffs_(i) >= 0) os << "+ ";
00294 double abs_coef = coeffs_(i);
00295 if (coeffs_(i) < 0) { abs_coef = -abs_coef; os << "- "; }
00296 if (abs_coef != 1) os << abs_coef << ' ';
00297 unsigned int totaldeg = 0;
00298 for (unsigned int j=0; j<nvar_; ++j) {
00299 if (polyn_(i,j) > 0) os << 'X' << j;
00300 if (polyn_(i,j) > 1) os << '^' << polyn_(i,j) << ' ';
00301 totaldeg += polyn_(i,j);
00302 }
00303 if (totaldeg == 0 && abs_coef == 1) os << abs_coef;
00304 }
00305 return os.str();
00306 }