Revision 1603 trunk/code/projects/fp_math/fp_math.c
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