Revision 1603
Small changes.. I definitely wouldn't use this for anything yet.
trunk/code/projects/fp_math/fp_math.c | ||
---|---|---|
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