1 /*! ======================================================================== 2 ** Extended Template and Library 3 ** Fast fastangle Abstraction Class Implementation 4 ** $Id$ 5 ** 6 ** Copyright (c) 2002 Robert B. Quattlebaum Jr. 7 ** 8 ** This package is free software; you can redistribute it and/or 9 ** modify it under the terms of the GNU General Public License as 10 ** published by the Free Software Foundation; either version 2 of 11 ** the License, or (at your option) any later version. 12 ** 13 ** This package is distributed in the hope that it will be useful, 14 ** but WITHOUT ANY WARRANTY; without even the implied warranty of 15 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 ** General Public License for more details. 17 ** 18 ** === N O T E S =========================================================== 19 ** 20 ** This is an internal header file, included by other ETL headers. 21 ** You should not attempt to use it directly. 22 ** 23 ** ========================================================================= */ 24 25 /* === S T A R T =========================================================== */ 26 27 #ifndef __ETL__FASTANGLE_H 28 #define __ETL__FASTANGLE_H 29 30 /* === H E A D E R S ======================================================= */ 31 32 #include <cmath> 33 #include <ETL/fixed> 34 35 #include "_fastangle_tables.h" 36 37 /* === M A C R O S ========================================================= */ 38 39 #ifndef PI 40 # define PI (3.1415926535897932384626433832795029L) 41 #endif 42 43 #define ETL_FASTANGLE_INIT() 44 45 /* === T Y P E D E F S ===================================================== */ 46 47 /* === C L A S S E S & S T R U C T S ======================================= */ 48 49 _ETL_BEGIN_NAMESPACE 50 51 /*! ======================================================================== 52 ** \class fastangle 53 ** \brief Optimized abstraction of the concept of an angle 54 ** 55 ** A more detailed description needs to be written. 56 */ 57 class fastangle 58 { 59 public: 60 typedef double value_type; 61 62 protected: 63 typedef fixed_base<ETL_FIXED_TYPE,ETL_FASTANGLE_LOOKUP_RES> unit; 64 65 unit v; //! Stored in rotations 66 67 public: 68 69 /* 70 ** Arithmetic Operators 71 */ 72 73 //! fastangle Addition Operator 74 fastangle 75 operator+(const fastangle &rhs)const 76 { 77 fastangle ret; 78 ret.v=v+rhs.v; 79 return ret; 80 } 81 82 //! fastangle Subtraction Operator 83 /*! \sa fastangle dist(const fastangle &) */ 84 fastangle 85 operator-(const fastangle &rhs)const 86 { 87 fastangle ret; 88 ret.v=v-rhs.v; 89 return ret; 90 } 91 92 //! fastangle Scalar Multiplication Operator 93 /*! This operator will multiply the given 94 fastangle by the given scalar value. */ 95 fastangle 96 operator*(const unit &rhs)const 97 { 98 fastangle ret; 99 ret.v=v*rhs; 100 return ret; 101 } 102 103 fastangle 104 operator/(const unit &rhs)const 105 { 106 fastangle ret; 107 ret.v=v/rhs; 108 return ret; 109 } 110 111 const fastangle & 112 operator+=(const fastangle &rhs) 113 { 114 v+=rhs.v; 115 return *this; 116 } 117 118 const fastangle & 119 operator-=(const fastangle &rhs) 120 { 121 v-=rhs.v; 122 return *this; 123 } 124 125 const fastangle & 126 operator*=(const unit &rhs) 127 { 128 v*=rhs; 129 return *this; 130 } 131 132 const fastangle & 133 operator/=(const unit &rhs) 134 { 135 v/=rhs; 136 return *this; 137 } 138 139 //! fastangle Negation 140 fastangle 141 operator-()const 142 { 143 fastangle ret; 144 ret.v=-v; 145 return ret; 146 } 147 148 //! 180 degree rotation operator 149 /*! Returns the fastangle directly opposite of 150 the given fastangle, and will yield a result 151 between 0 and 2PI */ 152 fastangle 153 operator~()const 154 { 155 fastangle ret; 156 ret.v=(unit)std::floor(v+0.5f); 157 return ret; 158 } 159 160 /*! Returns true if the shortest 161 fastangle between the left-hand and 162 right-hand side is clockwise */ 163 bool 164 operator<(const fastangle &rhs)const 165 { return v<rhs.v; } 166 // { return dist(rhs).v<(value_type)0.0; } 167 168 /*! Returns true if the shortest 169 fastangle between the left-hand and 170 right-hand side is counter-clockwise */ 171 bool 172 operator>(const fastangle &rhs)const 173 { return v>rhs.v; } 174 // { return dist(rhs).v>(value_type)0.0; } 175 176 /*! Returns true if the shortest 177 fastangle between the left-hand and 178 right-hand side is clockwise, 179 or if the angles are refer to the same 180 point on the unit circle. */ 181 bool 182 operator<=(const fastangle &rhs)const 183 { return v<=rhs.v; } 184 // { return dist(rhs).v<=(value_type)0.0; } 185 186 /*! Returns true if the shortest 187 fastangle between the left-hand and 188 right-hand side is counter-clockwise, 189 or if the angles are refer to the same 190 point on the unit circle. */ 191 bool 192 operator>=(const fastangle &rhs)const 193 { return v>=rhs.v; } 194 // { return dist(rhs).v>=(value_type)0.0; } 195 196 /*! Returns true if the angles 197 are refer to the same point 198 on the unit circle. */ 199 bool 200 operator==(const fastangle &rhs)const 201 { return v==rhs.v; } 202 // { return dist(rhs).v==(value_type)0.0; } 203 204 /*! Returns false if the angles 205 are refer to the same point 206 on the unit circle. */ 207 bool 208 operator!=(const fastangle &rhs)const 209 { return v!=rhs.v; } 210 // { return dist(rhs).v!=(value_type)0.0; } 211 212 //! fastangle Difference Function 213 /*! This function will return the 214 shortest physical distance between 215 two angles, from -PI/2 to PI/2 216 \warning Not yet tested 217 \sa fastangle operator-(const fastangle &) */ 218 fastangle dist(const fastangle & rhs)219 dist(const fastangle &rhs)const 220 { 221 fastangle ret; 222 ret.v=v-rhs.v; 223 ret.v-=(unit)std::floor(ret.v+0.5f); 224 return ret; 225 } 226 227 //! Rotation Modulus 228 /*! This function will return the 229 value of the fastangle between 0 and 2PI */ 230 fastangle mod()231 mod()const 232 { 233 fastangle ret(*this); 234 ret.v-=(unit)std::floor(ret.v); 235 return ret; 236 } 237 238 static fastangle zero()239 zero() 240 { 241 fastangle ret; 242 ret.v=0; 243 return ret; 244 } 245 246 bool operator!()const { return v==unit(0); } 247 248 /* 249 ** Conversion Classes 250 */ 251 252 class radians; 253 class degrees; 254 class rotations; 255 256 /* 257 ** Trigonometric Classes 258 */ 259 260 class sin; 261 class cos; 262 class tan; 263 264 /* 265 ** Friend classes 266 */ 267 268 friend class radians; 269 friend class degrees; 270 friend class rotations; 271 friend class sin; 272 friend class cos; 273 friend class tan; 274 275 /* 276 ** Bleh... 277 */ 278 279 typedef radians rad; 280 typedef degrees deg; 281 typedef rotations rot; 282 283 }; // END of class fastangle 284 285 /*! ======================================================================== 286 ** \class fastangle::radians 287 ** \brief fastangle representation in radians 288 ** 289 ** A more detailed description needs to be written. 290 */ 291 class fastangle::radians : public fastangle 292 { 293 public: radians(const value_type & x)294 radians(const value_type &x) { v=x/((value_type)PI*2.0f); } radians(const fastangle & a)295 radians(const fastangle &a):fastangle(a) { } mod()296 radians mod()const { return fastangle::mod(); } dist(const fastangle & rhs)297 radians dist(const fastangle &rhs)const { return fastangle::dist(rhs); } value_type()298 operator value_type()const { return get(); } get()299 value_type get()const { return (value_type)v*(value_type)PI*2.0f; } 300 }; // END of class fastangle::radians 301 302 /*! ======================================================================== 303 ** \class fastangle::degrees 304 ** \brief fastangle representation in degrees 305 ** 306 ** A more detailed description needs to be written. 307 */ 308 class fastangle::degrees : public fastangle 309 { 310 public: degrees(const value_type & x)311 degrees(const value_type &x) { v=x/360; } degrees(const fastangle & a)312 degrees(const fastangle &a):fastangle(a) { } mod()313 degrees mod()const { return fastangle::mod(); } dist(const fastangle & rhs)314 degrees dist(const fastangle &rhs)const { return fastangle::dist(rhs); } value_type()315 operator value_type()const { return get(); } get()316 value_type get()const { return v*360/*(value_type)(v-::floor(v))*360*/; } 317 }; // END of class fastangle::degrees 318 319 /*! ======================================================================== 320 ** \class fastangle::rotations 321 ** \brief fastangle representation in rotations 322 ** 323 ** A more detailed description needs to be written. 324 */ 325 class fastangle::rotations : public fastangle 326 { 327 public: rotations(const value_type & x)328 rotations(const value_type &x) { v=x; } rotations(const fastangle & a)329 rotations(const fastangle &a):fastangle(a) { } mod()330 rotations mod()const { return fastangle::mod(); } dist(const fastangle & rhs)331 rotations dist(const fastangle &rhs)const { return fastangle::dist(rhs); } value_type()332 operator value_type()const { return get(); } get()333 value_type get()const { return v; } 334 }; // END of class fastangle::rotations 335 336 /*! ======================================================================== 337 ** \class fastangle::sin 338 ** \brief fastangle representation as a sine function 339 ** 340 ** A more detailed description needs to be written. 341 */ 342 class fastangle::sin : public fastangle 343 { 344 public: sin(const value_type & x)345 sin(const value_type &x) { v.data()=_fastangle_asin_table[(int)((x+1)*(value_type)(1<<(ETL_FASTANGLE_LOOKUP_RES-1)))]; } sin(const fastangle & a)346 sin(const fastangle &a):fastangle(a) { } mod()347 sin mod()const { return fastangle::mod(); } dist(const fastangle & rhs)348 sin dist(const fastangle &rhs)const { return fastangle::dist(rhs); } value_type()349 operator value_type()const { return get(); } get()350 value_type get()const { return (value_type)_fastangle_sin_table[v.data()&( (1<<ETL_FASTANGLE_LOOKUP_RES)-1)]; } 351 }; // END of class fastangle::sin 352 353 /*! ======================================================================== 354 ** \class fastangle::cos 355 ** \brief fastangle representation as a cosine function 356 ** 357 ** A more detailed description needs to be written. 358 */ 359 class fastangle::cos : public fastangle 360 { 361 public: cos(const value_type & x)362 cos(const value_type &x) { v.data()=(1<<(ETL_FASTANGLE_LOOKUP_RES-2))-_fastangle_asin_table[(int)((x+1)*(value_type)(1<<(ETL_FASTANGLE_LOOKUP_RES-1)))]; } cos(const fastangle & a)363 cos(const fastangle &a):fastangle(a) { } mod()364 cos mod()const { return fastangle::mod(); } dist(const fastangle & rhs)365 cos dist(const fastangle &rhs)const { return fastangle::dist(rhs); } value_type()366 operator value_type()const { return get(); } get()367 value_type get()const { return (value_type)_fastangle_sin_table[(v.data()+(1<<(ETL_FASTANGLE_LOOKUP_RES-2)))&( (1<<ETL_FASTANGLE_LOOKUP_RES)-1)]; } 368 }; // END of class fastangle::cos 369 370 /*! ======================================================================== 371 ** \class fastangle::tan 372 ** \brief fastangle representation as a tangent function 373 ** 374 ** A more detailed description needs to be written. 375 */ 376 class fastangle::tan : public fastangle 377 { 378 public: tan(const value_type & x)379 tan(const value_type &x) 380 { 381 if(x>1) 382 v.data()=(1<<(ETL_FASTANGLE_LOOKUP_RES-2))-_fastangle_atan_table[(int)(((1.0/x)+1)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 383 else if(x<-1) 384 v.data()=-(1<<(ETL_FASTANGLE_LOOKUP_RES-1)) + (1<<(ETL_FASTANGLE_LOOKUP_RES-2)) - _fastangle_atan_table[(int)(((1.0/x)+1)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 385 else 386 v.data()=_fastangle_atan_table[(int)((x+1)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 387 } 388 tan(const value_type & y,const value_type & x)389 tan(const value_type &y,const value_type &x) 390 { 391 if(x>=0 && y>=0) // First quadrant 392 { 393 if(y>x) 394 v.data()=(1<<(ETL_FASTANGLE_LOOKUP_RES-2))-_fastangle_atan_table[(int)(((x/y)+1)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 395 else 396 v.data()=_fastangle_atan_table[(int)(((y/x)+1)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 397 } 398 else if(x>=0 && y<0) // Fourth quadrant 399 { 400 if(-y>x) 401 v.data()=-(1<<(ETL_FASTANGLE_LOOKUP_RES-1)) + (1<<(ETL_FASTANGLE_LOOKUP_RES-2))-_fastangle_atan_table[(int)(((x/y)+1.0)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 402 else 403 v.data()=_fastangle_atan_table[(int)(((y/x)+1.0)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 404 } 405 else if(x<0 && y>=0) // Second quadrant 406 { 407 if(y>-x) 408 v.data()=(1<<(ETL_FASTANGLE_LOOKUP_RES-2))-_fastangle_atan_table[(int)(((x/y)+1.0)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]; 409 else 410 v.data()=_fastangle_atan_table[(int)(((y/x)+1.0)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]+(1<<(ETL_FASTANGLE_LOOKUP_RES-1)); 411 } 412 else if(x<0 && y<0) // Third Quadrant 413 { 414 if(-y>-x) 415 v.data()=(1<<(ETL_FASTANGLE_LOOKUP_RES-2))-_fastangle_atan_table[(int)(((x/y)+1.0)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))] - (1<<(ETL_FASTANGLE_LOOKUP_RES-1)); 416 else 417 v.data()=_fastangle_atan_table[(int)(((y/x)+1.0)*(value_type)((1<<(ETL_FASTANGLE_LOOKUP_RES-1))-1))]-(1<<(ETL_FASTANGLE_LOOKUP_RES-1)); 418 } 419 else v.data()=0; 420 } tan(const fastangle & a)421 tan(const fastangle &a):fastangle(a) { } mod()422 tan mod()const { return fastangle::mod(); } dist(const fastangle & rhs)423 tan dist(const fastangle &rhs)const { return fastangle::dist(rhs); } value_type()424 operator value_type()const { return get(); } get()425 value_type get()const { return (value_type)_fastangle_tan_table[v.data()&( (1<<ETL_FASTANGLE_LOOKUP_RES)-1)]; } 426 }; // END of class fastangle::tan 427 428 _ETL_END_NAMESPACE 429 430 template <> 431 struct affine_combo<etl::fastangle,float> 432 { 433 etl::fastangle operator()(const etl::fastangle &a,const etl::fastangle &b,const float &t)const 434 { 435 return b.dist(a)*t+a; 436 } 437 438 etl::fastangle reverse(const etl::fastangle &x, const etl::fastangle &b, const float &t)const 439 { 440 return x.dist(b*t)*((float)1/((float)1-t)); 441 } 442 }; 443 444 template <> 445 struct distance_func<etl::fastangle> : public std::binary_function<etl::fastangle, etl::fastangle, etl::fastangle> 446 { 447 etl::fastangle operator()(const etl::fastangle &a,const etl::fastangle &b)const 448 { 449 etl::fastangle delta=b.dist(a); 450 if(delta<etl::fastangle::zero()) 451 return -delta; 452 return delta; 453 } 454 455 etl::fastangle cook(const etl::fastangle &x) { return x; } 456 etl::fastangle uncook(const etl::fastangle &x) { return x; } 457 }; 458 459 /* === E N D =============================================================== */ 460 461 #endif 462