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