Dealing with underflow in C " if " condition - using boolean - c

Question

I've a problem, i'm stuck with some underflow problem for my algorithm.
I'm basically dseisgning a path from a Bezier curve and to deal with this I had to work with some vector multiplication (cross and dot product) in order to have the angle between two vectors and the clock-counterclock direction from one to another one.
The problem is that when the path is a straight line one of the control variable has problem of underflow, basically blocking the execution and causing errors.
Here is the code:
void BezierInterp() {
NumOfSetpoints = 10;
float seqTH[11];
float orient[10];
float divider;
math.MatrixMult((float*) BCoeff, (float*) waypointX, 11, 4, 1,
(float*) setpoint0);
math.MatrixMult((float*) BCoeff, (float*) waypointY, 11, 4, 1,
(float*) setpoint1);
float dx1, dy1, dx2, dy2, dxy1, dxy2, dir;
dx1 = cos(state[2]);
dy1 = sin(state[2]);
dx2 = setpoint0[1] - setpoint0[0];
dy2 = setpoint1[1] - setpoint1[0];
dxy2 = sqrt(sq(dx2) + sq(dy2));
dir = dx1 * dy2 - dx2 * dy1;
if (dxy2<0.0001 && dxy2>-0.0001) {
seqTH[0] = 0.0;
}
else{
if (dir >= 0) {
seqTH[0] = acos((dx1 * dx2 + dy1 * dy2) / (dxy2));
} else {
seqTH[0] = -acos((dx1 * dx2 + dy1 * dy2) / (dxy2));
}}
for (uint8_t i = 1; i <= 9; i = i + 1) {
dx2 = setpoint0[i + 1] - setpoint0[i];
dy2 = setpoint1[i + 1] - setpoint1[i];
dxy2 = sqrt(sq(dx2) + sq(dy2));
dx1 = setpoint0[i] - setpoint0[i - 1];
dy1 = setpoint1[i] - setpoint1[i - 1];
dxy1 = sqrt(sq(dx1) + sq(dy1));
dir = dx1 * dy2 - dx2 * dy1;
divider= dxy1 * dxy2;
if (divider<0.0001 && divider>-0.0001) {
seqTH[0] = 0.0;
}
else {
if (dir >= 0) {
seqTH[i] = acos((dx1 * dx2 + dy1 * dy2) / (divider));
} else {
seqTH[i] = -acos((dx1 * dx2 + dy1 * dy2) / (divider));
}}
}
print_array("seqTh", seqTH, 11, 6);
orient[0] = state[2] + seqTH[0];
if (orient[0]<0.0001 && orient[0]>-0.0001){orient[0]=0.0001;}
for (uint8_t i = 1; i <= 9; i = i + 1) {
orient[i] = orient[i - 1] + seqTH[i];
if (orient[i]<0.0001 && orient[i]>-0.0001){orient[i]=0.0001;}
}
print_array("orient", orient, 10, 6);
for (uint8_t i = 1; i <= 9; i = i + 1) {
setpoint2[i] = orient[i - 1];
setpoint3[i] = Vref * cos(orient[i - 1]);
setpoint4[i] = Vref * sin(orient[i - 1]);
}
setpoint2[10] = orient[9];
setpoint3[10] = 0;
setpoint4[10] = 0;
setpoint5[10] = 0;
}
}
As you see in the attempt to avoid error I put several if conditions, but was not enough.
Actually the problem come probably from dir=dx1 * dy2 - dx2 * dy1;. that's when moving along x or y axis is too small to be a float.
A friend suggested to use a boolean value but I'm not sure how.
Maybe defining boolean dir; and then if the value is too small will be a 0 otherwise will be considered a 1 and in that case I could use the same procedure i'm using now for the detection of the direction.
Do you have any suggestion or maybe a different solution?
Thanks in advance
Ned

Answer 1

I'm not familiar with the method you're using, but when I've done this in the past I've detected the degenerate case of the Bezier (where the two end points and two control points fall on a straight line) as a special case.
This is also much faster to draw of course.