git.maemo.org Git - blok/blob - Box2D/Source/Common/Fixed.h

   1 /*
   2 Copyright (c) 2006 Henry Strickland & Ryan Seto
   3               2007-2008 Tobias Weyand (modifications and extensions)
   4
   5 Permission is hereby granted, free of charge, to any person obtaining a
   6 copy of this software and associated documentation files (the "Software"),
   7 to deal in the Software without restriction, including without limitation
   8 the rights to use, copy, modify, merge, publish, distribute, sublicense,
   9 and/or sell copies of the Software, and to permit persons to whom the
  10 Software is furnished to do so, subject to the following conditions:
  11
  12 The above copyright notice and this permission notice shall be included
  13 in all copies or substantial portions of the Software.
  14
  15 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  18 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
  19 OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
  20 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
  21 OTHER DEALINGS IN THE SOFTWARE.
  22
  23         (* http://www.opensource.org/licenses/mit-license.php *)
  24 */
  25
  26 #ifndef _FIXED_H_
  27 #define _FIXED_H_
  28
  29 #include <stdio.h>
  30
  31 #ifdef TARGET_IS_NDS
  32
  33 #include "nds.h"
  34
  35 #endif
  36
  37 #define FIXED_BP        16
  38 #define FIXED_MAX       ((1<<(32-FIXED_BP-1))-1)
  39 #define FIXED_MIN       (-(1<<(32-FIXED_BP-1)))
  40 #define FIXED_EPSILON   (Fixed(0.00007f))
  41
  42 #define G_1_DIV_PI              20861
  43
  44 class Fixed {
  45
  46         private:
  47
  48                 int     g; // the guts
  49
  50                 const static int BP= FIXED_BP;  // how many low bits are right of Binary Point
  51                 const static int BP2= BP*2;  // how many low bits are right of Binary Point
  52                 const static int BPhalf= BP/2;  // how many low bits are right of Binary Point
  53
  54                 double STEP();  // smallest step we can represent
  55
  56                 // for private construction via guts
  57                 enum FixedRaw { RAW };
  58                 Fixed(FixedRaw, int guts);
  59
  60         public:
  61
  62                 Fixed();
  63                 Fixed(const Fixed &a);
  64                 Fixed(float a);
  65                 Fixed(double a);
  66                 Fixed(int a);
  67                 Fixed(long a);
  68
  69                 Fixed& operator =(const Fixed a);
  70                 Fixed& operator =(float a);
  71                 Fixed& operator =(double a);
  72                 Fixed& operator =(int a);
  73                 Fixed& operator =(long a);
  74
  75                 operator float();
  76                 operator double();
  77                 operator int();
  78                 operator long();
  79                 operator unsigned short();
  80
  81                 operator float() const;
  82
  83                 Fixed operator +() const;
  84                 Fixed operator -() const;
  85
  86                 Fixed operator +(const Fixed a) const;
  87                 Fixed operator -(const Fixed a) const;
  88 #if 1
  89                 // more acurate, using long long
  90                 Fixed operator *(const Fixed a) const;
  91 #else
  92                 // faster, but with only half as many bits right of binary point
  93                 Fixed operator *(const Fixed a) const;
  94 #endif
  95                 Fixed operator /(const Fixed a) const;
  96
  97                 Fixed operator *(unsigned short a) const;
  98                 Fixed operator *(int a) const;
  99
 100                 Fixed operator +(float a) const;
 101                 Fixed operator -(float a) const;
 102                 Fixed operator *(float a) const;
 103                 Fixed operator /(float a) const;
 104
 105                 Fixed operator +(double a) const;
 106                 Fixed operator -(double a) const;
 107                 Fixed operator *(double a) const;
 108                 Fixed operator /(double a) const;
 109
 110                 Fixed operator >>(int a) const;
 111                 Fixed operator <<(int a) const;
 112
 113                 Fixed& operator +=(Fixed a);
 114                 Fixed& operator -=(Fixed a);
 115                 Fixed& operator *=(Fixed a);
 116                 Fixed& operator /=(Fixed a);
 117
 118                 Fixed& operator +=(int a);
 119                 Fixed& operator -=(int a);
 120                 Fixed& operator *=(int a);
 121                 Fixed& operator /=(int a);
 122
 123                 Fixed& operator +=(long a);
 124                 Fixed& operator -=(long a);
 125                 Fixed& operator *=(long a);
 126                 Fixed& operator /=(long a);
 127
 128                 Fixed& operator +=(float a);
 129                 Fixed& operator -=(float a);
 130                 Fixed& operator *=(float a);
 131                 Fixed& operator /=(float a);
 132
 133                 Fixed& operator +=(double a);
 134                 Fixed& operator -=(double a);
 135                 Fixed& operator *=(double a);
 136                 Fixed& operator /=(double a);
 137
 138                 bool operator ==(const Fixed a) const;
 139                 bool operator !=(const Fixed a) const;
 140                 bool operator <=(const Fixed a) const;
 141                 bool operator >=(const Fixed a) const;
 142                 bool operator  <(const Fixed a) const;
 143                 bool operator  >(const Fixed a) const;
 144
 145                 bool operator ==(float a) const;
 146                 bool operator !=(float a) const;
 147                 bool operator <=(float a) const;
 148                 bool operator >=(float a) const;
 149                 bool operator  <(float a) const;
 150                 bool operator  >(float a) const;
 151
 152                 bool operator ==(double a) const;
 153                 bool operator !=(double a) const;
 154                 bool operator <=(double a) const;
 155                 bool operator >=(double a) const;
 156                 bool operator  <(double a) const;
 157                 bool operator  >(double a) const;
 158
 159                 bool operator  >(int a) const;
 160                 bool operator  <(int a) const;
 161                 bool operator  >=(int a) const;
 162                 bool operator  <=(int a) const;
 163
 164                 Fixed abs();
 165                 Fixed sqrt();
 166 #ifdef TARGET_IS_NDS
 167                 Fixed cosf();
 168                 Fixed sinf();
 169                 Fixed tanf();
 170 #endif
 171 };
 172
 173 //
 174 // Implementation
 175 //
 176
 177 inline double Fixed::STEP() { return 1.0 / (1<<BP); }  // smallest step we can represent
 178
 179 // for private construction via guts
 180 inline Fixed::Fixed(FixedRaw, int guts) : g(guts) {}
 181
 182 inline Fixed::Fixed() : g(0) {}
 183 inline Fixed::Fixed(const Fixed &a) : g( a.g ) {}
 184 inline Fixed::Fixed(float a) : g( int(a * (float)(1<<BP)) ) {}
 185 inline Fixed::Fixed(double a) : g( int(a * (double)(1<<BP) ) ) {}
 186 inline Fixed::Fixed(int a) : g( a << BP ) {}
 187 inline Fixed::Fixed(long a) : g( a << BP ) {}
 188
 189 inline Fixed& Fixed::operator =(const Fixed a) { g= a.g; return *this; }
 190 inline Fixed& Fixed::operator =(float a) { g= Fixed(a).g; return *this; }
 191 inline Fixed& Fixed::operator =(double a) { g= Fixed(a).g; return *this; }
 192 inline Fixed& Fixed::operator =(int a) { g= Fixed(a).g; return *this; }
 193 inline Fixed& Fixed::operator =(long a) { g= Fixed(a).g; return *this; }
 194
 195 inline Fixed::operator float() { return g * (float)STEP(); }
 196 inline Fixed::operator double() { return g * (double)STEP(); }
 197 inline Fixed::operator int() { return g>>BP; }
 198 inline Fixed::operator long() { return g>>BP; }
 199 //#pragma warning(disable: 4244) //HARDWIRE added pragma to prevent VS2005 compilation error
 200 inline Fixed::operator unsigned short() { return g>>BP; }
 201 inline Fixed::operator float() const { return g / (float)(1<<BP); }
 202
 203 inline Fixed Fixed::operator +() const { return Fixed(RAW,g); }
 204 inline Fixed Fixed::operator -() const { return Fixed(RAW,-g); }
 205
 206 inline Fixed Fixed::operator +(const Fixed a) const { return Fixed(RAW, g + a.g); }
 207 inline Fixed Fixed::operator -(const Fixed a) const { return Fixed(RAW, g - a.g); }
 208
 209 #if 1
 210 // more acurate, using long long
 211 inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW,  (int)( ((long long)g * (long long)a.g ) >> BP)); }
 212
 213 #elif 0
 214
 215 // check for overflow and figure out where.  Must specify -rdynamic in linker
 216 #include <execinfo.h>
 217 #include <signal.h>
 218 #include <exception>
 219
 220 inline Fixed Fixed::operator *(const Fixed a) const {
 221         long long x =  ((long long)g * (long long)a.g );
 222         if(x > 0x7fffffffffffLL || x < -0x7fffffffffffLL) {
 223                 printf("overflow");
 224                 void *array[2];
 225                 int nSize = backtrace(array, 2);
 226                 char **symbols = backtrace_symbols(array, nSize);
 227                 for(int i=0; i<nSize; i++) {
 228                         printf(" %s", symbols[i]);
 229                 }
 230                 printf("\n");
 231         }
 232         return Fixed(RAW, (int)(x>>BP));
 233 }
 234
 235 #else
 236 // faster, but with only half as many bits right of binary point
 237 inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW, (g>>BPhalf) * (a.g>>BPhalf) ); }
 238 #endif
 239
 240
 241 #ifdef TARGET_IS_NDS
 242 // Division using the DS's maths coprocessor
 243 inline Fixed Fixed::operator /(const Fixed a) const
 244 {
 245         //printf("%d %d\n", (long long)g << BP, a.g);
 246         return Fixed(RAW, int( div64((long long)g << BP, a.g) ) );
 247 }
 248 #else
 249 inline Fixed Fixed::operator /(const Fixed a) const
 250 {
 251         return Fixed(RAW, int( (((long long)g << BP2) / (long long)(a.g)) >> BP) );
 252         //return Fixed(RAW, int( (((long long)g << BP) / (long long)(a.g)) ) );
 253 }
 254 #endif
 255
 256 inline Fixed Fixed::operator *(unsigned short a) const { return operator*(Fixed(a)); }
 257 inline Fixed Fixed::operator *(int a) const { return operator*(Fixed(a)); }
 258
 259 inline Fixed Fixed::operator +(float a) const { return Fixed(RAW, g + Fixed(a).g); }
 260 inline Fixed Fixed::operator -(float a) const { return Fixed(RAW, g - Fixed(a).g); }
 261 inline Fixed Fixed::operator *(float a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
 262 //inline Fixed Fixed::operator /(float a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
 263 inline Fixed Fixed::operator /(float a) const { return operator/(Fixed(a)); }
 264
 265 inline Fixed Fixed::operator +(double a) const { return Fixed(RAW, g + Fixed(a).g); }
 266 inline Fixed Fixed::operator -(double a) const { return Fixed(RAW, g - Fixed(a).g); }
 267 inline Fixed Fixed::operator *(double a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
 268 //inline Fixed Fixed::operator /(double a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
 269 inline Fixed Fixed::operator /(double a) const { return operator/(Fixed(a)); }
 270
 271 inline Fixed Fixed::operator >>(int a) const { return Fixed(RAW, g >> a); }
 272 inline Fixed Fixed::operator <<(int a) const { return Fixed(RAW, g << a); }
 273
 274 inline Fixed& Fixed::operator +=(Fixed a) { return *this = *this + a; }
 275 inline Fixed& Fixed::operator -=(Fixed a) { return *this = *this - a; }
 276 inline Fixed& Fixed::operator *=(Fixed a) { return *this = *this * a; }
 277 //inline Fixed& Fixed::operator /=(Fixed a) { return *this = *this / a; }
 278 inline Fixed& Fixed::operator /=(Fixed a) { return *this = operator/(a); }
 279
 280 inline Fixed& Fixed::operator +=(int a) { return *this = *this + (Fixed)a; }
 281 inline Fixed& Fixed::operator -=(int a) { return *this = *this - (Fixed)a; }
 282 inline Fixed& Fixed::operator *=(int a) { return *this = *this * (Fixed)a; }
 283 //inline Fixed& Fixed::operator /=(int a) { return *this = *this / (Fixed)a; }
 284 inline Fixed& Fixed::operator /=(int a) { return *this = operator/((Fixed)a); }
 285
 286 inline Fixed& Fixed::operator +=(long a) { return *this = *this + (Fixed)a; }
 287 inline Fixed& Fixed::operator -=(long a) { return *this = *this - (Fixed)a; }
 288 inline Fixed& Fixed::operator *=(long a) { return *this = *this * (Fixed)a; }
 289 //inline Fixed& Fixed::operator /=(long a) { return *this = *this / (Fixed)a; }
 290 inline Fixed& Fixed::operator /=(long a) { return *this = operator/((Fixed)a); }
 291
 292 inline Fixed& Fixed::operator +=(float a) { return *this = *this + a; }
 293 inline Fixed& Fixed::operator -=(float a) { return *this = *this - a; }
 294 inline Fixed& Fixed::operator *=(float a) { return *this = *this * a; }
 295 //inline Fixed& Fixed::operator /=(float a) { return *this = *this / a; }
 296 inline Fixed& Fixed::operator /=(float a) { return *this = operator/(a); }
 297
 298 inline Fixed& Fixed::operator +=(double a) { return *this = *this + a; }
 299 inline Fixed& Fixed::operator -=(double a) { return *this = *this - a; }
 300 inline Fixed& Fixed::operator *=(double a) { return *this = *this * a; }
 301 //inline Fixed& Fixed::operator /=(double a) { return *this = *this / a; }
 302 inline Fixed& Fixed::operator /=(double a) { return *this = operator/(a); }
 303
 304 inline Fixed operator +(int a, const Fixed b) { return Fixed(a)+b; }
 305 inline Fixed operator -(int a, const Fixed b) { return Fixed(a)-b; }
 306 inline Fixed operator *(int a, const Fixed b) { return Fixed(a)*b; }
 307 inline Fixed operator /(int a, const Fixed b) { return Fixed(a)/b; };
 308
 309 inline Fixed operator +(float a, const Fixed b) { return Fixed(a)+b; }
 310 inline Fixed operator -(float a, const Fixed b) { return Fixed(a)-b; }
 311 inline Fixed operator *(float a, const Fixed b) { return Fixed(a)*b; }
 312 inline Fixed operator /(float a, const Fixed b) { return Fixed(a)/b; }
 313
 314 inline bool Fixed::operator ==(const Fixed a) const { return g == a.g; }
 315 inline bool Fixed::operator !=(const Fixed a) const { return g != a.g; }
 316 inline bool Fixed::operator <=(const Fixed a) const { return g <= a.g; }
 317 inline bool Fixed::operator >=(const Fixed a) const { return g >= a.g; }
 318 inline bool Fixed::operator  <(const Fixed a) const { return g  < a.g; }
 319 inline bool Fixed::operator  >(const Fixed a) const { return g  > a.g; }
 320
 321 inline bool Fixed::operator ==(float a) const { return g == Fixed(a).g; }
 322 inline bool Fixed::operator !=(float a) const { return g != Fixed(a).g; }
 323 inline bool Fixed::operator <=(float a) const { return g <= Fixed(a).g; }
 324 inline bool Fixed::operator >=(float a) const { return g >= Fixed(a).g; }
 325 inline bool Fixed::operator  <(float a) const { return g  < Fixed(a).g; }
 326 inline bool Fixed::operator  >(float a) const { return g  > Fixed(a).g; }
 327
 328 inline bool Fixed::operator ==(double a) const { return g == Fixed(a).g; }
 329 inline bool Fixed::operator !=(double a) const { return g != Fixed(a).g; }
 330 inline bool Fixed::operator <=(double a) const { return g <= Fixed(a).g; }
 331 inline bool Fixed::operator >=(double a) const { return g >= Fixed(a).g; }
 332 inline bool Fixed::operator  <(double a) const { return g  < Fixed(a).g; }
 333 inline bool Fixed::operator  >(double a) const { return g  > Fixed(a).g; }
 334
 335 inline bool Fixed::operator  >(int a) const { return g > Fixed(a).g; }
 336 inline bool Fixed::operator  <(int a) const { return g < Fixed(a).g; }
 337 inline bool Fixed::operator  >=(int a) const{ return g >= Fixed(a).g; };
 338 inline bool Fixed::operator  <=(int a) const{ return g <= Fixed(a).g; };
 339
 340 inline bool operator ==(float a, const Fixed b) { return Fixed(a) == b; }
 341 inline bool operator !=(float a, const Fixed b) { return Fixed(a) != b; }
 342 inline bool operator <=(float a, const Fixed b) { return Fixed(a) <= b; }
 343 inline bool operator >=(float a, const Fixed b) { return Fixed(a) >= b; }
 344 inline bool operator  <(float a, const Fixed b) { return Fixed(a)  < b; }
 345 inline bool operator  >(float a, const Fixed b) { return Fixed(a)  > b; }
 346
 347 inline Fixed operator +(double a, const Fixed b) { return Fixed(a)+b; }
 348 inline Fixed operator -(double a, const Fixed b) { return Fixed(a)-b; }
 349 inline Fixed operator *(double a, const Fixed b) { return Fixed(a)*b; }
 350 inline Fixed operator /(double a, const Fixed b) { return Fixed(a)/b; }
 351
 352 inline bool operator ==(double a, const Fixed b) { return Fixed(a) == b; }
 353 inline bool operator !=(double a, const Fixed b) { return Fixed(a) != b; }
 354 inline bool operator <=(double a, const Fixed b) { return Fixed(a) <= b; }
 355 inline bool operator >=(double a, const Fixed b) { return Fixed(a) >= b; }
 356 inline bool operator  <(double a, const Fixed b) { return Fixed(a)  < b; }
 357 inline bool operator  >(double a, const Fixed b) { return Fixed(a)  > b; }
 358
 359 inline bool operator ==(int a, const Fixed b) { return Fixed(a) == b; }
 360 inline bool operator !=(int a, const Fixed b) { return Fixed(a) != b; }
 361 inline bool operator <=(int a, const Fixed b) { return Fixed(a) <= b; }
 362 inline bool operator >=(int a, const Fixed b) { return Fixed(a) >= b; }
 363 inline bool operator  <(int a, const Fixed b) { return Fixed(a)  < b; }
 364 inline bool operator  >(int a, const Fixed b) { return Fixed(a)  > b; }
 365
 366 inline int& operator +=(int& a, const Fixed b) { a = (Fixed)a + b; return a; }
 367 inline int& operator -=(int& a, const Fixed b) { a = (Fixed)a - b; return a; }
 368 inline int& operator *=(int& a, const Fixed b) { a = (Fixed)a * b; return a; }
 369 inline int& operator /=(int& a, const Fixed b) { a = (Fixed)a / b; return a; }
 370
 371 inline long& operator +=(long& a, const Fixed b) { a = (Fixed)a + b; return a; }
 372 inline long& operator -=(long& a, const Fixed b) { a = (Fixed)a - b; return a; }
 373 inline long& operator *=(long& a, const Fixed b) { a = (Fixed)a * b; return a; }
 374 inline long& operator /=(long& a, const Fixed b) { a = (Fixed)a / b; return a; }
 375
 376 inline float& operator +=(float& a, const Fixed b) { a = a + b; return a; }
 377 inline float& operator -=(float& a, const Fixed b) { a = a - b; return a; }
 378 inline float& operator *=(float& a, const Fixed b) { a = a * b; return a; }
 379 inline float& operator /=(float& a, const Fixed b) { a = a / b; return a; }
 380
 381 inline double& operator +=(double& a, const Fixed b) { a = a + b; return a; }
 382 inline double& operator -=(double& a, const Fixed b) { a = a - b; return a; }
 383 inline double& operator *=(double& a, const Fixed b) { a = a * b; return a; }
 384 inline double& operator /=(double& a, const Fixed b) { a = a / b; return a; }
 385
 386 inline Fixed Fixed::abs() { return (g>0) ? Fixed(RAW, g) : Fixed(RAW, -g); }
 387 inline Fixed abs(Fixed f) { return f.abs(); }
 388
 389 //inline Fixed atan2(Fixed a, Fixed b) { return atan2f((float) a, (float) b); }
 390 inline Fixed atan2(Fixed y, Fixed x)
 391 {
 392         Fixed abs_y = y.abs() + FIXED_EPSILON;  // avoid 0/0
 393         Fixed r, angle;
 394
 395         if(x >= 0.0f) {
 396                 r = (x - abs_y) / (x + abs_y);
 397                 angle = 3.1415926/4.0;
 398         } else {
 399                 r = (x + abs_y) / (abs_y - x);
 400                 angle = 3.0*3.1415926/4.0;
 401         }
 402         angle += Fixed(0.1963) * (r * r * r) - Fixed(0.9817) * r;
 403         return (y < 0) ? -angle : angle;
 404 }
 405
 406 #if TARGET_IS_NDS
 407
 408 static inline long nds_sqrt64(long long a)
 409 {
 410         SQRT_CR = SQRT_64;
 411         while(SQRT_CR & SQRT_BUSY);
 412         SQRT_PARAM64 = a;
 413         while(SQRT_CR & SQRT_BUSY);
 414
 415         return SQRT_RESULT32;
 416 }
 417
 418 static inline int32 div6464(int64 num, int64 den)
 419 {
 420         DIV_CR = DIV_64_64;
 421         while(DIV_CR & DIV_BUSY);
 422         DIV_NUMERATOR64 = num;
 423         DIV_DENOMINATOR64 = den;
 424         while(DIV_CR & DIV_BUSY);
 425
 426         return (DIV_RESULT32);
 427 }
 428
 429 inline Fixed Fixed::sqrt()
 430 {
 431         return Fixed(RAW, nds_sqrt64(((long long)(g))<<BP));
 432 }
 433 #else
 434 inline Fixed Fixed::sqrt()
 435 {
 436         long long m, root = 0, left = (long long)g<<FIXED_BP;
 437         for ( m = (long long)1<<( (sizeof(long long)<<3) - 2); m; m >>= 2 )
 438         {
 439                 if ( ( left & -m ) > root )
 440                         left -= ( root += m ), root += m;
 441                 root >>= 1;
 442         }
 443         return Fixed(RAW, root);
 444 }
 445 #endif
 446
 447 inline Fixed sqrt(Fixed a) { return a.sqrt(); }
 448 inline Fixed sqrtf(Fixed a) { return a.sqrt(); }
 449
 450 #endif
 451
 452 #ifdef TARGET_IS_NDS
 453 // Use the libnds lookup tables for trigonometry functions
 454 inline Fixed Fixed::cosf() {
 455         int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
 456         if(idx < 0)
 457                 idx += 512;
 458         return Fixed(RAW, COS_bin[idx] << 4);
 459 }
 460 inline Fixed cosf(Fixed x) { return x.cosf(); }
 461 inline Fixed Fixed::sinf() {
 462         int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
 463         if(idx < 0)
 464                         idx += 512;
 465         return Fixed(RAW, SIN_bin[idx] << 4);
 466 }
 467 inline Fixed sinf(Fixed x) { return x.sinf(); }
 468 inline Fixed Fixed::tanf() {
 469         int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
 470         if(idx < 0)
 471                                 idx += 512;
 472         return Fixed(RAW, TAN_bin[idx] << 4);
 473 }
 474 inline Fixed tanf(Fixed x) { return x.tanf(); }
 475
 476
 477 #endif