1 files changed, 120 insertions, 0 deletions
diff --git a/nuttx/lib/math/lib_atan2.c b/nuttx/lib/math/lib_atan2.c
new file mode 100644
index 000000000..b4ded0c16
--- /dev/null
+++ b/nuttx/lib/math/lib_atan2.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2009, 2010 Nick Johnson <nickbjohnson4224 at gmail.com>
+ * 
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <apps/math.h>
+#include <float.h>
+
+float atan2f(float y, float x) {
+	
+	if (y == 0.0) {
+		if (x >= 0.0) {
+			return 0.0;
+		}
+		else {
+			return M_PI;
+		}
+	}
+	else if (y > 0.0) {
+		if (x == 0.0) {
+			return M_PI_2;
+		}
+		else if (x > 0.0) {
+			return atanf(y / x);
+		}
+		else {
+			return M_PI - atanf(y / x);
+		}
+	}
+	else {
+		if (x == 0.0) {
+			return M_PI + M_PI_2;
+		}
+		else if (x > 0.0) {
+			return 2 * M_PI - atanf(y / x);
+		}
+		else {
+			return M_PI + atanf(y / x);
+		}
+	}
+}
+
+double atan2(double y, double x) {
+	
+	if (y == 0.0) {
+		if (x >= 0.0) {
+			return 0.0;
+		}
+		else {
+			return M_PI;
+		}
+	}
+	else if (y > 0.0) {
+		if (x == 0.0) {
+			return M_PI_2;
+		}
+		else if (x > 0.0) {
+			return atan(y / x);
+		}
+		else {
+			return M_PI - atan(y / x);
+		}
+	}
+	else {
+		if (x == 0.0) {
+			return M_PI + M_PI_2;
+		}
+		else if (x > 0.0) {
+			return 2 * M_PI - atan(y / x);
+		}
+		else {
+			return M_PI + atan(y / x);
+		}
+	}
+}
+
+long double atan2l(long double y, long double x) {
+	
+	if (y == 0.0) {
+		if (x >= 0.0) {
+			return 0.0;
+		}
+		else {
+			return M_PI;
+		}
+	}
+	else if (y > 0.0) {
+		if (x == 0.0) {
+			return M_PI_2;
+		}
+		else if (x > 0.0) {
+			return atanl(y / x);
+		}
+		else {
+			return M_PI - atanl(y / x);
+		}
+	}
+	else {
+		if (x == 0.0) {
+			return M_PI + M_PI_2;
+		}
+		else if (x > 0.0) {
+			return 2 * M_PI - atanl(y / x);
+		}
+		else {
+			return M_PI + atanl(y / x);
+		}
+	}
+}