Quick atan2 using integer math
M. K. Borri
Posts: 279
Not sure if it's worth using this rather than floating point, but here it is anyway. Might be good if you're doing GPS related stuff and want a function of reasonable precision that doesn't call any libraries.
'' These two are probably useful for GPS stuff (that's what I wrote them for anyway)
'' By spiritplumber@gmail.com
PUB CoordsToDist(x1,y1,x2,y2) | xx, yy, dist
'' this is a fast dirty sqrt that takes in two pairs of coordinates and returns
'' a distance, in tenths of whatever unit we used for the input, rounded. First XY
'' pair is origin and second XY pair is destination. If this is used with a 2D
'' velocity vector, it returns the speed -- use CoordsToDegs(0,0,VelX,VelY).
xx := (x2 - x1) * 10
yy := (y2 - x1) * 10
dist := ^^(xx * xx + yy * yy)+5/10
return dist
PUB CoordsToDegs(x1,y1,x2,y2) | xx, yy, absyy, absxx, div, quadrant, theta
'' this is a fast dirty atan2 that takes in two pairs of coordinates and returns
'' a value in tenths of a degree, rounded. First XY pair is origin and second XY pair is
'' destination. This is accurate to within 0.2 degrees. If this is used with a 2D velocity vector,
'' it returns the heading -- use CoordsToDegs(0,0,VelX,VelY).
xx := (x2 - x1)
yy := (y2 - x1)
' first off handle special cases, for efficiency
if (xx == 0)
if (yy == 0)
return -1 ' unable to determine heading since nothing is happening
if (yy > 0)
return 0
else
return 1800
if (yy == 0)
if (xx > 0)
return 900
else
return 2700
absyy := || yy ' same as rounding, well almost
absxx := || xx ' same as rounding, well almost
if absxx == absyy
theta := 450000
if absxx < absyy
div := || (((absxx*1000)/absyy))
theta := (div*608+500) - (div*div*157+500000)/1000
if absxx > absyy
div := || (((absyy*1000)/absxx))
theta := (900000) - ((div*608+500) - (div*div*157+500000)/1000)
if ((xx > 0) and (yy > 0))
quadrant := 0
if ((xx > 0) and (yy < 0))
quadrant := 1
if ((xx < 0) and (yy < 0))
quadrant := 2
if ((xx < 0) and (yy > 0))
quadrant := 3
theta := (theta/1000 + ( 900 * quadrant) )
return theta
Comments
question -- is it faster to do it this way, or using floating point?
PUB CoordsToDist(x1,y1,x2,y2) | xx, yy, dist '' this is a fast dirty sqrt that takes in two pairs of coordinates and returns '' a distance, in tenths of whatever unit we used for the input, rounded. First XY '' pair is origin and second XY pair is destination. If this is used with a 2D '' velocity vector, it returns the speed -- use CoordsToDegs(0,0,VelX,VelY). xx := (x2 - x1) * 1000 yy := (y2 - y1) * 1000 dist := (^^((xx * xx) + (yy * yy))+50)/100 return dist PUB CoordsToDegs(x1,y1,x2,y2) | xx, yy, absyy, absxx, div, quadrant, theta '' this is a fast dirty atan2 that takes in two pairs of coordinates and returns '' a value in tenths of a degree, rounded. First XY pair is origin and second XY pair is '' destination. This is accurate to within 0.2 degrees. If this is used with a 2D velocity vector, '' it returns the heading -- use CoordsToDegs(0,0,VelX,VelY). xx := (x2 - x1) yy := (y2 - y1) ' first off handle special cases, for efficiency if (xx == 0) if (yy == 0) return -1 ' unable to determine heading since nothing is happening if (yy > 0) return 2700 else return 0 if (yy == 0) if (xx > 0) return 900 else return 2700 absyy := || yy absxx := || xx if ((absxx > 65532) or (absyy > 65532)) ' 65532 is the maximum number this thing can handle... not 35, 32, i checked repeat absxx := absxx / 2 absyy := absyy / 2 while ((absxx > 65532) or (absyy > 65532)) if absxx == absyy theta := 450000 if absxx < absyy div := || (((absxx*1000)/absyy)) theta := (div*608+500) - (div*div*157+500000)/1000 if absxx > absyy div := || (((absyy*1000)/absxx)) theta := (900000) - ((div*608+500) - (div*div*157+500000)/1000) if ((xx > 0) and (yy > 0)) quadrant := 0 if ((xx > 0) and (yy < 0)) quadrant := 1 if ((xx < 0) and (yy < 0)) quadrant := 2 if ((xx < 0) and (yy > 0)) quadrant := 3 theta := ((theta+500)/1000 + ( 900 * quadrant) + 2700 ) // 3600 return thetaThe floating point routines have significantly more range and also more accuracy which can be very important in GPS applications.·Note: the floating point routines·use ccw angle directions·and radians rather than degrees (as with most math libraries). Radians can easily be converted to degrees for display (the library included functions for conversion).
Also note: the last posted version of CoordsToDist has very limited range (less than 47x47 units) before 32-bit integer overflow occurs. e.g. CoordToDist(0,0,46,46) = 651, but CoordToDist(0,0,47,47) = 111 (overflow). You may want to go back to multiplying by 10 instead of 1000 inside the routine to extend the range.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cam Thompson
Micromega Corporation
You're right about the 2 cogs for ATAN2. The floating point library is organized as 1 cog for the most common functions, but a second cog is required for extra functions including ATAN2. For a custom application it might be possible to cut and paste routines into a single cog, since all of the floating point routines in Float32 and Float32A are written as callable ASM routines, but with only 512 longs per cog if you use lots of functions you eventually need the 2nd cog.
Regards,
Cam
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cam Thompson
Micromega Corporation