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//////////////////////// 32// StartingPolynomial // 33//////////////////////// 34template<int Degree> 35template<int Degree2> 36StartingPolynomial<Degree+Degree2> StartingPolynomial<Degree>::operator * (const StartingPolynomial<Degree2>& p) const{ 37 StartingPolynomial<Degree+Degree2> sp; 38 if(start>p.start){sp.start=start;} 39 else{sp.start=p.start;} 40 sp.p=this->p*p.p; 41 return sp; 42} 43template<int Degree> 44StartingPolynomial<Degree> StartingPolynomial<Degree>::scale( double s ) const 45{ 46 StartingPolynomial q; 47 q.start = start*s; 48 q.p = p.scale(s); 49 return q; 50} 51template<int Degree> 52StartingPolynomial<Degree> StartingPolynomial<Degree>::shift(double s) const{ 53 StartingPolynomial q; 54 q.start=start+s; 55 q.p=p.shift(s); 56 return q; 57} 58 59 60template<int Degree> 61int StartingPolynomial<Degree>::operator < (const StartingPolynomial<Degree>& sp) const{ 62 if(start<sp.start){return 1;} 63 else{return 0;} 64} 65template<int Degree> 66int StartingPolynomial<Degree>::Compare(const void* v1,const void* v2){ 67 double d=((const StartingPolynomial*)(v1))->start-((const StartingPolynomial*)(v2))->start; 68 if ( d<0 ) return -1; 69 else if( d>0 ) return 1; 70 else return 0; 71} 72 73///////////////// 74// PPolynomial // 75///////////////// 76template< int Degree > 77PPolynomial< Degree >::PPolynomial( void ) 78{ 79 polyCount = 0; 80 polys = NullPointer( StartingPolynomial< Degree > ); 81} 82template< int Degree > 83PPolynomial<Degree>::PPolynomial( const PPolynomial<Degree>& p ) 84{ 85 polyCount = 0; 86 polys = NullPointer( StartingPolynomial< Degree > ); 87 set( p.polyCount ); 88 memcpy( polys , p.polys , sizeof( StartingPolynomial<Degree> ) * p.polyCount ); 89} 90 91template< int Degree > 92PPolynomial< Degree >::~PPolynomial( void ) 93{ 94 FreePointer( polys ); 95 polyCount = 0; 96} 97template< int Degree > 98void PPolynomial< Degree >::set( Pointer( StartingPolynomial< Degree > ) sps , int count ) 99{ 100 int c=0; 101 set( count ); 102 qsort( sps , count , sizeof( StartingPolynomial< Degree > ) , StartingPolynomial< Degree >::Compare ); 103 for( int i=0 ; i<count ; i++ ) 104 { 105 if( !c || sps[i].start!=polys[c-1].start ) polys[c++] = sps[i]; 106 else{polys[c-1].p+=sps[i].p;} 107 } 108 reset( c ); 109} 110template< int Degree > int PPolynomial< Degree >::size( void ) const{ return int(sizeof(StartingPolynomial<Degree>)*polyCount); } 111 112template< int Degree > 113void PPolynomial<Degree>::set( size_t size ) 114{ 115 FreePointer( polys ); 116 polyCount = size; 117 if( size ) 118 polys = AllocPointer< StartingPolynomial< Degree > >( size ); 119} 120template< int Degree > 121void PPolynomial<Degree>::reset( size_t newSize ) 122{ 123 polyCount = newSize; 124 polys = ReAllocPointer< StartingPolynomial< Degree > >( polys , newSize ); 125} 126template< int Degree > 127PPolynomial< Degree >& PPolynomial< Degree >::compress( double delta ) 128{ 129 int _polyCount = (int)polyCount; 130 Pointer( StartingPolynomial< Degree > ) _polys = polys; 131 132 polyCount = 1 , polys = NullPointer( StartingPolynomial< Degree > ); 133 for( int i=1 ; i<_polyCount ; i++ ) if( _polys[i].start-_polys[i-1].start>delta ) polyCount++; 134 if( polyCount==_polyCount ) polys = _polys; 135 else 136 { 137 polys = AllocPointer< StartingPolynomial< Degree > >( polyCount ); 138 polys[0] = _polys[0] , polyCount = 0; 139 for( int i=1 ; i<_polyCount ; i++ ) 140 { 141 if( _polys[i].start-_polys[i-1].start>delta ) polys[ ++polyCount ] = _polys[i]; 142 else polys[ polyCount ].p += _polys[i].p; 143 } 144 polyCount++; 145 FreePointer( _polys ); 146 } 147 return *this; 148} 149 150template< int Degree > 151PPolynomial<Degree>& PPolynomial<Degree>::operator = (const PPolynomial<Degree>& p){ 152 set(p.polyCount); 153 memcpy(polys,p.polys,sizeof(StartingPolynomial<Degree>)*p.polyCount); 154 return *this; 155} 156 157template<int Degree> 158template<int Degree2> 159PPolynomial<Degree>& PPolynomial<Degree>::operator = (const PPolynomial<Degree2>& p){ 160 set(p.polyCount); 161 for(int i=0;i<int(polyCount);i++){ 162 polys[i].start=p.polys[i].start; 163 polys[i].p=p.polys[i].p; 164 } 165 return *this; 166} 167 168template<int Degree> 169double PPolynomial<Degree>::operator ()( double t ) const 170{ 171 double v=0; 172 for( int i=0 ; i<int(polyCount) && t>polys[i].start ; i++ ) v += polys[i].p(t); 173 return v; 174} 175 176template<int Degree> 177double PPolynomial<Degree>::integral( double tMin , double tMax ) const 178{ 179 int m=1; 180 double start,end,s,v=0; 181 start=tMin; 182 end=tMax; 183 if(tMin>tMax){ 184 m=-1; 185 start=tMax; 186 end=tMin; 187 } 188 for(int i=0;i<int(polyCount) && polys[i].start<end;i++){ 189 if(start<polys[i].start){s=polys[i].start;} 190 else{s=start;} 191 v+=polys[i].p.integral(s,end); 192 } 193 return v*m; 194} 195template<int Degree> 196double PPolynomial<Degree>::Integral(void) const{return integral(polys[0].start,polys[polyCount-1].start);} 197template<int Degree> 198PPolynomial<Degree> PPolynomial<Degree>::operator + (const PPolynomial<Degree>& p) const{ 199 PPolynomial q; 200 int i,j; 201 size_t idx=0; 202 q.set(polyCount+p.polyCount); 203 i=j=-1; 204 205 while(idx<q.polyCount){ 206 if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} 207 else if (i>=int( polyCount)-1) {q.polys[idx]=p.polys[++j];} 208 else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];} 209 else {q.polys[idx]=p.polys[++j];} 210 idx++; 211 } 212 return q; 213} 214template<int Degree> 215PPolynomial<Degree> PPolynomial<Degree>::operator - (const PPolynomial<Degree>& p) const{ 216 PPolynomial q; 217 int i,j; 218 size_t idx=0; 219 q.set(polyCount+p.polyCount); 220 i=j=-1; 221 222 while(idx<q.polyCount){ 223 if (j>=int(p.polyCount)-1) {q.polys[idx]= polys[++i];} 224 else if (i>=int( polyCount)-1) {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);} 225 else if(polys[i+1].start<p.polys[j+1].start){q.polys[idx]= polys[++i];} 226 else {q.polys[idx].start=p.polys[++j].start;q.polys[idx].p=p.polys[j].p*(-1.0);} 227 idx++; 228 } 229 return q; 230} 231template<int Degree> 232PPolynomial<Degree>& PPolynomial<Degree>::addScaled(const PPolynomial<Degree>& p,double scale){ 233 int i,j; 234 StartingPolynomial<Degree>* oldPolys=polys; 235 size_t idx=0,cnt=0,oldPolyCount=polyCount; 236 polyCount=0; 237 polys=NULL; 238 set(oldPolyCount+p.polyCount); 239 i=j=-1; 240 while(cnt<polyCount){ 241 if (j>=int( p.polyCount)-1) {polys[idx]=oldPolys[++i];} 242 else if (i>=int(oldPolyCount)-1) {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;} 243 else if (oldPolys[i+1].start<p.polys[j+1].start){polys[idx]=oldPolys[++i];} 244 else {polys[idx].start= p.polys[++j].start;polys[idx].p=p.polys[j].p*scale;} 245 if(idx && polys[idx].start==polys[idx-1].start) {polys[idx-1].p+=polys[idx].p;} 246 else{idx++;} 247 cnt++; 248 } 249 free(oldPolys); 250 reset(idx); 251 return *this; 252} 253template<int Degree> 254template<int Degree2> 255PPolynomial<Degree+Degree2> PPolynomial<Degree>::operator * (const PPolynomial<Degree2>& p) const{ 256 PPolynomial<Degree+Degree2> q; 257 StartingPolynomial<Degree+Degree2> *sp; 258 int i,j,spCount=int(polyCount*p.polyCount); 259 260 sp=(StartingPolynomial<Degree+Degree2>*)malloc(sizeof(StartingPolynomial<Degree+Degree2>)*spCount); 261 for(i=0;i<int(polyCount);i++){ 262 for(j=0;j<int(p.polyCount);j++){ 263 sp[i*p.polyCount+j]=polys[i]*p.polys[j]; 264 } 265 } 266 q.set(sp,spCount); 267 free(sp); 268 return q; 269} 270template<int Degree> 271template<int Degree2> 272PPolynomial<Degree+Degree2> PPolynomial<Degree>::operator * (const Polynomial<Degree2>& p) const{ 273 PPolynomial<Degree+Degree2> q; 274 q.set(polyCount); 275 for(int i=0;i<int(polyCount);i++){ 276 q.polys[i].start=polys[i].start; 277 q.polys[i].p=polys[i].p*p; 278 } 279 return q; 280} 281template<int Degree> 282PPolynomial<Degree> PPolynomial<Degree>::scale( double s ) const 283{ 284 PPolynomial q; 285 q.set( polyCount ); 286 for( size_t i=0 ; i<polyCount ; i++ ) q.polys[i] = polys[i].scale(s); 287 if( s<0 ) qsort( q.polys , polyCount , sizeof( StartingPolynomial< Degree > ) , StartingPolynomial< Degree >::Compare ); 288 return q; 289} 290template< int Degree > 291PPolynomial< Degree > PPolynomial< Degree >::reflect( double r ) const 292{ 293 PPolynomial q; 294 q.set( polyCount ); 295 for( size_t i=0 ; i<polyCount ; i++ ) 296 { 297 q.polys[i].scale(-1.); 298 if( r ) q.polys[i].shift( 2.*r ); 299 } 300 qsort( q.polys , polyCount , sizeof( StartingPolynomial< Degree > ) , StartingPolynomial< Degree >::Compare ); 301 return q; 302} 303template<int Degree> 304PPolynomial<Degree> PPolynomial<Degree>::shift( double s ) const 305{ 306 PPolynomial q; 307 q.set(polyCount); 308 for(size_t i=0;i<polyCount;i++){q.polys[i]=polys[i].shift(s);} 309 return q; 310} 311template<int Degree> 312PPolynomial<Degree-1> PPolynomial<Degree>::derivative(void) const{ 313 PPolynomial<Degree-1> q; 314 q.set(polyCount); 315 for(size_t i=0;i<polyCount;i++){ 316 q.polys[i].start=polys[i].start; 317 q.polys[i].p=polys[i].p.derivative(); 318 } 319 return q; 320} 321template<int Degree> 322PPolynomial<Degree+1> PPolynomial<Degree>::integral(void) const{ 323 int i; 324 PPolynomial<Degree+1> q; 325 q.set(polyCount); 326 for(i=0;i<int(polyCount);i++){ 327 q.polys[i].start=polys[i].start; 328 q.polys[i].p=polys[i].p.integral(); 329 q.polys[i].p-=q.polys[i].p(q.polys[i].start); 330 } 331 return q; 332} 333template<int Degree> 334PPolynomial<Degree>& PPolynomial<Degree>::operator += ( double s ) {polys[0].p+=s;} 335template<int Degree> 336PPolynomial<Degree>& PPolynomial<Degree>::operator -= ( double s ) {polys[0].p-=s;} 337template<int Degree> 338PPolynomial<Degree>& PPolynomial<Degree>::operator *= ( double s ) 339{ 340 for(int i=0;i<int(polyCount);i++){polys[i].p*=s;} 341 return *this; 342} 343template<int Degree> 344PPolynomial<Degree>& PPolynomial<Degree>::operator /= ( double s ) 345{ 346 for(size_t i=0;i<polyCount;i++){polys[i].p/=s;} 347 return *this; 348} 349template<int Degree> 350PPolynomial<Degree> PPolynomial<Degree>::operator + ( double s ) const 351{ 352 PPolynomial q=*this; 353 q+=s; 354 return q; 355} 356template<int Degree> 357PPolynomial<Degree> PPolynomial<Degree>::operator - ( double s ) const 358{ 359 PPolynomial q=*this; 360 q-=s; 361 return q; 362} 363template<int Degree> 364PPolynomial<Degree> PPolynomial<Degree>::operator * ( double s ) const 365{ 366 PPolynomial q=*this; 367 q*=s; 368 return q; 369} 370template<int Degree> 371PPolynomial<Degree> PPolynomial<Degree>::operator / ( double s ) const 372{ 373 PPolynomial q=*this; 374 q/=s; 375 return q; 376} 377 378template<int Degree> 379void PPolynomial<Degree>::printnl(void) const{ 380 Polynomial<Degree> p; 381 382 if(!polyCount){ 383 Polynomial<Degree> p; 384 printf("[-Infinity,Infinity]\n"); 385 } 386 else{ 387 for(size_t i=0;i<polyCount;i++){ 388 printf("["); 389 if (polys[i ].start== DBL_MAX){printf("Infinity,");} 390 else if (polys[i ].start==-DBL_MAX){printf("-Infinity,");} 391 else {printf("%f,",polys[i].start);} 392 if(i+1==polyCount) {printf("Infinity]\t");} 393 else if (polys[i+1].start== DBL_MAX){printf("Infinity]\t");} 394 else if (polys[i+1].start==-DBL_MAX){printf("-Infinity]\t");} 395 else {printf("%f]\t",polys[i+1].start);} 396 p=p+polys[i].p; 397 p.printnl(); 398 } 399 } 400 printf("\n"); 401} 402template< > 403PPolynomial< 0 > PPolynomial< 0 >::BSpline( double radius ) 404{ 405 PPolynomial q; 406 q.set(2); 407 408 q.polys[0].start=-radius; 409 q.polys[1].start= radius; 410 411 q.polys[0].p.coefficients[0]= 1.0; 412 q.polys[1].p.coefficients[0]=-1.0; 413 return q; 414} 415template< int Degree > 416PPolynomial< Degree > PPolynomial<Degree>::BSpline( double radius ) 417{ 418 return PPolynomial< Degree-1 >::BSpline().MovingAverage( radius ); 419} 420template<int Degree> 421PPolynomial<Degree+1> PPolynomial<Degree>::MovingAverage( double radius ) const 422{ 423 PPolynomial<Degree+1> A; 424 Polynomial<Degree+1> p; 425 Pointer( StartingPolynomial< Degree+1 > ) sps; 426 sps = AllocPointer< StartingPolynomial< Degree+1 > >( polyCount*2 ); 427 428 429 for(int i=0;i<int(polyCount);i++){ 430 sps[2*i ].start=polys[i].start-radius; 431 sps[2*i+1].start=polys[i].start+radius; 432 p=polys[i].p.integral()-polys[i].p.integral()(polys[i].start); 433 sps[2*i ].p=p.shift(-radius); 434 sps[2*i+1].p=p.shift( radius)*-1; 435 } 436 A.set( sps , int(polyCount*2) ); 437 FreePointer( sps ); 438 return A*1.0/(2*radius); 439} 440template<int Degree> 441void PPolynomial<Degree>::getSolutions(double c,std::vector<double>& roots,double EPS,double min,double max) const{ 442 Polynomial<Degree> p; 443 std::vector<double> tempRoots; 444 445 p.setZero(); 446 for(size_t i=0;i<polyCount;i++){ 447 p+=polys[i].p; 448 if(polys[i].start>max){break;} 449 if(i<polyCount-1 && polys[i+1].start<min){continue;} 450 p.getSolutions(c,tempRoots,EPS); 451 for(size_t j=0;j<tempRoots.size();j++){ 452 if(tempRoots[j]>polys[i].start && (i+1==polyCount || tempRoots[j]<=polys[i+1].start)){ 453 if(tempRoots[j]>min && tempRoots[j]<max){roots.push_back(tempRoots[j]);} 454 } 455 } 456 } 457} 458 459template<int Degree> 460void PPolynomial<Degree>::write(FILE* fp,int samples,double min,double max) const{ 461 fwrite(&samples,sizeof(int),1,fp); 462 for(int i=0;i<samples;i++){ 463 double x=min+i*(max-min)/(samples-1); 464 float v=(*this)(x); 465 fwrite(&v,sizeof(float),1,fp); 466 } 467} 468