1 #include <stdio.h>
2 #include <string.h>
3 #include <stdlib.h>
4 #include <math.h>
5 #include "sfm.h"
6
7
8 double SFMHypot2(double dx, double dy);
9 double SFMHypot3(double dx, double dy, double dz);
10
11 double SFMSanitizeRadians(double r);
12 double SFMSanitizeDegrees(double d);
13 double SFMRadiansToDegrees(double r);
14 double SFMDegreesToRadians(double d);
15
16 double SFMDeltaRadians(double a1, double a2);
17 void SFMOrthoRotate2D(double theta, double *i, double *j);
18
19 double SFMMetersToFeet(double m);
20 double SFMFeetToMeters(double feet);
21 double SFMMetersToMiles(double m);
22 double SFMMilesToMeters(double miles);
23 double SFMMPHToMPC(double mph);
24 double SFMMPHToKTS(double mph);
25 double SFMKTSToMPH(double kts);
26 double SFMMPCToMPH(double mpc);
27 double SFMMPCToFPS(double mpc);
28 double SFMMPCToKPH(double mpc);
29
30 double SFMLBSToKG(double lbs);
31 double SFMKGToLBS(double kg);
32
33 void SFMMToDMS(
34 double m_x, double m_y, double m_r,
35 double dms_x_offset, double dms_y_offset,
36 float *dms_x, float *dms_y
37 );
38 char *SFMLongitudeToString(double dms_x);
39 char *SFMLatitudeToString(double dms_y);
40
41 double SFMStallCoeff(
42 double current_speed, double stall_speed, double speed_max
43 );
44
45
46 #define POW(x,y) (((x) > 0.0f) ? pow(x,y) : 0.0f)
47 #define SQRT(x) (((x) > 0.0f) ? sqrt(x) : 0.0f)
48
49 #define MAX(a,b) (((a) > (b)) ? (a) : (b))
50 #define MIN(a,b) (((a) < (b)) ? (a) : (b))
51 #define CLIP(a,l,h) (MIN(MAX((a),(l)),(h)))
52
53 #define RADTODEG(r) ((r) * 180 / PI)
54 #define DEGTORAD(d) ((d) * PI / 180)
55
56
57 /*
58 * Returns the 2d hypot of the given delta distances.
59 */
SFMHypot2(double dx,double dy)60 double SFMHypot2(double dx, double dy)
61 {
62 return(SQRT((dx * dx) + (dy * dy)));
63 }
64
65 /*
66 * Returns the 3d hypot of the given delta distances.
67 */
SFMHypot3(double dx,double dy,double dz)68 double SFMHypot3(double dx, double dy, double dz)
69 {
70 return(SQRT((dx * dx) + (dy * dy) + (dz * dz)));
71 }
72
73
74 /*
75 * Sanitize radian value:
76 */
SFMSanitizeRadians(double r)77 double SFMSanitizeRadians(double r)
78 {
79 while(r < 0)
80 r += (2 * PI);
81 while(r >= (2 * PI))
82 r -= (2 * PI);
83 return(r);
84 }
85
86 /*
87 * Sanitize degree value:
88 */
SFMSanitizeDegrees(double d)89 double SFMSanitizeDegrees(double d)
90 {
91 while(d < 0.0)
92 d += 360.0;
93 while(d >= 360.0)
94 d -= 360.0;
95 return(d);
96 }
97
98 /*
99 * Convert radians to degrees and sanitize:
100 */
SFMRadiansToDegrees(double r)101 double SFMRadiansToDegrees(double r)
102 {
103 return(SFMSanitizeDegrees(r * (180 / PI)));
104 }
105
106 /*
107 * Convert degrees to radians and sanitize:
108 */
SFMDegreesToRadians(double d)109 double SFMDegreesToRadians(double d)
110 {
111 return(SFMSanitizeRadians(d * (PI / 180)));
112 }
113
114
115 /*
116 * Returns the delta angle in radians of a2 - a1.
117 */
SFMDeltaRadians(double a1,double a2)118 double SFMDeltaRadians(double a1, double a2)
119 {
120 double theta = SFMSanitizeRadians(a2) - SFMSanitizeRadians(a1);
121
122 if(theta < -PI)
123 return((2 * PI) - a1 + a2);
124 else if(theta > PI)
125 return(-((2 * PI) - a2 + a1));
126 else
127 return(theta);
128 }
129
130 /*
131 * Rotates i and j about the crossed perpendicular axis (orthogonal)
132 * to the plane with i and j in theta. Where theta is in radians and
133 * in bearing.
134 */
SFMOrthoRotate2D(double theta,double * i,double * j)135 void SFMOrthoRotate2D(double theta, double *i, double *j)
136 {
137 double sin_theta = sin(theta);
138 double cos_theta = cos(theta);
139 double i0, j0;
140
141 if((i == NULL) || (j == NULL))
142 return;
143
144 i0 = *i;
145 j0 = *j;
146
147 *i = (i0 * cos_theta) + (j0 * sin_theta);
148 *j = (j0 * cos_theta) - (i0 * sin_theta);
149 }
150
151 /*
152 * Convert meters to feet:
153 */
SFMMetersToFeet(double m)154 double SFMMetersToFeet(double m)
155 {
156 return(m * 3.280833);
157 }
158 /*
159 * Convert feet to meters:
160 */
SFMFeetToMeters(double feet)161 double SFMFeetToMeters(double feet)
162 {
163 return(feet / 3.280833);
164 }
165
166 /*
167 * Convert meters to miles.
168 */
SFMMetersToMiles(double m)169 double SFMMetersToMiles(double m)
170 {
171 return(m * 0.00062137);
172 }
173 /*
174 * Convert miles to meters.
175 */
SFMMilesToMeters(double miles)176 double SFMMilesToMeters(double miles)
177 {
178 return(miles / 0.00062137);
179 }
180
181 /*
182 * Convert miles (US Statute) per hour to meters per cycle:
183 */
SFMMPHToMPC(double mph)184 double SFMMPHToMPC(double mph)
185 {
186 return(
187 mph * (1609.347 / 3600.0 * (double)SFMCycleUnitsUS / 1000000.0)
188 );
189 }
190
191 /*
192 * Convert miles (US Statute) per hour to nautical miles per hour:
193 */
SFMMPHToKTS(double mph)194 double SFMMPHToKTS(double mph)
195 {
196 return(mph / 1.15151515);
197 }
198
199 /*
200 * Convert nautical miles per hour to miles (US Statute) per hour:
201 */
SFMKTSToMPH(double kts)202 double SFMKTSToMPH(double kts)
203 {
204 return(kts * 1.15151515);
205 }
206
207 /*
208 * Convert meters per cycle to miles (US Statute) per hour:
209 */
SFMMPCToMPH(double mpc)210 double SFMMPCToMPH(double mpc)
211 {
212 return(
213 mpc / (1609.34738 / 3600.0 * (double)SFMCycleUnitsUS / 1000000.0)
214 );
215 }
216
217 /*
218 * Convert meters per cycle to feet per second:
219 */
SFMMPCToFPS(double mpc)220 double SFMMPCToFPS(double mpc)
221 {
222 return(
223 mpc * (3.280833 * (double)SFMCycleUnitsUS / 1000000.0)
224 );
225 }
226
227 /*
228 * Convert meters per cycle to kilometers per hour:
229 */
SFMMPCToKPH(double mpc)230 double SFMMPCToKPH(double mpc)
231 {
232 return(
233 mpc * (3.6 * (double)SFMCycleUnitsUS / 1000000.0)
234 );
235 }
236
237 /*
238 * Convert pounds (avoirdupois) to kg:
239 */
SFMLBSToKG(double lbs)240 double SFMLBSToKG(double lbs)
241 {
242 return(lbs * 0.453592);
243 }
244 /*
245 * Convert kg to pounds:
246 */
SFMKGToLBS(double kg)247 double SFMKGToLBS(double kg)
248 {
249 return(kg / 0.453592);
250 }
251
252
253 /*
254 * Converts meters to DMS in units of degrees.
255 *
256 * Range for dms_x is unsanitized;
257 * Range for dms_y is -90.0 to 90.0.
258 *
259 * m_r must be positive and non-zero.
260 */
SFMMToDMS(double m_x,double m_y,double m_r,double dms_x_offset,double dms_y_offset,float * dms_x,float * dms_y)261 void SFMMToDMS(
262 double m_x, double m_y, double m_r,
263 double dms_x_offset, double dms_y_offset,
264 float *dms_x, float *dms_y
265 )
266 {
267 float dms_y_radians, mz_r;
268
269
270 if((dms_x == NULL) || (dms_y == NULL) || (m_r <= 0))
271 return;
272
273
274 /* Calculate y_dms first */
275 *dms_y = (float)((m_y / m_r * (180 / PI)) + dms_y_offset);
276
277 /* Sanitize dms_y */
278 if(*dms_y > 90.0f)
279 *dms_y = 180.0f - (*dms_y);
280 if(*dms_y < -90.0f)
281 *dms_y = 180.0f + (*dms_y);
282
283 dms_y_radians = (float)((*dms_y) * (PI / 180));
284
285
286 /* Calculate x_dms */
287 mz_r = (float)(m_r * cos(dms_y_radians));
288 if(mz_r > 0.0f)
289 *dms_x = (float)((m_x / mz_r * (180 / PI)) + dms_x_offset);
290 else
291 *dms_x = 0.0f;
292 }
293
294 /*
295 * Returns a statically allocated string containing
296 * a formatted longitude string.
297 *
298 * This function never returns NULL.
299 */
SFMLongitudeToString(double dms_x)300 char *SFMLongitudeToString(double dms_x)
301 {
302 double m, s;
303 static char str[256];
304
305
306 /* Sanitize */
307 while(dms_x < -140.0)
308 dms_x += (2 * 140.0);
309 while(dms_x > 140.0)
310 dms_x -= (2 * 140.0);
311
312 /* West or east? */
313 if(dms_x < 0.0)
314 {
315 /* West */
316 dms_x = fabs(dms_x);
317 m = (dms_x - floor(dms_x)) * 60.0;
318 s = floor((m - floor(m)) * 60.0);
319 m = floor(m);
320 sprintf(
321 str,
322 "%.0f'%s%.0f:%s%.0fW",
323 floor(dms_x),
324 (m < 10.0f) ? "0" : "", m,
325 (s < 10.0f) ? "0" : "", s
326 );
327 }
328 else
329 {
330 /* East */
331 m = (dms_x - floor(dms_x)) * 60.0;
332 s = floor((m - floor(m)) * 60.0);
333 m = floor(m);
334 sprintf(
335 str,
336 "%.0f'%s%.0f:%s%.0fE",
337 floor(dms_x),
338 (m < 10.0f) ? "0" : "", m,
339 (s < 10.0f) ? "0" : "", s
340 );
341 }
342
343 return(str);
344 }
345
346 /*
347 * Returns a statically allocated string containing
348 * a formatted latitude string.
349 *
350 * This function never returns NULL.
351 */
SFMLatitudeToString(double dms_y)352 char *SFMLatitudeToString(double dms_y)
353 {
354 double m, s;
355 static char str[256];
356
357 #if 0
358 /* Sanitize */
359 /* Should already be sanitized */
360 while(dms_y < -90.0)
361 dms_y += 90.0;
362 while(dms_y > 90.0)
363 dms_y -= 90.0;
364 #endif
365 /* Above or below the equator? */
366 if(dms_y < 0.0)
367 {
368 /* South */
369 dms_y = fabs(dms_y);
370 m = (dms_y - floor(dms_y)) * 60.0;
371 s = floor((m - floor(m)) * 60.0);
372 m = floor(m);
373 sprintf(
374 str,
375 "%.0f'%s%.0f:%s%.0fS",
376 floor(dms_y),
377 (m < 10.0f) ? "0" : "", m,
378 (s < 10.0f) ? "0" : "", s
379 );
380 }
381 else
382 {
383 /* North */
384 m = (dms_y - floor(dms_y)) * 60.0;
385 s = floor((m - floor(m)) * 60.0);
386 m = floor(m);
387 sprintf(
388 str,
389 "%.0f'%s%.0f:%s%.0fN",
390 floor(dms_y),
391 (m < 10.0f) ? "0" : "", m,
392 (s < 10.0f) ? "0" : "", s
393 );
394 }
395
396 return(str);
397 }
398
399
400 /*
401 * Calculates a stall coefficient based on the given stall
402 * speed (all units in meters per cycle), current speed, and
403 * maximum speed. Return is in the range of 0 to 1, where 1
404 * is the highest stall coefficient (when current speed is 0).
405 */
SFMStallCoeff(double current_speed,double stall_speed,double speed_max)406 double SFMStallCoeff(
407 double current_speed, double stall_speed, double speed_max
408 )
409 {
410 if(current_speed > stall_speed)
411 return(0.0);
412 else if(stall_speed <= 0.0)
413 return(0.0);
414 else if(current_speed <= 0.0)
415 return(1.0);
416 else
417 #if 0
418 return(1.0 - POW(current_speed / stall_speed, 1.5));
419 #endif
420 return(1.0 - (current_speed / stall_speed));
421 }
422