diff --git a/dda.c b/dda.c index 545a6be..b037ba4 100644 --- a/dda.c +++ b/dda.c @@ -50,127 +50,6 @@ TARGET current_position __attribute__ ((__section__ (".bss"))); /// \brief numbers for tracking the current state of movement MOVE_STATE move_state __attribute__ ((__section__ (".bss"))); -/* - utility functions -*/ - -// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml -/*! linear approximation 2d distance formula - \param dx distance in X plane - \param dy distance in Y plane - \return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2}\f$ - - see http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml -*/ -uint32_t approx_distance( uint32_t dx, uint32_t dy ) -{ - uint32_t min, max, approx; - - if ( dx < dy ) - { - min = dx; - max = dy; - } else { - min = dy; - max = dx; - } - - approx = ( max * 1007 ) + ( min * 441 ); - if ( max < ( min << 4 )) - approx -= ( max * 40 ); - - // add 512 for proper rounding - return (( approx + 512 ) >> 10 ); -} - -// courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance -/*! linear approximation 3d distance formula - \param dx distance in X plane - \param dy distance in Y plane - \param dz distance in Z plane - \return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2 + \Delta z^2}\f$ - - see http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance -*/ -uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz ) -{ - uint32_t min, med, max, approx; - - if ( dx < dy ) - { - min = dy; - med = dx; - } else { - min = dx; - med = dy; - } - - if ( dz < min ) - { - max = med; - med = min; - min = dz; - } else if ( dz < med ) { - max = med; - med = dz; - } else { - max = dz; - } - - approx = ( max * 860 ) + ( med * 851 ) + ( min * 520 ); - if ( max < ( med << 1 )) approx -= ( max * 294 ); - if ( max < ( min << 2 )) approx -= ( max * 113 ); - if ( med < ( min << 2 )) approx -= ( med * 40 ); - - // add 512 for proper rounding - return (( approx + 512 ) >> 10 ); -} - -/*! - integer square root algorithm - \param a find square root of this number - \return sqrt(a - 1) < returnvalue <= sqrt(a) - - see http://www.embedded-systems.com/98/9802fe2.htm -*/ -// courtesy of http://www.embedded-systems.com/98/9802fe2.htm -uint16_t int_sqrt(uint32_t a) { - uint32_t rem = 0; - uint32_t root = 0; - uint16_t i; - - for (i = 0; i < 16; i++) { - root <<= 1; - rem = ((rem << 2) + (a >> 30)); - a <<= 2; - root++; - if (root <= rem) { - rem -= root; - root++; - } - else - root--; - } - return (uint16_t) ((root >> 1) & 0xFFFFL); -} - -// this is an ultra-crude pseudo-logarithm routine, such that: -// 2 ^ msbloc(v) >= v -/*! crude logarithm algorithm - \param v value to find \f$log_2\f$ of - \return floor(log(v) / log(2)) -*/ -const uint8_t msbloc (uint32_t v) { - uint8_t i; - uint32_t c; - for (i = 31, c = 0x80000000; i; i--) { - if (v & c) - return i; - c >>= 1; - } - return 0; -} - /*! Inititalise DDA movement structures */ void dda_init(void) { diff --git a/dda.h b/dda.h index d8fcc8b..37cc69e 100644 --- a/dda.h +++ b/dda.h @@ -159,12 +159,6 @@ extern TARGET current_position; methods */ -uint32_t approx_distance( uint32_t dx, uint32_t dy ) __attribute__ ((hot)); -uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz ) __attribute__ ((hot)); - -// const because return value is always the same given the same v -const uint8_t msbloc (uint32_t v) __attribute__ ((const)); - // initialize dda structures void dda_init(void); diff --git a/dda_maths.c b/dda_maths.c index f2144bb..fe613a5 100644 --- a/dda_maths.c +++ b/dda_maths.c @@ -1,6 +1,6 @@ /** \file - \brief Mathematic algorithms for the digital differential analyser (DDA). + \brief Mathematic algorithms for the digital differential analyser (DDA). */ #include "dda_maths.h" @@ -63,3 +63,114 @@ const int32_t muldivQR(int32_t multiplicand, uint32_t qn, uint32_t rn, return negative_flag ? -((int32_t)quotient) : (int32_t)quotient; } +// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml +/*! linear approximation 2d distance formula + \param dx distance in X plane + \param dy distance in Y plane + \return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2}\f$ + + see http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml +*/ +uint32_t approx_distance(uint32_t dx, uint32_t dy) { + uint32_t min, max, approx; + + if ( dx < dy ) { + min = dx; + max = dy; + } else { + min = dy; + max = dx; + } + + approx = ( max * 1007 ) + ( min * 441 ); + if ( max < ( min << 4 )) + approx -= ( max * 40 ); + + // add 512 for proper rounding + return (( approx + 512 ) >> 10 ); +} + +// courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance +/*! linear approximation 3d distance formula + \param dx distance in X plane + \param dy distance in Y plane + \param dz distance in Z plane + \return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2 + \Delta z^2}\f$ + + see http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance +*/ +uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz) { + uint32_t min, med, max, approx; + + if ( dx < dy ) { + min = dy; + med = dx; + } else { + min = dx; + med = dy; + } + + if ( dz < min ) { + max = med; + med = min; + min = dz; + } else if ( dz < med ) { + max = med; + med = dz; + } else { + max = dz; + } + + approx = ( max * 860 ) + ( med * 851 ) + ( min * 520 ); + if ( max < ( med << 1 )) approx -= ( max * 294 ); + if ( max < ( min << 2 )) approx -= ( max * 113 ); + if ( med < ( min << 2 )) approx -= ( med * 40 ); + + // add 512 for proper rounding + return (( approx + 512 ) >> 10 ); +} + +/*! + integer square root algorithm + \param a find square root of this number + \return sqrt(a - 1) < returnvalue <= sqrt(a) + + see http://www.embedded-systems.com/98/9802fe2.htm +*/ +// courtesy of http://www.embedded-systems.com/98/9802fe2.htm +uint16_t int_sqrt(uint32_t a) { + uint32_t rem = 0; + uint32_t root = 0; + uint16_t i; + + for (i = 0; i < 16; i++) { + root <<= 1; + rem = ((rem << 2) + (a >> 30)); + a <<= 2; + root++; + if (root <= rem) { + rem -= root; + root++; + } + else + root--; + } + return (uint16_t) ((root >> 1) & 0xFFFFL); +} + +// this is an ultra-crude pseudo-logarithm routine, such that: +// 2 ^ msbloc(v) >= v +/*! crude logarithm algorithm + \param v value to find \f$log_2\f$ of + \return floor(log(v) / log(2)) +*/ +const uint8_t msbloc (uint32_t v) { + uint8_t i; + uint32_t c; + for (i = 31, c = 0x80000000; i; i--) { + if (v & c) + return i; + c >>= 1; + } + return 0; +} diff --git a/dda_maths.h b/dda_maths.h index de819ab..28028a9 100644 --- a/dda_maths.h +++ b/dda_maths.h @@ -50,4 +50,17 @@ inline int32_t um_to_steps_e(int32_t distance) { STEPS_PER_M_E % 1000000UL, 1000000UL); } +// approximate 2D distance +uint32_t approx_distance(uint32_t dx, uint32_t dy); + +// approximate 3D distance +uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz); + +// integer square root algorithm +uint16_t int_sqrt(uint32_t a); + +// this is an ultra-crude pseudo-logarithm routine, such that: +// 2 ^ msbloc(v) >= v +const uint8_t msbloc (uint32_t v); + #endif /* _DDA_MATHS_H */