pref: 换用卡马克的快速开方求倒函数

虽然用的地方不是很多()

app/gl_common.c

@@ -1,15 +1,36 @@
 #include "zf_common_headfile.h"
 #include "gl_headfile.h"
 
-
 int32_t limit(int32_t x, int32_t low, int32_t up)
 {
-    return x > up ? up : x < low ? low : x;
+    return x > up ? up : x < low ? low
+                                 : x;
+}
+int clip(int x, int low, int up)
+{
+    return x > up ? up : x < low ? low
+                                 : x;
 }
- int clip(int x, int low, int up) {
-     return x > up ? up : x < low ? low : x;
- }
 
-float fclip(float x, float low, float up) {
-     return x > up ? up : x < low ? low : x;
- }
+float fclip(float x, float low, float up)
+{
+    return x > up ? up : x < low ? low
+                                 : x;
+}
+
+float Q_sqrt(float number)
+{
+    long i;
+    float x2, y;
+    const float threehalfs = 1.5F;
+
+    x2 = number * 0.5F;
+    y  = number;
+    i  = *(long *)&y;           // evil floating point bit level hacking
+    i  = 0x5f3759df - (i >> 1); // what the fuck?
+    y  = *(float *)&i;
+    y  = y * (threehalfs - (x2 * y * y)); // 1st iteration
+    y  = y * (threehalfs - (x2 * y * y)); // 2nd iteration, this can be removed
+
+    return (1.0 / y);
+}

app/gl_common.h

@@ -27,5 +27,6 @@
 int32_t limit(int32_t x, int32_t low, int32_t up);
 int clip(int x, int low, int up);
 float fclip(float x, float low, float up);
+float Q_sqrt(float number);
 
 #endif /* COMMON_H_ */

app/gl_garage.c

@@ -12,7 +12,7 @@ float calculate_vector_angle(float x1, float y1, float x2, float y2)
     float dx = x2 - x1;
     float dy = y2 - y1;
 
-    float vector_length = sqrt(dx * dx + dy * dy);
+    float vector_length = Q_sqrt(dx * dx + dy * dy);
     float angle_radians = acos(dx / vector_length);
     float angle_degrees = angle_radians * 180 / M_PI;
 

app/gl_handle_img.c

@@ -141,8 +141,8 @@ void GetLinesResample(float pts_in[][2], int32_t num1, float pts_out[][2], int32
                 float dx1 = x1 - x;
                 float dy1 = y1 - y;
 
-                float dist0 = sqrt(dx0 * dx0 + dy0 * dy0);
-                float dist1 = sqrt(dx1 * dx1 + dy1 * dy1);
+                float dist0 = Q_sqrt(dx0 * dx0 + dy0 * dy0);
+                float dist1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
 
                 float r0 = (dist1 - dist) / (dist1 - dist0);
                 float r1 = 1 - r0;
@@ -200,7 +200,7 @@ void GetMidLine_Left(float pts_left[][2], int32_t pts_left_count, float mid_left
     for (int i = 0; i < pts_left_count; i++) {
         float dx = pts_left[clip(i + approx_num, 0, pts_left_count - 1)][1] - pts_left[clip(i - approx_num, 0, pts_left_count - 1)][1];
         float dy = pts_left[clip(i + approx_num, 0, pts_left_count - 1)][0] - pts_left[clip(i - approx_num, 0, pts_left_count - 1)][0];
-        float dn = sqrt(dx * dx + dy * dy);
+        float dn = Q_sqrt(dx * dx + dy * dy);
         dx /= dn;
         dy /= dn;
         mid_left[i][0] = pts_left[i][0] + dx * dist;
@@ -215,7 +215,7 @@ void GetMidLine_Right(float pts_right[][2], int32_t pts_right_count, float mid_r
     for (int i = 0; i < pts_right_count; i++) {
         float dx = pts_right[clip(i + approx_num, 0, pts_right_count -1)][1] - pts_right[clip(i - approx_num, 0, pts_right_count -1 )][1];
         float dy = pts_right[clip(i + approx_num, 0, pts_right_count -1)][0] - pts_right[clip(i - approx_num, 0, pts_right_count -1)][0];
-        float dn = sqrt(dx * dx + dy * dy);
+        float dn = Q_sqrt(dx * dx + dy * dy);
         dx /= dn;//sin
         dy /= dn;//cos
             mid_right[i][0] = pts_right[i][0] - dx * dist;

app/gl_tracking.c

@@ -86,7 +86,7 @@ void MidLineTrack()
     for (int i = 0; i < mid_track_count; i++) {
         float dx   = mid_track[i][1] - cx;
         float dy   = mid_track[i][0] - cy;
-        float dist = sqrt(dx * dx + dy * dy);
+        float dist = Q_sqrt(dx * dx + dy * dy);
         if (dist < min_dist) {
             min_dist = dist;
             begin_id = i;
@@ -108,14 +108,14 @@ void MidLineTrack()
 
         // 计算远锚点偏差值
         float dx = rptsn[aim_idx][1] - cx;
-        float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER;
-        float dn = sqrt(dx * dx + dy * dy);
+        float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER;
+        float dn = (float)Q_sqrt(dx * dx + dy * dy);
         //  float error = -atan2f(dx, dy) * 180 / PI32;
 
         // 计算近锚点偏差值
         dx_near = rptsn[aim_idx_near][1] - cx;
         // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
-        // float dn_near = sqrt(dx_near * dx_near + dy_near * dy_near);
+        // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
         // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
 
         // //考虑近点