From ae70d9388e82cb7a192c74647d1d0f836c146cee Mon Sep 17 00:00:00 2001
From: bmy <2583236812@qq.com>
Date: Sun, 10 Mar 2024 20:02:40 +0800
Subject: [PATCH] =?UTF-8?q?pref:=20=E6=8D=A2=E7=94=A8=E5=8D=A1=E9=A9=AC?=
 =?UTF-8?q?=E5=85=8B=E7=9A=84=E5=BF=AB=E9=80=9F=E5=BC=80=E6=96=B9=E6=B1=82?=
 =?UTF-8?q?=E5=80=92=E5=87=BD=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

虽然用的地方不是很多()
---
 app/gl_common.c     | 37 +++++++++++++++++++++++++++++--------
 app/gl_common.h     |  1 +
 app/gl_garage.c     |  2 +-
 app/gl_handle_img.c |  8 ++++----
 app/gl_tracking.c   |  8 ++++----
 5 files changed, 39 insertions(+), 17 deletions(-)

diff --git a/app/gl_common.c b/app/gl_common.c
index e35e349..2ba1a42 100644
--- a/app/gl_common.c
+++ b/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);
+}
\ No newline at end of file
diff --git a/app/gl_common.h b/app/gl_common.h
index 2c9f2fe..730c5b7 100644
--- a/app/gl_common.h
+++ b/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_ */
\ No newline at end of file
diff --git a/app/gl_garage.c b/app/gl_garage.c
index 967eed3..3509f82 100644
--- a/app/gl_garage.c
+++ b/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;
 
diff --git a/app/gl_handle_img.c b/app/gl_handle_img.c
index 23745bb..3ffa398 100644
--- a/app/gl_handle_img.c
+++ b/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;
diff --git a/app/gl_tracking.c b/app/gl_tracking.c
index 9f5ef1a..b0e3713 100644
--- a/app/gl_tracking.c
+++ b/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;
 
         // //考虑近点