Revision 1603
| trunk/code/projects/fp_math/fp_math.c (revision 1603) | ||
|---|---|---|
| 86 | 86 |
|
| 87 | 87 |
//Move theta into [0, pi/2] w/ appropriate sign. |
| 88 | 88 |
theta = ABS(theta) % FP_TWO_PI; |
| 89 |
|
|
| 89 |
|
|
| 90 |
//Reflecting into [0, pi] doesn't change sign. |
|
| 90 | 91 |
if(theta > FP_PI) |
| 91 | 92 |
theta = FP_TWO_PI - theta; |
| 92 | 93 |
|
| 94 |
//Reflecting into [0, pi/2] does. |
|
| 93 | 95 |
if(theta > FP_PI_OVER_TWO) {
|
| 94 | 96 |
theta = FP_PI - theta; |
| 95 | 97 |
negative = 1; |
| 96 | 98 |
} |
| 97 |
|
|
| 98 |
//Find the nearest table values. FIXME |
|
| 99 |
|
|
| 100 |
//Find n such that theta is between x_{n} and x_{n+1}
|
|
| 99 | 101 |
n = theta / TABLE_STEP; |
| 100 | 102 |
while( n < TABLE_LENGTH - 1 |
| 101 |
&& (x_np1 = pgm_read_dword(&linspace[n+1]) < theta)) |
|
| 103 |
&& pgm_read_dword(&linspace[n+1]) < theta) |
|
| 102 | 104 |
n++; |
| 103 | 105 |
|
| 104 |
//theta is between x_{n} and x_{n+1}
|
|
| 105 |
|
|
| 106 |
//Edge case |
|
| 106 | 107 |
if(n == TABLE_LENGTH - 1) {
|
| 107 | 108 |
//Perform linear interpolation, since we're close to zero anyway. |
| 108 | 109 |
x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]); |
| ... | ... | |
| 114 | 115 |
return negative ? -result : result; |
| 115 | 116 |
} |
| 116 | 117 |
|
| 117 |
if(n == TABLE_LENGTH) {
|
|
| 118 |
//We didn't find a value! Oh no! |
|
| 119 |
usb_puts("fp_math: Fatal! We couldn't find surrounding table values! \n\r");
|
|
| 120 |
|
|
| 121 |
return 0; |
|
| 122 |
} |
|
| 123 |
|
|
| 124 |
//Address the general case. Quadratic interpolation. |
|
| 118 |
//Perform quadratic interpolation. |
|
| 119 |
|
|
| 125 | 120 |
//Load in the necessary values. |
| 126 | 121 |
x_n = pgm_read_dword(&linspace[n]); |
| 127 | 122 |
x_np1 = pgm_read_dword(&linspace[n + 1]); |
Also available in: Unified diff