mirror of
https://github.com/lvgl/lvgl.git
synced 2025-01-21 06:53:01 +08:00
318 lines
17 KiB
C
318 lines
17 KiB
C
/**
|
|
* @file lv_math.c
|
|
*
|
|
*/
|
|
|
|
/*********************
|
|
* INCLUDES
|
|
*********************/
|
|
#include "lv_math.h"
|
|
#include <stdbool.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
/*********************
|
|
* DEFINES
|
|
*********************/
|
|
|
|
/**********************
|
|
* TYPEDEFS
|
|
**********************/
|
|
|
|
/**********************
|
|
* STATIC PROTOTYPES
|
|
**********************/
|
|
|
|
/**********************
|
|
* STATIC VARIABLES
|
|
**********************/
|
|
static const int16_t sin0_90_table[] = {
|
|
0, 572, 1144, 1715, 2286, 2856, 3425, 3993, 4560, 5126, 5690, 6252, 6813, 7371, 7927, 8481,
|
|
9032, 9580, 10126, 10668, 11207, 11743, 12275, 12803, 13328, 13848, 14364, 14876, 15383, 15886, 16383, 16876,
|
|
17364, 17846, 18323, 18794, 19260, 19720, 20173, 20621, 21062, 21497, 21925, 22347, 22762, 23170, 23571, 23964,
|
|
24351, 24730, 25101, 25465, 25821, 26169, 26509, 26841, 27165, 27481, 27788, 28087, 28377, 28659, 28932, 29196,
|
|
29451, 29697, 29934, 30162, 30381, 30591, 30791, 30982, 31163, 31335, 31498, 31650, 31794, 31927, 32051, 32165,
|
|
32269, 32364, 32448, 32523, 32587, 32642, 32687, 32722, 32747, 32762, 32767};
|
|
|
|
/**********************
|
|
* MACROS
|
|
**********************/
|
|
|
|
/**********************
|
|
* GLOBAL FUNCTIONS
|
|
**********************/
|
|
|
|
/**
|
|
* Return with sinus of an angle
|
|
* @param angle
|
|
* @return sinus of 'angle'. sin(-90) = -32767, sin(90) = 32767
|
|
*/
|
|
int16_t lv_trigo_sin(int16_t angle)
|
|
{
|
|
int16_t ret = 0;
|
|
angle = angle % 360;
|
|
|
|
if(angle < 0) angle = 360 + angle;
|
|
|
|
if(angle < 90) {
|
|
ret = sin0_90_table[angle];
|
|
} else if(angle >= 90 && angle < 180) {
|
|
angle = 180 - angle;
|
|
ret = sin0_90_table[angle];
|
|
} else if(angle >= 180 && angle < 270) {
|
|
angle = angle - 180;
|
|
ret = -sin0_90_table[angle];
|
|
} else { /*angle >=270*/
|
|
angle = 360 - angle;
|
|
ret = -sin0_90_table[angle];
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* Calculate a value of a Cubic Bezier function.
|
|
* @param t time in range of [0..LV_BEZIER_VAL_MAX]
|
|
* @param u0 start values in range of [0..LV_BEZIER_VAL_MAX]
|
|
* @param u1 control value 1 values in range of [0..LV_BEZIER_VAL_MAX]
|
|
* @param u2 control value 2 in range of [0..LV_BEZIER_VAL_MAX]
|
|
* @param u3 end values in range of [0..LV_BEZIER_VAL_MAX]
|
|
* @return the value calculated from the given parameters in range of [0..LV_BEZIER_VAL_MAX]
|
|
*/
|
|
int32_t lv_bezier3(uint32_t t, int32_t u0, int32_t u1, int32_t u2, int32_t u3)
|
|
{
|
|
uint32_t t_rem = 1024 - t;
|
|
uint32_t t_rem2 = (t_rem * t_rem) >> 10;
|
|
uint32_t t_rem3 = (t_rem2 * t_rem) >> 10;
|
|
uint32_t t2 = (t * t) >> 10;
|
|
uint32_t t3 = (t2 * t) >> 10;
|
|
|
|
uint32_t v1 = ((uint32_t)t_rem3 * u0) >> 10;
|
|
uint32_t v2 = ((uint32_t)3 * t_rem2 * t * u1) >> 20;
|
|
uint32_t v3 = ((uint32_t)3 * t_rem * t2 * u2) >> 20;
|
|
uint32_t v4 = ((uint32_t)t3 * u3) >> 10;
|
|
|
|
return v1 + v2 + v3 + v4;
|
|
}
|
|
#define BITSPERLONG 32
|
|
|
|
#define TOP2BITS(x) ((x & (3L << (BITSPERLONG-2))) >> (BITSPERLONG-2))
|
|
|
|
void lv_sqrt(uint32_t x, lv_sqrt_res_t * q)
|
|
{
|
|
|
|
if(x == 0) {
|
|
q->f = 0;
|
|
q->i = 0;
|
|
return;
|
|
}
|
|
|
|
/*Look up for x=1..1024*/
|
|
/*Look up for x=1..1024*/
|
|
static const uint8_t ci[] = {
|
|
1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
|
|
6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 8, 8, 8, 8, 8, 8, 8,
|
|
8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 10, 10, 10, 10,
|
|
10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11,
|
|
11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12,
|
|
12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13,
|
|
13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14,
|
|
14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 15, 15, 15, 15, 15, 15, 15, 15, 15,
|
|
15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 16, 16, 16, 16,
|
|
16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16,
|
|
16, 16, 16, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17,
|
|
17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18,
|
|
18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 19, 19, 19,
|
|
19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
|
|
19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20,
|
|
20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 21,
|
|
21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21,
|
|
21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 21, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22,
|
|
22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22, 22,
|
|
22, 22, 22, 22, 22, 22, 22, 22, 22, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,
|
|
23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23,
|
|
23, 23, 23, 23, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24,
|
|
24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24, 24,
|
|
24, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25,
|
|
25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25, 25,
|
|
26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26,
|
|
26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26,
|
|
26, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,
|
|
27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27,
|
|
27, 27, 27, 27, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28,
|
|
28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28,
|
|
28, 28, 28, 28, 28, 28, 28, 28, 28, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29,
|
|
29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29,
|
|
29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,
|
|
30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30,
|
|
30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 31,
|
|
31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31,
|
|
31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31,
|
|
31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 32,
|
|
};
|
|
|
|
static const uint8_t cf[] = {
|
|
0, 106, 187, 0, 60, 115, 165, 212, 0, 42, 81, 119, 155, 190, 223, 0, 32, 62, 92, 121, 149, 177, 204, 230,
|
|
0, 25, 50, 75, 99, 122, 145, 168, 191, 213, 235, 0, 21, 42, 63, 83, 103, 123, 143, 162, 181, 200, 219,
|
|
238, 0, 18, 36, 54, 72, 89, 107, 124, 141, 158, 174, 191, 207, 224, 240, 0, 16, 32, 47, 63, 78, 94, 109,
|
|
124, 139, 154, 169, 184, 198, 213, 227, 242, 0, 14, 28, 42, 56, 70, 84, 97, 111, 125, 138, 151, 165, 178,
|
|
191, 204, 217, 230, 243, 0, 13, 25, 38, 51, 63, 76, 88, 100, 113, 125, 137, 149, 161, 173, 185, 197, 209,
|
|
221, 233, 244, 0, 12, 23, 35, 46, 58, 69, 80, 92, 103, 114, 125, 136, 147, 158, 169, 180, 191, 202, 213,
|
|
224, 235, 245, 0, 11, 21, 32, 42, 53, 63, 74, 84, 95, 105, 115, 125, 136, 146, 156, 166, 176, 186, 196,
|
|
206, 216, 226, 236, 246, 0, 10, 20, 29, 39, 49, 59, 68, 78, 87, 97, 107, 116, 126, 135, 145, 154, 163,
|
|
173, 182, 191, 201, 210, 219, 228, 238, 247, 0, 9, 18, 27, 36, 45, 54, 63, 72, 81, 90, 99, 108, 117, 126,
|
|
135, 143, 152, 161, 170, 178, 187, 196, 204, 213, 222, 230, 239, 247, 0, 9, 17, 26, 34, 42, 51, 59, 68,
|
|
76, 84, 93, 101, 109, 118, 126, 134, 142, 151, 159, 167, 175, 183, 191, 200, 208, 216, 224, 232, 240, 248,
|
|
0, 8, 16, 24, 32, 40, 48, 56, 64, 71, 79, 87, 95, 103, 111, 118, 126, 134, 142, 149, 157, 165, 172, 180,
|
|
188, 195, 203, 211, 218, 226, 233, 241, 248, 0, 8, 15, 23, 30, 37, 45, 52, 60, 67, 75, 82, 89, 97, 104,
|
|
112, 119, 126, 133, 141, 148, 155, 163, 170, 177, 184, 192, 199, 206, 213, 220, 227, 235, 242, 249, 0, 7,
|
|
14, 21, 28, 35, 42, 50, 57, 64, 71, 78, 85, 92, 99, 105, 112, 119, 126, 133, 140, 147, 154, 161, 168, 174,
|
|
181, 188, 195, 202, 209, 215, 222, 229, 236, 243, 249, 0, 7, 13, 20, 27, 34, 40, 47, 54, 60, 67, 74, 80,
|
|
87, 93, 100, 107, 113, 120, 126, 133, 139, 146, 153, 159, 166, 172, 179, 185, 192, 198, 205, 211, 217,
|
|
224, 230, 237, 243, 250, 0, 6, 13, 19, 26, 32, 38, 45, 51, 57, 64, 70, 76, 83, 89, 95, 101, 108, 114, 120,
|
|
126, 133, 139, 145, 151, 158, 164, 170, 176, 182, 189, 195, 201, 207, 213, 219, 225, 232, 238, 244, 250,
|
|
0, 6, 12, 18, 24, 30, 36, 42, 49, 55, 61, 67, 73, 79, 85, 91, 97, 103, 109, 115, 121, 127, 132, 138, 144,
|
|
150, 156, 162, 168, 174, 180, 186, 192, 198, 203, 209, 215, 221, 227, 233, 239, 244, 250, 0, 6, 12, 17,
|
|
23, 29, 35, 41, 46, 52, 58, 64, 69, 75, 81, 87, 92, 98, 104, 109, 115, 121, 127, 132, 138, 144, 149, 155,
|
|
161, 166, 172, 178, 183, 189, 194, 200, 206, 211, 217, 223, 228, 234, 239, 245, 250, 0, 6, 11, 17, 22, 28,
|
|
33, 39, 44, 50, 55, 61, 66, 72, 77, 83, 88, 94, 99, 105, 110, 116, 121, 127, 132, 138, 143, 148, 154, 159,
|
|
165, 170, 175, 181, 186, 192, 197, 202, 208, 213, 219, 224, 229, 235, 240, 245, 251, 0, 5, 11, 16, 21, 27,
|
|
32, 37, 43, 48, 53, 58, 64, 69, 74, 79, 85, 90, 95, 101, 106, 111, 116, 121, 127, 132, 137, 142, 148, 153,
|
|
158, 163, 168, 174, 179, 184, 189, 194, 199, 205, 210, 215, 220, 225, 230, 235, 241, 246, 251, 0, 5, 10,
|
|
15, 20, 26, 31, 36, 41, 46, 51, 56, 61, 66, 71, 76, 81, 86, 92, 97, 102, 107, 112, 117, 122, 127, 132,
|
|
137, 142, 147, 152, 157, 162, 167, 172, 177, 182, 187, 192, 197, 202, 207, 212, 216, 221, 226, 231, 236,
|
|
241, 246, 251, 0, 5, 10, 15, 20, 25, 29, 34, 39, 44, 49, 54, 59, 64, 69, 73, 78, 83, 88, 93, 98, 103, 107,
|
|
112, 117, 122, 127, 132, 136, 141, 146, 151, 156, 161, 165, 170, 175, 180, 185, 189, 194, 199, 204, 208,
|
|
213, 218, 223, 227, 232, 237, 242, 247, 251, 0, 5, 9, 14, 19, 24, 28, 33, 38, 43, 47, 52, 57, 61, 66, 71,
|
|
75, 80, 85, 89, 94, 99, 104, 108, 113, 118, 122, 127, 131, 136, 141, 145, 150, 155, 159, 164, 169, 173,
|
|
178, 182, 187, 192, 196, 201, 206, 210, 215, 219, 224, 229, 233, 238, 242, 247, 251, 0, 5, 9, 14, 18, 23,
|
|
27, 32, 36, 41, 46, 50, 55, 59, 64, 68, 73, 77, 82, 86, 91, 95, 100, 104, 109, 113, 118, 122, 127, 131,
|
|
136, 140, 145, 149, 154, 158, 163, 167, 172, 176, 181, 185, 189, 194, 198, 203, 207, 212, 216, 221, 225,
|
|
229, 234, 238, 243, 247, 252, 0, 4, 9, 13, 18, 22, 26, 31, 35, 40, 44, 48, 53, 57, 62, 66, 70, 75, 79, 83,
|
|
88, 92, 96, 101, 105, 110, 114, 118, 123, 127, 131, 136, 140, 144, 149, 153, 157, 162, 166, 170, 175, 179,
|
|
183, 187, 192, 196, 200, 205, 209, 213, 218, 222, 226, 230, 235, 239, 243, 247, 252, 0, 4, 9, 13, 17, 21,
|
|
26, 30, 34, 38, 43, 47, 51, 55, 60, 64, 68, 72, 76, 81, 85, 89, 93, 98, 102, 106, 110, 114, 119, 123,
|
|
127, 131, 135, 140, 144, 148, 152, 156, 160, 165, 169, 173, 177, 181, 185, 190, 194, 198, 202, 206, 210,
|
|
215, 219, 223, 227, 231, 235, 239, 244, 248, 252, 0, 4, 8, 12, 16, 21, 25, 29, 33, 37, 41, 45, 49, 53, 58,
|
|
62, 66, 70, 74, 78, 82, 86, 90, 94, 98, 103, 107, 111, 115, 119, 123, 127, 131, 135, 139, 143, 147, 151,
|
|
155, 159, 163, 168, 172, 176, 180, 184, 188, 192, 196, 200, 204, 208, 212, 216, 220, 224, 228, 232, 236,
|
|
240, 244, 248, 252, 0,
|
|
|
|
};
|
|
if(x <= 1024) {
|
|
x--;
|
|
q->i = ci[x];
|
|
q->f = cf[x];
|
|
return;
|
|
}
|
|
|
|
/*
|
|
* Source:
|
|
* http://web.archive.org/web/20080303101624/http://c.snippets.org/snip_lister.php?fname=isqrt.c
|
|
* https://stackoverflow.com/questions/1100090/looking-for-an-efficient-integer-square-root-algorithm-for-arm-thumb2
|
|
*/
|
|
|
|
uint32_t a = 0L; /* accumulator */
|
|
uint32_t r = 0L; /* remainder */
|
|
uint32_t e = 0L; /* trial product */
|
|
|
|
uint32_t i;
|
|
for (i = 0; i < BITSPERLONG / 2 + 8; i++) {
|
|
r = (r << 2) + TOP2BITS(x); x <<= 2;
|
|
a <<= 1;
|
|
e = (a << 1) + 1;
|
|
if (r >= e) {
|
|
r -= e;
|
|
a++;
|
|
}
|
|
}
|
|
|
|
q->f = a & 0xFF;
|
|
q->i = a >> 8;
|
|
}
|
|
|
|
/**
|
|
* Calculate the atan2 of a vector.
|
|
* @param x
|
|
* @param y
|
|
* @return the angle in degree calculated from the given parameters in range of [0..360]
|
|
*/
|
|
uint16_t lv_atan2(int x, int y)
|
|
{
|
|
// Fast XY vector to integer degree algorithm - Jan 2011 www.RomanBlack.com
|
|
// Converts any XY values including 0 to a degree value that should be
|
|
// within +/- 1 degree of the accurate value without needing
|
|
// large slow trig functions like ArcTan() or ArcCos().
|
|
// NOTE! at least one of the X or Y values must be non-zero!
|
|
// This is the full version, for all 4 quadrants and will generate
|
|
// the angle in integer degrees from 0-360.
|
|
// Any values of X and Y are usable including negative values provided
|
|
// they are between -1456 and 1456 so the 16bit multiply does not overflow.
|
|
|
|
unsigned char negflag;
|
|
unsigned char tempdegree;
|
|
unsigned char comp;
|
|
unsigned int degree; // this will hold the result
|
|
//signed int x; // these hold the XY vector at the start
|
|
//signed int y; // (and they will be destroyed)
|
|
unsigned int ux;
|
|
unsigned int uy;
|
|
|
|
// Save the sign flags then remove signs and get XY as unsigned ints
|
|
negflag = 0;
|
|
if(x < 0) {
|
|
negflag += 0x01; // x flag bit
|
|
x = (0 - x); // is now +
|
|
}
|
|
ux = x; // copy to unsigned var before multiply
|
|
if(y < 0) {
|
|
negflag += 0x02; // y flag bit
|
|
y = (0 - y); // is now +
|
|
}
|
|
uy = y; // copy to unsigned var before multiply
|
|
|
|
// 1. Calc the scaled "degrees"
|
|
if(ux > uy) {
|
|
degree = (uy * 45) / ux; // degree result will be 0-45 range
|
|
negflag += 0x10; // octant flag bit
|
|
} else {
|
|
degree = (ux * 45) / uy; // degree result will be 0-45 range
|
|
}
|
|
|
|
// 2. Compensate for the 4 degree error curve
|
|
comp = 0;
|
|
tempdegree = degree; // use an unsigned char for speed!
|
|
if(tempdegree > 22) { // if top half of range
|
|
if(tempdegree <= 44) comp++;
|
|
if(tempdegree <= 41) comp++;
|
|
if(tempdegree <= 37) comp++;
|
|
if(tempdegree <= 32) comp++; // max is 4 degrees compensated
|
|
} else { // else is lower half of range
|
|
if(tempdegree >= 2) comp++;
|
|
if(tempdegree >= 6) comp++;
|
|
if(tempdegree >= 10) comp++;
|
|
if(tempdegree >= 15) comp++; // max is 4 degrees compensated
|
|
}
|
|
degree += comp; // degree is now accurate to +/- 1 degree!
|
|
|
|
// Invert degree if it was X>Y octant, makes 0-45 into 90-45
|
|
if(negflag & 0x10) degree = (90 - degree);
|
|
|
|
// 3. Degree is now 0-90 range for this quadrant,
|
|
// need to invert it for whichever quadrant it was in
|
|
if(negflag & 0x02) { // if -Y
|
|
if(negflag & 0x01) // if -Y -X
|
|
degree = (180 + degree);
|
|
else // else is -Y +X
|
|
degree = (180 - degree);
|
|
} else { // else is +Y
|
|
if(negflag & 0x01) // if +Y -X
|
|
degree = (360 - degree);
|
|
}
|
|
return degree;
|
|
}
|
|
|
|
/**********************
|
|
* STATIC FUNCTIONS
|
|
**********************/
|