1 files changed, 55 insertions, 0 deletions
diff --git a/libk/src/math/floor.c b/libk/src/math/floor.c
new file mode 100644
index 0000000..ce8e07e
--- /dev/null
+++ b/libk/src/math/floor.c
@@ -0,0 +1,55 @@
+#include <stdint.h>
+#include <float.h>
+#include <math.h>
+
+#include "internal/libm.h"
+
+#define EPS DBL_EPSILON
+
+static const double toint = 1/EPS;
+
+double floor(double x) {
+	union {double f; uint64_t i;} u = {x};
+	int e = u.i >> 52 & 0x7ff;
+	double y;
+
+	if (e >= 0x3ff+52 || x == 0)
+		return x;
+	if (u.i >> 63)
+		y = x - toint + toint - x;
+	else
+		y = x + toint - toint - x;
+	if (e <= 0x3ff-1) {
+		return u.i >> 63 ? -1 : 0;
+	}
+	if (y > 0) {
+        FORCE_EVAL(y);
+    	return x + y - 1;
+    }
+	return x + y;
+}
+
+float floorf(float x) {
+	union {float f; uint32_t i;} u = {x};
+	int e = (int)(u.i >> 23 & 0xff) - 0x7f;
+	uint32_t m;
+
+	if (e >= 23)
+		return x;
+	if (e >= 0) {
+		m = 0x007fffff >> e;
+		if ((u.i & m) == 0)
+			return x;
+		FORCE_EVAL(x + 0x1p120f);
+		if (u.i >> 31)
+			u.i += m;
+		u.i &= ~m;
+	} else {
+		FORCE_EVAL(x + 0x1p120f);
+		if (u.i >> 31 == 0)
+			u.i = 0;
+		else if (u.i << 1)
+			u.f = -1.0;
+	}
+	return u.f;
+}