Rewrite rounding function without float adds

Force float sqrt to use hardware sqrt

include/libc/math.h

@@ -8,6 +8,6 @@ double sin(double);
 float cosf(float);
 double cos(double);
 
-float sqrtf(float);
+#define sqrtf fsqrt;
 
 #endif

src/game/geo_misc.c

@@ -66,11 +66,24 @@ void make_vertex(Vtx *vtx, s32 n, f32 x, f32 y, f32 z, s16 tx, s16 ty, u8 r, u8
  * Round `num` to the nearest `s16`.
  */
 s16 round_float(f32 num) {
+    union {u32 i; f32 f;}data;
+    data.f=num;
     // Note that double literals are used here, rather than float literals.
-    if (num >= 0.0) {
-        return num + 0.5;
-    } else {
-        return num - 0.5;
+    u32 num_int= data.i;
+
+    s32 mantissa=num_int &  ( (1<<23)-1);
+    mantissa=mantissa |  (1<<23) ;
+
+    s32 exponent = (num_int >>23) & 0xff ;
+    s32 sign=((num_int & (1<<31) )>0 ?-1:1)  ;
+    s32 shift= exponent-150;
+
+    if (shift>=0) {
+        return sign*(mantissa<<shift);
+    } else { 
+	int pos=-(shift+1);
+	int digit= (mantissa & ( 1 << pos )) >> pos;
+	return digit ? sign*((mantissa >> -shift)-1+2*digit) : sign*(mantissa >>-shift);
     }
 }
 

src/nds/math.c

@@ -0,0 +1,34 @@
+
+#include "nds_include.h" 
+
+#define f32 float
+#define u32 uint32_t
+#define s32 int32_t
+#define u64 uint64_t
+
+#ifdef TARGET_NDS
+#define NDS_ITCM_CODE __attribute__((section(".itcm")))
+#else
+#define NDS_ITCM_CODE
+#endif
+
+NDS_ITCM_CODE f32 fsqrt(f32 x){
+    union{f32 f; u32 i;}xu;
+    xu.f=x;
+    //grab exponent
+    s32 exponent= (xu.i & (0xff<<23));
+    if(exponent==0)return 0.0;
+    exponent=exponent-(127<<23);
+    exponent=exponent>>1; //right shift on negative number depends on compiler
+    u64 mantissa=xu.i & ((1<<23)-1);
+    mantissa=(mantissa+(1<<23))<<23;
+    if ((exponent & (1<<22))>0){
+    mantissa=mantissa<<1;
+    }
+    //printf("exponent: %d\n", exponent);
+    //printf("mantissa: %ld\n", mantissa);
+    u32 new_mantissa= (u32) sqrt64(mantissa); //modify this line to use hardware sqrt
+
+    xu.i= ((exponent+(127<<23))& (0xff<<23) ) | (new_mantissa & ((1<<23)-1));
+    return xu.f;
+}