Getting arccosine and arctan using CORDIC functions

DiverBob · 2021-03-02 04:18

Using Spin2 has anyone created a functions to get arccosine and arctan values on the P2? With some great help on the forum I’ve now got good examples of the QSin, qcos, polxy, xypol, and rotxy math methods. Between these I was able to calculate tangent also. Still learning about the CORDIC and wanted to see if anyone has already figured out arccosine and arctan for the P2.

Bob

DiverBob · 2021-03-03 01:40

Based on my studies there isn’t any simple way to come up with arccosine. I did find some interesting approaches to solving for an angle over in Stack Overflow website so here are some code examples I found in case anyone else is interested. I have not tried these approaches out or converted them to Spin2 yet (Original code examples is C++).

double acos(x) {
   return (-0.69813170079773212 * x * x - 0.87266462599716477) * x + 1.5707963267948966;
}

The notes indicate it can have an error of up to 10.31 degrees, most likely that is at both ends of the range
This next one is via Nvidia, comments stated it is pretty accurate

float acos(float x) {
  float negate = float(x < 0);
  x = abs(x);
  float ret = -0.0187293;
  ret = ret * x;
  ret = ret + 0.0742610;
  ret = ret * x;
  ret = ret - 0.2121144;
  ret = ret * x;
  ret = ret + 1.5707288;
  ret = ret * sqrt(1.0-x);
  ret = ret - 2 * negate * ret;
  return negate * 3.14159265358979 + ret;
}

Trey Reynolds posted this version:

function acos(x)
    local a=1.43+0.59*x a=(a+(2+2*x)/a)/2
    local b=1.65-1.41*x b=(b+(2-2*x)/b)/2
    local c=0.88-0.77*x c=(c+(2-a)/c)/2
    return (8*(c+(2-a)/c)-(b+(2-2*x)/b))/6
end

I’ll try these out and see how they do. I want to convert everything over so I’m only doing integer math for speed, have to see what that does to accuracy.

Bob

Rayman · 2021-03-03 02:16

The 90's 3d raycast code creates tables at the start. Maybe that's the way to do it. Very fast...

Another way, might be guess and then iteration to improve result. Might be slow, but more precise.
Or, mix table and iteration methods together...

DiverBob · 2021-03-03 02:28

I’ll try out the examples above and see what kind of speed I get out of them. I only need 0.1 degree precision so I may just end up using a lookup table with interpolation. Gives me a goal for tomorrow!

JRoark · 2021-03-03 03:04

Is an expansion of a Taylor series an option?

Electrodude · 2021-03-03 03:18

For arctan, you can use the second return value of XYPOL(x, y), which is equal to atan(y / x). XYPOL is equivalent to what might be expressed in other languages as (hypot(x, y), atan2(y, x)).

ersmith · 2021-03-03 03:23

@DiverBob said:
Using Spin2 has anyone created a functions to get arccosine and arctan values on the P2? With some great help on the forum I’ve now got good examples of the QSin, qcos, polxy, xypol, and rotxy math methods. Between these I was able to calculate tangent also. Still learning about the CORDIC and wanted to see if anyone has already figured out arccosine and arctan for the P2.

Bob

arctan basically is xypol, if you think about it. It's easier to see with the atan2(y, x) function, which finds the angle between point (x, y) and the x axis -- this is exactly what xypol does. arctan(x) is just atan2(y, 1).
arcsin(x) and arccos(x) are similarly derivable from atan2 by looking at the triangles, e.g. arcsin(z) is the angle a such that sin(a) = z, which means it's the angle in the triangle whose height (y coordinate) is z and which has a unit hypotenuse: so it is atan2(z, sqrt(1-z*z)). arccos(z) is the same except it's the x coordinate which is z.

DiverBob · 2021-03-03 03:25

@Electrodude said:
For arctan, you can use the second return value of XYPOL(x, y), which is equal to atan(y / x). XYPOL is equivalent to what might be expressed in other languages as (hypot(x, y), atan2(y, x)).

Thanks, I didn’t realize that was what xypol delivered. I ran some test entries with the method but didn’t stop to figure out what the results represented.

@JRoark said:
Is an expansion of a Taylor series an option?

The Taylor series appears to output the most accurate values but it very costly in cpu cycles. I want to see if what kind of accuracy I can get with some of the faster methods available. Since I only need 0.5 degree accuracy for my robot I should be able to get away with a simpler computation.

ersmith · 2021-03-03 04:46

Here's an implementation of arcsin and arccos in Spin2. Tested with flexspin, but it should compile with Chip's compiler (except maybe you'll have to replace SmartSerial with a different serial object). Change SCALE to get different number of places after the decimal point. For example with SCALE = 1_0000 there are 4 places after the decimal point, so 0.5 is represented as 5000, 0.25 as 2500, 41.3 as 413000, and so on.

'
' arccos and arcsin for Spin2, written by Eric R. Smith
' placed in the public domain
'
obj
  ser: "spin/SmartSerial"
  fmt: "spin/ers_fmt"

'
' everything is done in fixed point, scaled
' by the value here
'
con
  ' SCALE = 1_000   ' value representing 1.0; for 3 digits of precision
  SCALE = 1_0000   ' for 4 digits precision
  ' FULLCIRCLE = 360 * SCALE   ' for degrees
  FULLCIRCLE = trunc(2 * PI * SCALE) ' for radians

pub demo() | x, a, b
  ser.start(63, 62, 0, 230_400)
  send := @ser.tx
  send("Trig test", 13, 10)
  x := trunc(-0.9 * SCALE)
  repeat while x < SCALE
    a := acos(x)
    b := asin(x)
    send("x = ", fmt.dec(x), " acos=", fmt.dec(a), " asin=", fmt.dec(b), 13, 10)
    x += SCALE / 5

'
' input: angle in radians as a 1.4 decimal fixed point number
'                    so a full circle is 2*31416 units

pub acos(x) : r | y, y2, len, angle
  ' calculate x, y such that the vector from (0,0) to (x,y)
  ' makes an angle z with the x axis
  y2 := SCALE - ((x * x) / SCALE)
  y := sqrt(y2 * SCALE)            
  len, angle := xypol(x, y)
  r := angle sca FULLCIRCLE

pub asin(y) : r | x, x2, len, angle
  ' calculate x, y such that the vector from (0,0) to (x,y)
  ' makes an angle z with the x axis
  x2 := SCALE - ((y * y) / SCALE)
  x := sqrt(x2 * SCALE)            
  len, angle := xypol(x, y)
  ' use sar and scas here to make the result signed
  ' in the range -pi .. pi rather than 0..2pi
  ' this makes for a nicer return value for arcsin
  r := (angle sar 2) scas FULLCIRCLE
  debug(sdec(x), sdec(y), uhex(angle), sdec(FULLCIRCLE), sdec(r))

DiverBob · 2021-03-03 13:19

@ersmith said:
Here's an implementation of arcsin and arccos in Spin2. Tested with flexspin, but it should compile with Chip's compiler (except maybe you'll have to replace SmartSerial with a different serial object). Change SCALE to get different number of places after the decimal point. For example with SCALE = 1_0000 there are 4 places after the decimal point, so 0.5 is represented as 5000, 0.25 as 2500, 41.3 as 413000, and so on.

'
' arccos and arcsin for Spin2, written by Eric R. Smith
' placed in the public domain
'
obj
  ser: "spin/SmartSerial"
  fmt: "spin/ers_fmt"
  
'
' everything is done in fixed point, scaled
' by the value here
'
con
  ' SCALE = 1_000   ' value representing 1.0; for 3 digits of precision
  SCALE = 1_0000   ' for 4 digits precision
  ' FULLCIRCLE = 360 * SCALE   ' for degrees
  FULLCIRCLE = trunc(2 * PI * SCALE) ' for radians
  
pub demo() | x, a, b
  ser.start(63, 62, 0, 230_400)
  send := @ser.tx
  send("Trig test", 13, 10)
  x := trunc(-0.9 * SCALE)
  repeat while x < SCALE
    a := acos(x)
    b := asin(x)
    send("x = ", fmt.dec(x), " acos=", fmt.dec(a), " asin=", fmt.dec(b), 13, 10)
    x += SCALE / 5

'
' input: angle in radians as a 1.4 decimal fixed point number
'                    so a full circle is 2*31416 units

pub acos(x) : r | y, y2, len, angle
  ' calculate x, y such that the vector from (0,0) to (x,y)
  ' makes an angle z with the x axis
  y2 := SCALE - ((x * x) / SCALE)
  y := sqrt(y2 * SCALE)            
  len, angle := xypol(x, y)
  r := angle sca FULLCIRCLE

pub asin(y) : r | x, x2, len, angle
  ' calculate x, y such that the vector from (0,0) to (x,y)
  ' makes an angle z with the x axis
  x2 := SCALE - ((y * y) / SCALE)
  x := sqrt(x2 * SCALE)            
  len, angle := xypol(x, y)
  ' use sar and scas here to make the result signed
  ' in the range -pi .. pi rather than 0..2pi
  ' this makes for a nicer return value for arcsin
  r := (angle sar 2) scas FULLCIRCLE
  debug(sdec(x), sdec(y), uhex(angle), sdec(FULLCIRCLE), sdec(r))

That is great Eric! I will go through the math and figure out how you did it. I was looking at xypol yesterday but hadn’t thought about using it that way. This looks much cleaner than some of the interpolations I found yesterday and it uses the built-in CORDIC method. I was talking with Chip and the group at last nights meeting about this and there were a couple of ideas but no real solutions. I can’t wait to try this one out.

DiverBob · 2021-03-03 23:48

Slowly creating a library of SPIN2 trigonometry methods based on integer math with a lot of help from everyone. Eric's asin and acos worked great but somehow I managed to break them when I was working on the atan and atan2 items. Eric's code is good, it's something that I did but I just can't seem to find the error. acos and asin should be returning the original angle value (60 degrees) and it was working correctly for most of the afternoon. I think I'm too close to the problem and can't see the forest for the trees at this point. I've been running this code in the Prop Tool with Debug for the testing output.

Francis Bauer · 2021-03-04 00:58

@DiverBob said:
Slowly creating a library of SPIN2 trigonometry methods based on integer math with a lot of help from everyone. Eric's asin and acos worked great but somehow I managed to break them when I was working on the atan and atan2 items. Eric's code is good, it's something that I did but I just can't seem to find the error. acos and asin should be returning the original angle value (60 degrees) and it was working correctly for most of the afternoon. I think I'm too close to the problem and can't see the forest for the trees at this point. I've been running this code in the Prop Tool with Debug for the testing output.

In the current code that you posted you are feeding the asin and acos routines variables x and y instead of s1 and s2. When I put s1 and s2 into the asin and acos calls, they do indeed return values close to 60 degs.

So the asin and acos routines do appear to work, atan still needs more work...

DiverBob · 2021-03-04 01:40

@"Francis Bauer" said:
In the current code that you posted you are feeding the asin and acos routines variables x and y instead of s1 and s2. When I put s1 and s2 into the asin and acos calls, they do indeed return values close to 60 degs.

So the asin and acos routines do appear to work, atan still needs more work...

Thanks for the fresh set of eyes, don’t know how I missed that.
Atan is still a work in progress, I had been tweaking that when I noticed I’d messed up the asin() and acos(). I’m still deciphering Eric’s comments on how to use xypol() to derive atan and atan2.

Rayman · 2021-03-04 02:36

You can get arctan from arcsin, but you have to do a square root to get there....

DiverBob · 2021-03-04 19:22

@Electrodude said:
For arctan, you can use the second return value of XYPOL(x, y), which is equal to atan(y / x). XYPOL is equivalent to what might be expressed in other languages as (hypot(x, y), atan2(y, x)).

@ersmith said:
arctan basically is xypol, if you think about it. It's easier to see with the atan2(y, x) function, which finds the angle between point (x, y) and the x axis -- this is exactly what xypol does. arctan(x) is just atan2(y, 1).
arcsin(x) and arccos(x) are similarly derivable from atan2 by looking at the triangles, e.g. arcsin(z) is the angle a such that sin(a) = z, which means it's the angle in the triangle whose height (y coordinate) is z and which has a unit hypotenuse: so it is atan2(z, sqrt(1-z*z)). arccos(z) is the same except it's the x coordinate which is z.

I’ve been experimenting with coding for several hours and not getting the expected results. Tan(60) = ~1.732 so atan(1.732) should return ~60 degrees back (when working in DEG vs RAD). Don’t really understand how XYPOL(x,y) fits in here when x and y are position values and I’m supplying tan(angle) value. I tried some trig substitutions but didn’t get good results from that either.

con
  FULLCIRCLE = 360 * SCALE
  SCALE = 1_000

Pub Main() | x, y
  Atan(1_732)

pub atan(x) :r | angle
  Angle := asin(x/sqrt((1+(x*X)))
  r := angle sca FULLCIRCLE

Next problem, XYPOL(x,y) expects a x,y value so it seems to fit right in with the atan2(y,x) definition. Using 3000 and 4000 (using integer math by scaling everything by 1_000) for y, x entered into _,angle := XYPOL(4000,3000) gives the expected result of 53_130 degrees but changing the x value to 4000 so the expected angle is 45_000, however the output was still 53_130. Since both X and y are positive I am only dealing with quadrant 1 at this point.

con
  FULLCIRCLE = 360 * SCALE
  SCALE = 1_000

Pub Main() | x, y
  X := 4000 
  Y := 4000
  Atan2(y, x)

pub atan2(y, x) :r | angle
' get atan2 using xypol()
  _, angle := xypol(x, y)
  r := angle sca FULLCIRCLE

ersmith · 2021-03-04 19:44

When I tried it I got the expected value of 45000 for atan2(4000, 4000). Also, if you have atan2 then you can implement atan(a) as atan2(a, SCALE). Here's my complete program:

CON
  _clkfreq = 180_000_000
  SCALE = 1_000
  FULLCIRCLE = 360 * SCALE

OBJ
    ser: "jm_serial"

PUB demo()
  ser.start(230_400)
  ser.str(string("atan2 demo", 13, 10))
  try(4000, 4000)
  try(4000, 3000)
  try(1_732, 1_000)
  repeat

pub try(y, x) | r
  r := atan2(y, x)
  ser.dec(r)
  ser.str(string(13, 10))


PUB atan2(y, x) : r | angle
  _, angle := xypol(x, y)
  r := angle sca FULLCIRCLE

and here's the output it produces

atan2 demo
45000
53130
59999

DiverBob · 2021-03-05 22:42

Thanks Eric, I had the right formula but after a day’s re-coding I was using the wrong variables coming into the atan2() code so I was getting bad output based on what I was expecting.

DiverBob · 2021-03-05 23:41

Here is a library of Spin2 CORDIC based trig functions (sin, cos, tan, asin, acos, atan, atan2) that use scaling for integer-based calculations. All angle inputs and outputs are in Degrees, not Radians.
There are also a couple of examples of using polxy() and rotxy() included that work but haven't been fully tested for all angle combinations

Rayman · 2023-07-22 16:20

Glad you all figured this out! Turns out I need inverse cosine to have my robot dog calculate its own servo angles.
The above looks perfect for this. Guessing that's why @DiverBob needed it too...

JonnyMac · 2023-07-22 16:50

@DiverBob: Some months ago I was chatting with Chip about math functions and he showed me a couple tricks that you may want to use -- the power() method from Chip always runs in 760 ticks (including call and return) while your looped version (which is where I started, too), is always slower and the size of the exponent will compound that. For projects like Ray's robot this may be important.

pub power(x, y) : result

'' Raise x to the y power
'' -- x and y must be positive

  result := qexp(qlog(x) * y)


pub root(x, y) : result

'' Get the yth root of x
'' -- x and y must be positive

  result := qexp(qlog(x) / y)

evanh · 2023-07-23 02:29

That's good info, Jon, thanks.

I created a related topic as well - https://forums.parallax.com/discussion/173878/qrotate-and-qvector-made-simple/p1

DiverBob · 2023-07-23 03:12

@evanh said:
That's good info, Jon, thanks.

I created a related topic as well - https://forums.parallax.com/discussion/173878/qrotate-and-qvector-made-simple/p1

Your posting helped me a lot in really understanding how the P2 dealt with angles.

@JonnyMac said:
@DiverBob: Some months ago I was chatting with Chip about math functions and he showed me a couple tricks that you may want to use -- the power() method from Chip always runs in 760 ticks (including call and return) while your looped version (which is where I started, too), is always slower and the size of the exponent will compound that. For projects like Ray's robot this may be important.
pub power(x, y) : result

'' Raise x to the y power
'' -- x and y must be positive

  result := qexp(qlog(x) * y)


pub root(x, y) : result

'' Get the yth root of x
'' -- x and y must be positive

  result := qexp(qlog(x) / y)

Jon, as always you (and Chip!) come up with some great insights on P2 coding. Never even looked at qexp() as an option. Thank you for the information, now I get to dig back into the math again! Actually I really needed an excuse to get back programming, been taking a 3 month break from the robot and programming, about time to get moving on it again!

RS_Jim · 2023-07-23 12:12

welcome back Bob!

Rayman · 2023-07-23 20:03

I'm thinking about trying to adjust the functions so they take angles in degrees*scale instead of degrees.
Maybe that would improve accuracy?

Electrodude · 2023-07-23 21:39

@Rayman said:
I'm thinking about trying to adjust the functions so they take angles in degrees*scale instead of degrees.
Maybe that would improve accuracy?

Why don't you use binary radians everywhere? They're the only angle units that don't suffer discontinuities when you go around a whole circle, and they are (for this very reason) already the native units of the P2's CORDIC.

evanh · 2023-07-24 01:06

Binary radian - Never thought to call it that ... looking up Wikipedia, yeah, it's right but, as is the case with many names in computing, it's a repurposed naming.

Getting arccosine and arctan using CORDIC functions

Comments