### Author Topic: More angle wrapping related misery in code  (Read 1546 times)

0 Members and 1 Guest are viewing this topic.

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### More angle wrapping related misery in code
« on: May 30, 2024, 01:15:43 am »
I've got some more angle related troubles with wrapping and subtractions, the usual matters of modulo vs remainder vs ...

I'm trying to make a move of a provided angle, "DeltaConverted", from starting at a present angular value, "angle", and get outputs which tell me not only what the (normalised wrapped) angle will be "MoveToAngle", but also how many turns are done "MoveToCount" (relative to a zero point).

I've been trying in C/C++:
DeltaConverted is an int16_t
angle is a float
MoveToDir a uint8_t, though this isn't yet used in the code shown
MoveToAngle is a float
MoveToCount is an int16_t
Code: [Select]
    float MakeMove=1.0*DeltaConverted+angle;    if(DeltaConverted==0){      MoveToDir=0;    }else if(DeltaConverted<0){      MoveToDir=-1;    }else{      MoveToDir=1;    }    //MakeMove is the angle, non normalised, we will end up at, this far it logically works (inevitably)    MoveToAngle=Clamp360(MakeMove);//this bit works too    MoveToCount=(int16_t)(fabs(MakeMove))/360;//here's where the trouble begins    if(MakeMove<0){      MoveToCount=0-MoveToCount;      MoveToCount=MoveToCount-1;    }

Note that I define:

Code: [Select]
int16_t Clamp360(int16_t value){  value= value % 360;  if(value >= 0){    return value;  }else{    return 360 - (0 - value);  }}
which I know does work well.

But once this code has run whilst the MoveToAngle value correctly gves where in the normalised circle we'll be when the move is done, MoveToCount does not always correctly tell us how many whole turns we've done (as versus the zero point).

Namely, problems happen when the position specified by MakeMove is negative.

I thought the

if(MakeMove<0)

part would solve this

but I get weird effects then when getting to -2 turns.

For example, for positive MakeMove values it works.
MakeMove=359, gives 359 as the angle and 0 counts
MakeMove=360 gives 0 as angle and 1 counts
MakeMove=361 gives 1 for angle and 1 counts

it keeps working for higher MakeMove values just fine.

It works too when
MakeMove=1, gives angle=1, count=0
MakeMove=0, gives angle=0, count=0
MakeMove=-1, gives angle=359,count=-1

but then for MakeMove=-360 and down things get odd
MakeMove=-359 gives angle=1, -1 counts, fine so far
MakeMove=-360 gives angle=0, -2 counts, trouble!
MakeMove=-361 gives angle=359, -2 counts, back to behaving itself

trouble comes again at -720
MakeMove=-719 gives angle=1, counts=-2, fine
MakeMove=-720 gives angle=0, counts=-3, trouble!
MakeMove=-721 gives angle=359, counts -3, fine again

I've tried adding extra conditions around the zero and negative MakeMove cases but I can't seem to fix this, not without causing other troubles instead, like not getting MoveToCount values less than 0 ever appearing.

Again, like by other angle wrapping issues thread a while back, it should be simple, but I seem to have some serious problem when angle wrapping comes up, somehow its so much harder to hold in my head than anything else so really tough to try to think through the lines the way one can with code that obeys normal non-wrapping mathematical laws.

How do I fix this?

I tried searching online for this, but couldn't find the right key words so was only bringing up things about how to do the Clamp360 stuff and the normalisation itself, I couldn't think of the right keywords to bring results about counting full revolutions properly.

Thanks

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #1 on: May 30, 2024, 11:29:32 am »
To count the number of times angle has wrapped, copying the sign of the direction of the wrapping, the same as
Code: [Select]
    float angle;    int16_t wraps = 0;    while (angle >= 360.0f) {        wraps++;        angle -= 360.0f;    }    while (angle < 0.0f) {        wraps--;        angle += 360.0f;    }use
Code: [Select]
    wraps = (int16_t)floorf(angle / 360.0f);    angle -= (float)wraps * 360.0f;This is because floorf(1.0f) == 1.0f, floorf(0.5f) == 0.0f, floorf(0.0f) == 0.0f, floorf(-0.5f) == -1.0f, and floorf(-1.0f) == -1.0ffloor()/floorf() rounds towards negative infinity: it returns the largest integer not greater than the argument.

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### Re: More angle wrapping related misery in code
« Reply #2 on: May 30, 2024, 01:51:37 pm »
I'm getting the horrid feeling I'm having enough angle wrapping problems I'm going to need to refactor the code.

Are there any web resources with examples for all possible use cases of angle operations in C (or equivalent) shown?

Both dealing with wrapping, and comparing angles in the wrapped frame against angles in an unwrapped frame.

I'm getting situations where I need to compare angles derived from trig in "this frame" (between 0 and 360) against angles referenced to a true zero, where I can't normalise because I need to keep the information associated with the absolute number of revolutions.

I've got situations where I need to take a motor shaft with an angle recorded in an absolute frame, then position it to match with already normalised angles within whichever number of rotations it has already done. All while keeping track of the absolute frame position to return to later.

I'm trying to weigh up handling the number of revolutions in a separate variable from the actual angle, but this seems to make it only more confusing to assess whether a pair of angles are say 70 degrees apart vs 430 degrees apart, vs -1370 degrees apart...

If there's a good and complete guide of examples for writing heavily angle focused code, it would be really helpful to see.

Thanks

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #3 on: May 30, 2024, 04:06:27 pm »
If you used binary angular measurement, i.e. 2B = 360°, everything would be much easier.

In the general case, it all boils down to understanding and leveraging the four operations:
• roundf()/round(): Round to nearest integer, halfway cases away from zero.
• truncf()/trunc(): Truncate, AKA round to nearest integer not greater than the argument in magnitude, AKA round towards zero.  This is what casting to an integer type does in C.
• floorf()/floor(): Round to nearest integer not greater than the argument, AKA round towards negative infinity, AKA floor.
• ceilf()/ceil(): Round to nearest integer not smaller than the argument, AKA round towards positive infinity, AKA ceiling.
Here is an illustrative table:
Code: [Select]
     x │ -2.00 -1.75 -1.50 -1.25 -1.00 -0.75 -0.50 -0.25  0.00  0.25  0.50  0.75  1.00  1.25  1.50  1.75  2.00───────┼─────────────────────────────────────────────────────────────────────────────────────────────────────── trunc │   -2    -1    -1    -1    -1    -0    -0    -0     0     0     0     0     1     1     1     1     2  round │   -2    -2    -2    -1    -1    -1    -1    -0     0     0     1     1     1     1     2     2     2  floor │   -2    -2    -2    -2    -1    -1    -1    -1     0     0     0     0     1     1     1     1     2   ceil │   -2    -1    -1    -1    -1    -0    -0    -0     0     1     1     1     1     2     2     2     2 Note how only floor(x/N) and ceil(x/N) have exactly N zeros ending/starting at zero?
And how truncation gives you a double-wide zero zone?  And rounding the narrowest zero zone?

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### Re: More angle wrapping related misery in code
« Reply #4 on: May 30, 2024, 10:35:56 pm »
Are there guides anywhere about properly getting the natural feel for which of the different rounding operations I need in each given scenario when doing wrapped angle calculations?

Also, using the overflow of an integer variable to enforce angle wrapping with binary angular measurement still doesn't solve problems where I need to compare normalised angles against things like running totals of the full amount of angle accumulated over multiple motions over time?

Given this surely crops up a lot in contexts like when people build stepper/servo systems which can rotate an infinite number of times and must not misbehave on any angle wrap, or even total count wrap, events... surely there should be some good examples online for getting the feel in full for wrapped and unwrapped angle handling code? Does anyone know of any?

It's easy to know how to do the angle calculations manually in any given single situation, but devising code to handle all the possible circumstances involved in angle wrapping is such a headache. Its like why they invented atan2 as a way to avoid having to write out all the different conditions involved in atan when doing all four quadrants and with zeroes possible for x or y, but more headache-causing... why isn't there a generalised solution to many-turn angle wrapping in common use?

Thanks
« Last Edit: May 30, 2024, 10:45:27 pm by Infraviolet »

#### Siwastaja

• Super Contributor
• Posts: 8307
• Country:
##### Re: More angle wrapping related misery in code
« Reply #5 on: May 31, 2024, 05:47:11 am »
Just rewrite to use binary angular measurements, like you were advised to in the previous thread:
https://www.eevblog.com/forum/programming/angle-wrapping-in-code-easy-problem-need-to-double-check/

You can dedicate some number of bits for "full turns". For example, if you use int64_t to store the angle, you can use 32 highest bits to denote turn number (2 billion in each direction), and lowest 32 bits to denote the angle.

For easy access, you can do stuff like
typedef union
{
struct { uint32_t angle; int32_t full_turn_number; };
int64_t both;
} angle_type;

The following users thanked this post: bpiphany

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### Re: More angle wrapping related misery in code
« Reply #6 on: June 11, 2024, 09:59:18 pm »
Ok, I've been trying to use binary angle measurements, but I've found two reasons that it doesn't work well.

1. I need to add and subtract in very minor increments sometimes, these don't get included in a non-float variable. I don't need to output the angle to a better precision than 1/256th of a revolution ever, but I need to store it at much higher precision so when a fraction of a fraction of a "degree" gets added time after time, eventually it does change the integer value. I tried place shifting to use 2 bytes for the angle itself, but it became problematic.

2. My system has certain properties where division of the angle by other numbers, particularly the number 7 as this is a 7 pole pair motor I'm tracking and controlling the position of, needs to be preserved appropriately across wraps. Otherwise when the angle wrapped over I could have tracked the electrical rotations properly, but not the mechanical ones. Divison by prime numbers like 7 isn't going to go well with binary integer variables.

My thoughts then are that I should be designing some special class of variable where angular wrapping is automatically handled, something where rather than using normal maths functions on the variablels, everything is handled via specialised functions which respect and preserve angle wrapping. But I'm unsure how to go about this, can anyone recommend a method? I'm just so surprised there aren;t existing pieces of example source code for a problem like this which must be very common.

EDIT: I've realised that floats are a bad idea again, as the precisions can change when the size of the number changes. So I'm going to try going back to binary, but I know the division by 7 and then need to have the largest scale wrapping occur in a place which preserved divisibility by 7 will be a problem. Is there a way to make a customised class variable type for this scenario? So it will wrap exactly where I want it to, and respect this wrap automatically when doing addition and subtraction?
« Last Edit: June 11, 2024, 10:32:15 pm by Infraviolet »

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #7 on: June 12, 2024, 12:45:06 am »
You can do this by using a mixed fixed point integer format to describe turns.
The upper part describes the number of turns in normal fashion, but the fractional part is a multiple of a suitable radix.

We know your radix should be a multiple of 7, because you want to describe values 1/7, 2/7, etc. exactly.

How many full rotations of range you need, and how large integer types are you willing to use?
What numbers (besides 7) does your fractional radix need to be divisible by?

(We still use radix 60 for minutes and seconds, because 60 = 2·2·3·5 = 3·4·5; it is divisible by 2, 3, 4, 5, 6, 10, 12, 15, 20, and 30.)

Nicest fractional radixes for you would be 63 (=7·9), 511 (=7·73), 4095 (=7·585), and 32767 (=7·4681), because these are just one less than a power of two.

In general, if we have two numbers a = ai+ar/R and b = bi+br/R, R being the fractional radix, then their sum is
a+b = ai+bi + (ar+br)/R
and difference is
a-b = ai-bi + (ar-br)/R
and product is
a·b = ai·bi + (ai·br + bi·ar)/R + (ar·br)/R²
and the final term, with divisor R², only affects rounding: if ar·br ≥ R²/2, then the result is rounded up.

Multiplying by an integer is simple,
a·k = ai·k + ar·k/R
as is converting a ratio of integers a = k/m assuming 0 ≤ k < m:
ai = quotient(k/m), ar = R·remainder(k/m)/m

Dividing by an integer is relatively simple,
b = a/k = ai/k + ar/(k·R)
which ends up being calculated as
q = quotient(ai / k), r = remainder(ai / k)
bi = q, br = round((ar + r·R)/k)
with signs handled separately.

Full division is annoying,
c = a/b = (R·ai+ar) / (R·bi+br)
which ends up being calculated as
q = quotient((R·ai+ar) / (R·bi+br)), r = remainder((R·ai+ar) / (R·bi+br))
ci = q, cr = R·r / (R·bi+br)
with signs handled separately.

For the divide-by R terms, you do need to carry overflow to the integer part.

For example, if you were to use 16-bit integers and a fractional radix of 4095, each such variable could represent 0 .. 16-1/4095 turns or -8 .. +8-1/4095 turns, inclusive, at a resolution of 1/4095 turn = 0.000244200244200... turn.  For addition and subtraction of such rotation values you could use
Code: [Select]
// t16_add(t1, t2) = t1 + t2uint16_t  t16_add(const uint16_t  t1, const uint16_t  t2) {    const uint16_t  t = t1 + t2;    return t + ((t & 4095) == 4095);}// t16_sub(t1, t2) = t1 - t2uint16_t  t16_sub(const uint16_t  t1, const uint16_t  t2) {    const uint16_t  t = t1 - t2;    return t + ((t & 4095) == 4095);}// Normalize fractional partuint16_t  t16_norm(const uint16_t  t) {    return t + ((t & 4095) == 4095);}which also work as-is for signed 16-bit integers, int16_t, if the compiler uses the normal unsigned-to-signed wrapping rules (same as signed-to-unsigned, i.e. adding or substracting 65536 from the value to be converted if necessary).

In general, the normalization for M = 2ⁿ-1, R = 2ⁿ-k is
return t + ((t & M) >= R) ? k : 0;
but when k==1, M = R, and the above simplifies to
return t + ((t & R) == R);
because ((t & R) == R ? 1 : 0) is equivalent to ((t & R) == R).

Also, when R = 2n-1, (K*R) simplifies to ((K<<n) - K), i.e. left shift by the number of bits in R, and one subtraction.
« Last Edit: June 12, 2024, 12:52:38 am by Nominal Animal »

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### Re: More angle wrapping related misery in code
« Reply #8 on: June 12, 2024, 02:50:16 am »
Another point on this, when working with uint8_t variables, how do I now find an absolute difference in a wrap respecting way? If I've got a 0 to "256" (with 256 going back to 0) angular measure, and want to find how far, in either direction, away from it another uint8_t angular measure s, what's the right operation? I think it is something to do with casting, but I need to make it handle the bits in a raw fashion rather than mathematically as one would for normal numbers? And the compiler usually sets things up to work for normal numbers.

When I was using floats I had expressions like:
if( Clamp180(Expected - Actual) >= 90 ){

what does the equivalent become for uint8_t?

« Last Edit: June 12, 2024, 02:59:10 am by Infraviolet »

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #9 on: June 12, 2024, 03:05:25 am »
Another point on this, when working with uint8_t variables, how do I now find an absolute difference in a wrap respecting way? If I've got a 0 to "256" (with 256 going back to 0) angular measure, and want to find how far, in either direction, away from it another uint8_t angular measure s, what's the right operation?
From uint8_t a to uint8_t b, there are two ways.  First is
(int8_t)(b - a)
which assumes unsigned to signed conversion for uint8_t types uses modulo 256 arithmetic like all current hardware architectures have; the second is
((a < b) ? (int8_t)(b - a) : -(int8_t)(a - b))
which yields the exact same values without relying on the modulo arithmetic.

Now, care to answer the two questions in boldface in #7?

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### Re: More angle wrapping related misery in code
« Reply #10 on: June 12, 2024, 03:22:19 am »
Here's my best attempt at the boldface questions:
I've been thinking a bit more, I'm going to list out what I think I need from the code I have to write, maybe this would enable people to suggest what to do:

1. I need some sort of representation of a circle dividable in to 256 or so (or 360) parts which can be used to actually set angles, and fed in to sine like functions. Other than for incrementing and decremtning I never need more accuracy than this.
2. I need this representation to be able to hold within it "decimal" parts which can be incremented and decremented by about 1/16th of the divisble units in requirement 1. At present I'm using the bottom 4 bits of an int32_t for this purpose, the next 8 bits as the x/256 angle, and the higher bits as a revs count.
3. I need the revs count, when it eventually wraps over, to do so at a multiple of 7 revolutions. And also preserivng wrapping for 252 (7*6*6). There are no other divisions which need preserving across a wraparound, just position within the 0-256 angle, how many 7s of revs and how many 252s of revs.
4. When that wrap happens I need both the angle within a revolution, and the total/7 and the toal/252 parts of the revolutions count, to be continuous
5. I never need to add or subtract more than 10000 (approx) from the revolutions count at once. I want all subtractions and additions to work during a crossing of the wrap-over boundary. I am not ok with solutions which put the wrap-over boundary up in the millions or billions of revolutions, but don't protect against weirdness when it occurs. I don't mind a relatively low wrapping point but I want everything to behave normally across it.
6. I need a function which will tell me how great the lead, or lag, between two angles in the 0-256 range range is, and which behaves normally on wraparound, the angles being no more than 2 revolutions apart
7.I need a function which lets me know if a given angle is between two others, between them on the short side, when all are being interpreted within a single revolution
8.I need a function which finds and tells me the mid-way point between two angles, going the short way between them, the angles being not more than a single full revolution apart
9.If one angle is behind or ahead of another by as much as 10000 revolutions I also need to be able to find how many of the "256ths" "degrees" are between the
10.In some situations need to be able to store the revolutions part, and the 7 revolutions part, away safely, normalise the value in to the 0-256 range, then make multiple revolutions within this reference frame, before later restoring the reference frame back to the main one which I started from.

The best I've got so far, before trying to understand radixes, is an int32_t with the bottom 4 bits as the bit that lets me accumulate small changes, the next 8 bits being >>4 ed and & ed with 0xff when I want the angle within a revolution, and the rest of the bits being a revolutions count. This, so far, has no means to deal with the preserving of 7 revolutions when wrapping. and I keep confusing myself when trying to implement the angle difference finding and averaging (I don't want to resort to trig there, it is too slow for code running fast on a weak microcontroller) to get the mid-way points.

Do these requreiments suggest how I should be setting up the code? It seems every change I make to appease one requirement makes the others more tricky to achieve.

I know my descriptions here aren't the best, but there's something about this problem I just don't seem to be getting, and I can't quite follow what.

Thanks
« Last Edit: June 12, 2024, 03:39:10 am by Infraviolet »

#### IanB

• Super Contributor
• Posts: 12015
• Country:
##### Re: More angle wrapping related misery in code
« Reply #11 on: June 12, 2024, 03:45:48 am »
OK, you seem to have greatly expanded your needs since your first post, but I have a feeling you are making things too complicated, and that's where you are getting into trouble.

In your first post you wrote "C/C++", but this is not a good thing to say, since "C" and "C++" are different languages.

Since you mentioned C++, I will choose that one and give you some simple example code below that may help.

Firstly, I will use C++ to advantage, and define a new type to represent angular position.

Secondly, I will avoid all use of mathematical functions, and will avoid all considerations of integer overflow or wrap-around.

The example below is very bare bones, and not at all polished, but it may give you an idea of how to proceed:

Code: [Select]
#include <iostream>using namespace std;struct Angle{    int m_revolutions; // number of complete revolutions made from zero position    int m_angle;       // current angle of rotation between 0 and 359 degrees in positive direction    // default constructor initializes angle to zero    Angle() : m_revolutions(0), m_angle(0) {}    void Advance(int degrees)    {        // advance position by number of degrees in positive direction        m_angle += degrees;        while (m_angle > 359)        {            m_angle -= 360;            ++m_revolutions;        }        while (m_angle < 0)        {            m_angle += 360;            --m_revolutions;        }    }    void Retard(int degrees)    {        // retard position by number of degrees in negative direction        Advance(-degrees);    }    static int Difference(Angle angle1, Angle angle2)    {        // find the difference angle1 - angle2 between two positions in degrees        return (angle1.m_revolutions * 360 + angle1.m_angle) - (angle2.m_revolutions * 360 + angle2.m_angle);    }};void PrintAngle(Angle angle){    const char* plural = (angle.m_revolutions == 1 || angle.m_revolutions == -1) ? "" : "s";    cout << "Current position is " << angle.m_revolutions << " complete revolution" << plural << " from zero plus " << angle.m_angle << " degrees in forward direction" << endl;}int main(){    // start at zero    Angle angle;    PrintAngle(angle);    // advance by 205 degrees    angle.Advance(205);    PrintAngle(angle);        // advance by 278 degrees    angle.Advance(278);    PrintAngle(angle);    // remember this angle    Angle angle2 = angle;    // go back by 409 degrees    angle.Retard(409);    PrintAngle(angle);    // go back 134 degrees    angle.Retard(134);    PrintAngle(angle);    // find difference between angle and angle2    int diff = Angle::Difference(angle2, angle);    cout << "Difference between two positions is " << diff << " degrees." << endl;    return 0;}
Code: [Select]
Current position is 0 complete revolutions from zero plus 0 degrees in forward directionCurrent position is 0 complete revolutions from zero plus 205 degrees in forward directionCurrent position is 1 complete revolution from zero plus 123 degrees in forward directionCurrent position is 0 complete revolutions from zero plus 74 degrees in forward directionCurrent position is -1 complete revolution from zero plus 300 degrees in forward directionDifference between two positions is 543 degrees.
« Last Edit: June 12, 2024, 03:48:27 am by IanB »

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### Re: More angle wrapping related misery in code
« Reply #12 on: June 13, 2024, 03:10:00 am »
Can someone check over the following functions for me and advise whether they should be utterly reliable when angle wrapping is considered? I tested them by running some loops with all the possible input values to each, but naturally those loops gave rather more results than I could examine easily for correctness.

Code: [Select]
int8_t AngleDiff(uint8_t In2, uint8_t In1){  int8_t Diff= ( ( In1 < In2 ) ? (int8_t)(In2 - In1) : -(int8_t)(In1-In2));  return Diff; }AngleDiff(x,y) should give the shortest distance between the two angles x and y, signed to indicate which angle is "greater" ("greater" being somewhat modified to respect wrapping). The input angles are supplied in single byte binary angular measurement form, 256=360 degrees.

Code: [Select]
uint8_t AngleAverage(uint8_t AngleIn1, uint8_t AngleIn2){//works, but results may be jumpy when the two input angles are 180 degrees, or nearly 180, apart (128/256)  int8_t Difference = AngleDiff(AngleIn1,AngleIn2);  uint8_t Out=0;  int8_t HalfDiff=abs(Difference >> 1);  if(Difference>0){    Out=AngleIn2 + HalfDiff; //to the smaller angle add half the difference  }else{    Out=AngleIn1 + HalfDiff; //to the smaller angle add half the difference  }  return (uint8_t) (Out & 0xff);}AngleAverage(x,y) should always give an output angle mid-way between the two supplied angles, again binary angular measurement is used.

Code: [Select]
bool CheckBetween(uint8_t testingVal, uint8_t LowerLimit, uint8_t UpperLimit){//will not work for LowerLimit==UpperLimit  if(LowerLimit == UpperLimit){    return 0;  }    if(AngleDiff(UpperLimit,LowerLimit) > 0 ){    if(AngleDiff(testingVal,LowerLimit) >=0 && AngleDiff(UpperLimit,testingVal)>=0){      return 1;    }else{      return 0;    }  }else{    if(AngleDiff(testingVal,UpperLimit) >=0 && AngleDiff(LowerLimit,testingVal)>=0){      return 1;    }else{      return 0;    }  }}CheckBetween is supposed to recognise whether a supplied angle to be tested is within the angle between the two limit angles, on the short side between them. This is not required to work accurately where both limits are close to 180 degrees apart (difference of -128, -127 or 127 or 128), all practical uses of this will have limits somewhere between 10/256 and 110/256 apart.

Should the three of these work correctly with wrapping in all circumstances?

....

My next problem seems to be comparing binary angular measures using int32_t variables. I need to produce something similar to AngleDiff, but able to account for situations where the two supplied angles are multiple revolutions apart. I'm presently using an int32_t where the bottom 4 bits are sub-"degree" angles for incrementing/decrementing, the next 8 bits are the angle out of 256, then above that the bits count revolutions in a signed manner. But this is where wrapping over with multiples of 7 and 252 begins to matter. When I want to feed the angular parts of these in to the functions above I can just do (Int32TAngleVariable >> 4) & 0xff . (Int32TAngleVariable >> 12) doesn't work well to show the revolutions count, due to the signing bit at the start of an int32_t.

Somehow I need to be able to accurately compare, for differences of multiple revolutions plus 256th parts of a single revolution, the angle I want to move to and the angle I am at. And have these both change together in the correct way if I cross a wraparound boundary, making sure also to place this boundary at a multiple of 252 revolutions. In initial testing I'm working with just a multiple of 7 revolutions, this makes early testing easier as I can get to the error conditions without having to turn for ages first, which I'll change to a multiple of 252 later on. My thoughts are though that (int32_t)25200 << 12 might make a good positive value to wrap at, and the negative equivalent of this a good bottom end of the range?

Thanks

« Last Edit: June 13, 2024, 03:13:55 am by Infraviolet »

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #13 on: June 14, 2024, 03:11:37 am »
If you want a cyclic integer type with BITS bits (minimum 2), set constants
HALF = 1 << (BITS - 1);
MASK = HALF - 1;
noting that (HALF | MASK) has all low BITS set.

Then, the signed distance from from to to is
((to - from) & MASK) - ((to - from) & HALF)
and will always evaluate to a value in -HALF....MASK, inclusive.

I suggest writing it using a suitable int_fastN_t type (N=8, 16, 32, 64, whatever can represent -HALF), and form
int_fastN_t  d = to - from;
return (d & MASK) - (d & HALF);
for the signed distance.

For example, with BITS=4, HALF=8, MASK=7:
• Distance from 0 to 7 is +7
• Distance from 7 to 0 is -7
• Distance from 0 to 8 is -8, and distance from 8 to 0 is -8, because halfway is always negative
• Distance from 15 to 0 is +1
• Distance from 0 to 15 is -1
One can use standard operations like addition, subtraction, multiplication and division with such values using intN_t or int_fastN_t types, as long as N > BITS.

When N == BITS, (intN_t)(to - from) is the signed distance exactly.

The unsigned distance can be written as
int_fastN_t  d = to - from;
return (d & HALF) ? half - (d & MASK) : (d & MASK);
which will always return a nonnegative distance in 0..HALF, inclusive.

The test for from <= to (but correctly handling the wrapping) can be written as
!((to - from) & HALF)

Testing whether c is between a and b (inclusive, correctly handling the wrapping), can be written as
int_fastN_t  ac = (c - a) & (MASK | HALF);
int_fastN_t  cb = (b - c) & (MASK | HALF);
int_fastN_t  ab = (b - a) & HALF;
return (!ac) || (!cb) || (ab == (ac & HALF) && ab == (cb & HALF));
which is equivalent to testing that the order of c,a and b,c are the same as b,a.  The (!ac) tests for equality on the lower bound, and (!cb) for equality on the upper bound.

Midway between a and b is easiest to calculate as
(a + signed_distance(a, b) / 2) & (MASK | HALF);

In general, interpolating between a and b via n/N, N > 0, can be done via
(a + (signed_distance(a, b) * n) / N)) & (MASK | HALF);
if your intermediate type has sufficient range (enough bits for the temporary product).  It yields a when n=0, and b when n=N.

If you have two unsigned integer variables from and to in 0..FULL-1, where FULL is the size of the full range (and not a power of two because for those you want to use the above instead), 2·HALF=FULL, then the signed distance from from to to can be calculated using
int_fastN_t  d = to - from;
return (d >  HALF) ? d - FULL :
(d < -HALF) ? d + FULL : d;
The distance from FULL-1 to 0 is +1, and from 0 to FULL-1 is -1; this too wraps correctly.  Assuming inputs are within valid range, the returned value is in -HALF...+HALF, inclusive.

Unsigned distance is just the absolute value of the above, for example
int_fastN_t  d = to - from;
return (d >  HALF) ? FULL - d :
(d < -HALF) ? FULL + d :
(d <     0) ?       -d : d;

For from <= to, you can use
int_fastN_t  d = to - from;
return (d < -HALF) || (d >= 0 && d <= HALF));

Midway between a and b can be calculated as
int_fastN_t  d = a + signed_distance(a, b)/2;
return (d < 0) ? d + FULL : (d >= FULL) ? d - FULL : d;
Linear interpolation from n=0 at a to n=N at b is similarly
int_fastN_t  d = a + (signed_distance(a, b) * n) / N;
return (d < 0) ? d + FULL : (d >= FULL) ? d - FULL : d;

This expression yields 0 if from == to, 1 if from < to, and 2 if from > to.  I call it compare(from, to):
int_fastN_t  d = to - from;
return (d != 0) + (d >= HALF) + (d > -HALF && d < 0);
With that, it is easy to check if c is between a and b, assuming they are all in 0..FULL-1:
return ((compare(a, c) | compare(c, b) | compare(a, b)) != 3);
Essentially, it verifies that c-a and b-c are in the same direction as b-a.

It is possible to combine these two, using low bits for the binary (fractional or angle) part, and upper bits for an arbitrary cyclic range, by simply shifting FULL and HALF left by fractional bits for the combined arbitrary range.  (That is, your arbitrary range will have FULL and HALF with at least BITS least significant bits zero.)

To simplify things, one could use full 16 bits for the fractional/angle part (for use with uint16_t or uint_fast16_t), and the leftover 14 bits or so for the integral angle.  We need a couple of extra bits, to make sure the integral part can represent values in -FULL..2*FULL-1, inclusive.  The angular precision is then 360°/65536 = 45°/8192 = 0.0054931640625°, or 10125/512 = 19.775390625 arcseconds.  For sines and cosines, you need a quarter wave, which here would be split into 16384 steps.

A suitable value for the integral number of turns would be 16380 = 65·252 = 2340·7.  On architectures with an unsigned 32-bit by 32-bit multiplication returning the upper 32 bits, compilers can optimize "(uint32_t)(x) % 16380" and "(uint16_t)(x) % 16380" to one multiplication, some bit shifts and a couple of additions, because it is sufficiently close to a power of two (2¹⁴ = 16384).  On such architectures (Cortex-M3/M4/M7, RISC-V rv32, x86, x86-64, ARM64/AARCH64), using uint32_t as the combined type, enforcing wrapping after operations that modify the full turns part via combined %= 1073479680; (1073479680 = 16380·65536) generates just one multiplication, some bit shifts, and a couple of additions and subtractions.

#### Siwastaja

• Super Contributor
• Posts: 8307
• Country:
##### Re: More angle wrapping related misery in code
« Reply #14 on: June 14, 2024, 08:39:35 am »
1. I need to add and subtract in very minor increments sometimes, these don't get included in a non-float variable. I don't need to output the angle to a better precision than 1/256th of a revolution ever, but I need to store it at much higher precision so when a fraction of a fraction of a "degree" gets added time after time, eventually it does change the integer value. I tried place shifting to use 2 bytes for the angle itself, but it became problematic.

Which is exactly why you should use binary angles. 64-bit (32+32) binary angle will always have the range of -2 to +2 billion full turns and resolution of 0.000000083819 degrees, which I'm 99% sure is good enough for your use case. A double consumes the same amount of memory (that is, 8 bytes), and has more (excessive?) initial resolution but then after some number of full turns the resolution diminishes in an unexpected and weird way.

Quote
2. My system has certain properties where division of the angle by other numbers, particularly the number 7 as this is a 7 pole pair motor I'm tracking and controlling the position of,

Sounds weird. I have controlled numerous motors of 2 or 3 or 7 or whatever pole pairs, and never found any reason to divide anything in run-time by the pole pair number. Maybe something to do with commutation of the motor and calculating the segment, but then again an electrical revolution could be for example 6 sectors, and as such the 7-pole pair motor would have 6*7=42, not 7 sectors.

In any case, I don't understand why basic operations like division, subtraction, addition would be any different. Well of course except they don't need extra wrapping logic, which is the whole point.

To ignore full turns and just divide the offset angle within the turn, just mask out the full turn bits, for example by using the helper union I showed in an earlier reply. Then just divide. The operator for integer division is /, in both C and C++.

Quote
Divison by prime numbers like 7 isn't going to go well with binary integer variables.

Why do you think so? Dividing integers by integers is one of the most fundamental operations a computer does. Sure, it internally does not round, but that is made insignificant by using enough resolution to begin with, so that "off-by-half" errors do not matter.

You don't need any classes or abstraction in any of this. Everything should fit within a few lines of code. If not, you are doing something seriously wrong.
« Last Edit: June 14, 2024, 08:42:23 am by Siwastaja »

#### SiliconWizard

• Super Contributor
• Posts: 14893
• Country:
##### Re: More angle wrapping related misery in code
« Reply #15 on: June 14, 2024, 08:23:11 pm »
All this is relatively "basic" fixed-point, and this is IMO an essential skill to have for embedded programming.

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #16 on: June 14, 2024, 10:51:48 pm »
You don't need any classes or abstraction in any of this. Everything should fit within a few lines of code. If not, you are doing something seriously wrong.
I agree, especially when using supported exact-size types (uintN_t, intN_t).

All this is relatively "basic" fixed-point, and this is IMO an essential skill to have for embedded programming.
I agree.

Let me summarize.

Assuming a sane C/C++ compiler that performs arithmetic bit shifts on negative values, and applies the same modulo logic to signed exact-size integer types as to unsigned ones, the one "trick" you do want to use is to use an unsigned exact-size integer to hold your cyclic values.

That way, the signed distance from uintN_t a to uintN_t b is exactly (intN_t)(b-a).
That cast back to uintN_t, or directly calculating (uintN_t)(b-a), yields the distance in the positive direction.  Its most significant bit will be set, if the positive distance is longer than the negative distance.

This also means that uintN_t c is between a and b if and only if the most significant bits of (uintN_t)(b-a), (uintN_t)(c-a), and (uintN_t)(b-c) are all the same, either all set or all clear.  Logically it means the signed distances between a,b, a,c, and b,c have the same sign, ie. shortest when measured in the same direction.

When you use smaller than exact-size integer types, the first part of reply #13 shows how to do the sign extension correctly.  While it looks like extra work, it really just boils down to the fact that in two's complement notation –– which intN_t must use per the C standard ––, the highest bit has value -2N-1, compared to +2N-1 for uintN_t.  All other bits have positive logical values, +2k at bit position k, when k=0 for the least significant integral bit, up to k=N-2 being the second-most significant bit.

(For example, in int8_t, 33 = 32+1 = 0b00100001, and -95 = -128+32+1 = 0b10100001.  To negate a value, you toggle/invert all bits ("complement") and add +1 +2; this is where the "two's complement" name comes from.  (Edit: It doesn't; see SiliconWizards response below.)  I've found the equivalent "highest bit value is negative" more useful in practice, though.

Non-power-of-two periodicity is less often needed, but boils down the same logic.  There are many ways to implement it, but the second part of reply #13 shows one way.  This differs from the exact-size integer stuff above in that for these, you can use either a signed or an unsigned base integer type.  Unsigned % is faster (with a fixed modulus (or "range" as I call it) more easily optimizes to a multiplication and additions/subtractions), but detecting underflow (at e.g. subtraction) is easier with signed types.  In practice, you should use a fixed-size integer type, signed or unsigned, and explicitly cast the sum/product to a suitable fixed-size signed or unsigned integer type as needed.

In addition to angles, these are also needed to access cyclic buffers, and when accessing e.g. hash tables.  When the index can only overflow, it is very easy to not realize that the if (index >= size) index -= size; applies the same modular arithmetic to the array index.  When the index is only ever incremented by one, it can even be written as if (index == size) index = 0;.  For hash tables, you often see index = hash % size, but it does require that hash is nonnegative (preferably a fixed-size unsigned integer type), and size to be positive (and not zero).

When you aren't sure how to implement the cyclicity/modularity scheme you want, create a table.  Label each row with each possible from value, and each column with a possible to value, and their difference in the table cell.  You'll immediately see the structure, and if you have constructed such a table for each of the schemes you might need, you'll immediately recognize it.  For example, signed distances and midway positions modulo 7 are
On the left, zeros at diagonal have gray background, midway distances light gray, negative distances red, and positive distances blue.
« Last Edit: June 15, 2024, 01:00:11 am by Nominal Animal »

#### SiliconWizard

• Super Contributor
• Posts: 14893
• Country:
##### Re: More angle wrapping related misery in code
« Reply #17 on: June 15, 2024, 12:11:02 am »
Great, but allow me to just discuss the following point:

To negate a value, you toggle/invert all bits ("complement") and add +2; this is where the "two's complement" name comes from.

The two's complement is actually negating the value bitwise (flipping all bits) and then adding *one*, not two. The "two" in two's complement doesn't come from adding two to the complemented value, but from the fact that the sum of the original number and the two's complemented number is 2^N (where N is the width of the original and complemented numbers), but when only considering N bits (so modulo 2^N), the sum yields zero, which is arithmetically consistent [ x + (-x) = 0 (mod 2^N) ].

Likewise, the one's complement is just the negated value (all bits flipped). The reason for the naming is that the sum of the original number and complemented number, is, in this case, all ones. So yes, those terms of one's and two's complements are more "illustrative" than coming from a strict single definition.

The two's complement is actually very simple to understand - since the sum of the bitwise negated number and the number itself is always made of all ones, if you add 1 to the sum, you get zero (mod 2^N).

The following users thanked this post: Nominal Animal

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #18 on: June 15, 2024, 12:59:54 am »
Great, but allow me to just discuss the following point:

To negate a value, you toggle/invert all bits ("complement") and add +2; this is where the "two's complement" name comes from.

The two's complement is actually negating the value bitwise (flipping all bits) and then adding *one*, not two. The "two" in two's complement doesn't come from adding two to the complemented value, but from the fact that the sum of the original number and the two's complemented number is 2^N (where N is the width of the original and complemented numbers), but when only considering N bits (so modulo 2^N), the sum yields zero, which is arithmetically consistent [ x + (-x) = 0 (mod 2^N) ].
You're obviously right

Thanks for the correction.  I over-stroke the misleading bit in my post.  I do wonder where this brain-fart erupted from.

Edit: It's an hour later, and I'm still blushing with shame.  However, this is an excellent example of how I must check and not rely on my recollection.  Perhaps it is easier to others, but for myself, I need to verify and not just trust, as this error well shows.  Which is why I also like and recommend 'trust, but verify' as a policy.
« Last Edit: June 15, 2024, 01:49:57 am by Nominal Animal »

#### SiliconWizard

• Super Contributor
• Posts: 14893
• Country:
##### Re: More angle wrapping related misery in code
« Reply #19 on: June 15, 2024, 03:32:45 am »
There is no shame in making a mistake.

The following users thanked this post: Siwastaja

#### Siwastaja

• Super Contributor
• Posts: 8307
• Country:
##### Re: More angle wrapping related misery in code
« Reply #20 on: June 15, 2024, 09:53:42 am »
There is no shame in making a mistake.

This, and while always double-checking everything is a good strategy for designing a nuclear reactor or space station, in an informal forum discussion like this we can give Nominal Animal some slack for sometimes posting an unchecked claim, something which most other people do all the time.

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #21 on: June 16, 2024, 03:32:48 am »
There is no shame in making a mistake.
True.  And as long as my own mistakes are caught and we can correct them, I am happy.

In my own education, I've been lucky to have teachers, mentors and professors who weren't afraid of admitting they didn't know something, nor of admitting and correcting an error they've made.  It has made such a difference to my life, I wish everyone had similar luck, especially in elementary school.

#### Nominal Animal

• Super Contributor
• Posts: 6514
• Country:
##### Re: More angle wrapping related misery in code
« Reply #22 on: June 16, 2024, 05:13:03 am »
I just realized a case that well describes the issues with negative or signed distances or positions:
converting a distance d measured in integer inches, to feet f and inches i.
(I can do the conversion to arc-minutes and arc-seconds from decimal degrees, if that would be better.)

When the distance is a nonnegative integer, we can use
f = d / 12;
i = d - f * 12;
or equivalently,
f = d / 12;
i = d % 12;
Nothing new here, everyone knows this.

If the distance d can be negative, ie. we use signed distance to indicate both distance and direction, or it is a position with respect to origin, we have several options how such negative distances are represented.  Positive distances representation is always the same.

(Note that the below handle both negative and positive distances; you do not need to handle positive and negative distances separately.)
• If we want f = -1, i = -2 to represent negative 1' 2", d = -14, then:
f = d / 12;
i = d - f*12;
Basically all current C compilers also let you use i = d % 12, but the C standard is a bit confused about the sign of the remainder for negative values.  This pattern is well recognized either way by current C and C++ compilers, and will optimize it to efficient code if possible.

• If we want f = -2, i = +10 to represent negative 1' 2", d = -14, then:
f = d / 12;
i = d - f*12;
if (i < 0) {
f -= 1;
i += 12;
}

• If we want f = 1, i = 2 to represent negative 1' 2", d = -14, then:
temp = abs(d); // or temp = (d < 0) ? -d : d;
f = temp / 12;
i = temp - f*12; // or i = temp % 12;

• If you want f = -1, i = 2 to represent negative 1' 2", d = -14, then you are wrong.
This representation cannot differentiate negative distances less than one feet from positive ones.  For example, both d = -5 and d = +5 would be represented by f = 0, i = 5.

When the distance is float d in inches, feet is float f, and inches is float i:
• For never negative d:
f = truncf(d / 12.0f);
i = d - f * 12.0f; // or i = fmodf(d, 12.0f);

• If we want f = -1, i = -2 for negative 1' 2" (d = -14), then:
f = truncf(d / 12.0f);
i = d - f * 12.0f; // or i = fmodf(d, 12.0f);

• If we want f = -2, i = +10 for negative 1' 2" (d = -14), then:
f = floorf(d / 12.0f);
i = d - f * 12.0;

• If we want f = 1, i = 2 for negative 1' 2" (d = -14), then:
temp = fabsf(d);
f = truncf(temp / 12.0f);
i = temp - f * 12.0f; // or i = fmodf(d, 12.0f);

• If we want f = -1, i = 2 to represent negative 1' 2" (d = -14), then
f = floorf(d / 12.0f);
i = fabsf(d - f * 12.0f);
but note that you cannot determine the direction by comparing d or f to zero; instead, you have to use the signbit() macro.  Essentially, for negative directions, signbit(d) and signbit(f) will return 1, and for positive directions 0.  This works, because float (and double) have separate representations for +0 and -0, even though they compare equal and have the same logical value.
Again, note that whichever case you pick, it will convert both positive and negative distances correctly; positive distances always the same normal way in all cases.

When f is truncated or a cast to some integer type, you have the choice of either explicit multiplication or fmodf() function.  For integer divisors, I prefer the multiplication.  For arbitrary divisors like Pi, I recommend fmodf() for maximum precision.

A common suggestion is to replace the truncf() call with a simple cast to some integer type, for example (int)(d / 12.0f). This works –– C99 and later explicitly state it as rounding towards zero, truncation –– but limits the range of useful d to 12.0f*INT_MIN .. 12.0f*INT_MAX.  Because float has only 24 bits of precision anyway, and ints are typically at least 32-bit, that is usually okay.  In many architectures the cast is faster, too, especially if immediately re-cast back to float.

When the divisor is smaller than 1.0, such a cast can severely restrict the range even for floats, though.  So be careful, and do not assume it is strictly equivalent to truncation.

The usable range restriction due to a cast to int instead of an explicit trunc() is much more of an issue when using doubles, unless you use (int64_t) or (intmax_t), which have at least 64-bit range (double has 53 bits of precision).

Such a suggestion is therefore useful iff you first consider whether it negatively impacts the range of usable floating-point numbers or not.  Here, if you don't mind losing precision for very long distances, truncf() is better.  If you know your distances are always within INT_MIN to INT_MAX feet, then the (int) cast is functionally equivalent and might be a tiny bit faster.
« Last Edit: June 16, 2024, 05:16:05 am by Nominal Animal »

#### Infraviolet

• Super Contributor
• Posts: 1095
• Country:
##### Re: More angle wrapping related misery in code
« Reply #23 on: June 17, 2024, 02:23:07 am »
Ok, I think I'm now in a position where I might be able to solve my problems as soon as I have a way to do the following:

Take the difference between two numbers(Angle1 - Angle2), these numbers being wrapping numbers which wrap at an arbitrary value (exact value to be selected later, but definitely not a convenient power of 2 or anything like that, that wrapping will happen for MaxRevs and -MaxRevs, not for a 0 to MaxRevs range. So it wraps more like a -180 to 180 than a 0 to 360).

Subtraction must be able to happen across the wrap point, if Angle1 is not far above -MaxRevs and Angle2 is not far below +MaxRevs I'd expect the subtraction to happen across the wrap point

But the sign of the output of the subtraction must be such that it will, including when subtracting across the wrap point, give a clear indication of whether Angle1 or Angle2 is "bigger". Bigger not meaning here an actual size, but rather working n the way that for a 0 to 360 circle you might say 30 is "bigger" than 350 because you go upward from 350 to reach 30 by the shorter route.

--------------

I've got this to work easily where binary numbers are used as the limit, because subtracting them works across the limit of, for example, a uint8_t's wrap point, then going back to zero. But I'm a bit more confused when using numbers with an arbitrary wrap point, and where that wrap point goes between +MaxRevs and -MaxRevs (or +Maxrevs and -Maxrevs+1 I think more accurately, so that -MaxRevs and +MaxRevs are not duplicate values for the same point*).

I'm doing this with signed integer numbers, not floats, so I don't need to worry about decimal parts. But I need to know how to write a generic version which can be easily rewrriten for whichever size of signed integer I choose to use once I know exactly what my MaxRevs value will be.

Can someone advise on the logic for this please?

*clamping being done with the below, though part of me wonders whether it would be wiser to have a >= for the positive limit and a < for the lower limit. The slowness of this for values of many*MaxRevs is ok, because I can't foresee of any way a value >2*MaxRevs or < -2*MaxRevs could ever produce itself, any given operation would do a single addition or subtraction that could go from within range to that far outside the range.

while(count > MaxRevs){
count=count-2*MaxRevs;
}
while(count <= -MaxRevs){
count=count+2*MaxRevs;
}

Thanks

#### SiliconWizard

• Super Contributor
• Posts: 14893
• Country:
##### Re: More angle wrapping related misery in code
« Reply #24 on: June 17, 2024, 02:35:55 am »
As far as FP goes with GCC or (LLVM, which has a very similar behavior for "compatibility" reasons), there's one compilation flag that can make *a lot* of difference:  -ffast-math
assuming, of course, that you know what it is doing and if that's acceptable in your case. The default behavior (without this option enabled) aims for correctness, and can yield pretty slow code. If you can accept the degraded correctness, enabling -ffast-math will make your FP code much faster.

Some pointers here: https://gcc.gnu.org/wiki/FloatingPointMath

Regarding trig functions (which have been also discussed lately), note that glibc's implementation is also very slow (but "surprisingly" accurate). I think Newlib's implementation is pretty close to that of glibc.
If you can accept some loss of accuracy, implementing your own trig functions (discussed already) *will* make a lot of difference if performance is a bottleneck.

Smf