0x001bd85c,TPitchLineCircle::Clamp bugfix

jni/TPitchLineCircle.cpp

@@ -1,7 +1,12 @@
 #include "TPitchLineCircle.h"
 #include "memctrl.h"
-
+#include <algorithm>
 int TPitchLineCircle::s_eType;
+
+void Intersect(float a1,float a2,float a3,float a4,float &a5,float &a6)
+{
+  LOGE("Intersect%x%x%x%x%x%x",a1,a2,a3,a4,a5,a6);
+}
 //-------------------------------------------------------------------------------------------------
 //001BE194 经过单元测试，功能正常
 //^_^---
@@ -14,7 +19,11 @@ int LineCircleIntersection(CFTTVector32 cfttvector32_r0_begin,
                            CFTTVector32 cfttvector32_arg_c_begin,
                            float f_arg_18,
                            float* p_f_arg_1c) {
-  return 0;
+  LOGE("%x%x%x",cfttvector32_r0_begin.float_0,cfttvector32_r0_begin.float_4,cfttvector32_r0_begin.float_8);
+  LOGE("%x%x%x",cfttvector32_r3_begin.float_0,cfttvector32_r3_begin.float_4,cfttvector32_r3_begin.float_8);
+  LOGE("%x%x%x",cfttvector32_arg_c_begin.float_0,cfttvector32_arg_c_begin.float_4,cfttvector32_arg_c_begin.float_8);
+  LOGE("%f%f%f%x",f_arg_8,f_arg_18,*p_f_arg_1c,p_f_arg_1c);
+  return rand();
 }
 //-------------------------------------------------------------------------------------------------
 //001BE690
@@ -28,7 +37,299 @@ int TPitchLineCircle::GetType() {
 }
 //-------------------------------------------------------------------------------------------------
 //001BD85C
-void TPitchLineCircle::Clamp(CFTTVector32 vec32) {
+void TPitchLineCircle::Clamp(CFTTVector32 cVec32_arg1) {
+  float s0 = -0.5f;                // 001BD866
+  float s2 = 0.5f;                 // 001BD86C
+  float s4 = cVec32_arg1.float_0;  // 001BD872
+  float s6 = cVec32_arg1.float_8;  // 001BD878
+  float s18 = s4 * s0;             // 001BD888
+  float s16 = s4 * s2;             // 001BD88C
+  s0 = s6 * s0;                    // 001BD890
+  s2 = s6 * s2;                    // 001BD894
+  float s8, s10, s12, s17, s19, s20, s21, s22, s23, s24, s25, s26, s27, s28, s29, s30;
+  int i_sp_20;
+  int i_sp_24 = 0;    // 001BD8EC
+  uint8 i_sp_28 = 0;  // 001BD8F0
+  float f_sp_2C;
+  int i_sp_30;
+  int i_sp_3C;
+  float f_sp_40;
+  float f_sp_44;
+  float f_sp_48[2];  // 48 4C;
+  float f_sp_50[2];  // 50 54
+  // float f_sp_58;
+  // float f_sp_5C;
+  uchar c_sp_5E[2];
+  float f_sp_60[2];           // 60 64
+  CFTTVector32 cVec32_sp_68;  // 68 6C 70
+  CFTTVector32 cVec32_sp_74;  // 74 78 7C
+  CFTTVector32 cVec32_sp_80[4];
+  cVec32_sp_80[0].float_0 = s18;  // 001BD898 80
+  cVec32_sp_80[0].float_4 = 0;    // 001BD870 84
+  cVec32_sp_80[0].float_8 = s0;   // 001BD89C 88
+
+  cVec32_sp_80[1].float_0 = s16;  // 001BD8A0 8C
+  cVec32_sp_80[1].float_4 = 0;   // 001BD876 90
+  cVec32_sp_80[1].float_8 = s0;  // 001BD8A4 94
+
+  cVec32_sp_80[2].float_0 = s16;  // 001BD8A8 98
+  cVec32_sp_80[2].float_4 = 0;    // 001BD87C 9C
+  cVec32_sp_80[2].float_8 = s2;   // 001BD8AC A0
+
+  cVec32_sp_80[3].float_0 = s18;  // 001BD8B0 A4
+  cVec32_sp_80[3].float_4 = 0;    // 001BD87E A8
+  cVec32_sp_80[3].float_8 = s2;   // 001BD8B4 AC
+
+  s4 = this->vec32_4.float_8;                         // 001BD8B8
+  s20 = this->vec32_4.float_0;                        // 001BD8BC
+  i_sp_20 = (s0 <= s4 ? 1 : 0) & (s4 <= s2 ? 1 : 0);  // 001BD8DC
+
+  int i_r4 = 0;
+  while (i_r4 != 4) {
+    // cVec32_sp_68 = cVec32_sp_80[i_r4];
+    float d16_0 = cVec32_sp_80[i_r4].float_0;  // 001BD900
+    float d16_4 = cVec32_sp_80[i_r4].float_4;
+    cVec32_sp_68.float_8 = cVec32_sp_80[i_r4].float_8;
+    cVec32_sp_68.float_0 = d16_0;
+    cVec32_sp_68.float_4 = d16_4;
+
+    i_r4++;  // 001BD8FA
+    // cVec32_sp_74 = cVec32_sp_80[i_r4 & 3];
+    d16_0 = cVec32_sp_80[i_r4 & 3].float_0;
+    d16_4 = cVec32_sp_80[i_r4 & 3].float_4;
+    cVec32_sp_74.float_8 = cVec32_sp_80[i_r4 & 3].float_8;
+    cVec32_sp_74.float_0 = d16_0;
+    cVec32_sp_74.float_4 = d16_4;
+
+    s23 = cVec32_sp_74.float_4;  // 001BD926
+    s25 = cVec32_sp_68.float_4;  // 001BD92A
+    s19 = cVec32_sp_74.float_0;  // 001BD92E
+    s17 = s23 - s25;             // 001BD932
+    s24 = cVec32_sp_68.float_0;  // 001BD932
+    s21 = cVec32_sp_74.float_8;  // 001BD93A
+    s28 = s19 - s24;             // 001BD93E
+    s26 = cVec32_sp_68.float_8;  // 001BD942
+    s30 = s21 - s26;             // 001BD946
+    s0 = s17 * s17;              // 001BD94A
+    s0 = s0 + s28 * s28;         // 001BD94E
+    s0 = s0 + s30 * s30;         // 001BD952
+    s22 = sqrtf(s0);             // 001BD968
+
+    float f_r6 = this->vec32_4.float_0;
+    float f_r10 = this->vec32_4.float_4;
+    float f_r11 = this->vec32_4.float_8;
+    s27 = f_r10;           // 001BD97A
+    s29 = f_r11;           // 001BD97E
+    s0 = s25 - s27;        // 001BD982
+    s25 = f_r6;            // 001BD986                 VMOV            S25, R6 ; vec32_4.float_0
+    s4 = s24 - s25;        // 001BD98A                 VSUB.F32        S4, S24, S25
+    s24 = this->field_14;  // 001BD98E                 VLDR            S24, [R0,#0x92+var_80+2] ; this->field_14
+    s2 = s0 * s0;          // 001BD992                 VMUL.F32        S2, S0, S0
+    s0 = s26 - s29;        // 001BD996                 VSUB.F32        S0, S26, S29
+    s26 = this->field_10;  // 001BD99A                 VLDR            S26, [R0,#0x92+var_84+2] ; this->field_10
+    s2 = s2 + s4 * s4;     // 001BD99E                 VMLA.F32        S2, S4, S4
+    s2 = s2 + s0 * s0;     // 001BD9A2                 VMLA.F32        S2, S0, S0
+    s0 = sqrtf(s2);        // 001BD9B8
+
+    s2 = s23 - s27;  // 001BD9C0                 VSUB.F32        S2, S23, S27
+    // cVec32_sp_60.float_0 = s0;   //001BD9C4                 VSTR            S0, [SP,#0xF0+var_92_vec32_94+2] ; sp+0x60,
+    f_sp_60[0] = s0;    // 001BD9C4                 VSTR            S0, [SP,#0xF0+var_92_vec32_94+2] ; sp+0x60,
+    s4 = s19 - s25;     // 001BD9C8                 VSUB.F32        S4, S19, S25
+    s6 = s21 - s29;     // 001BD9CC                 VSUB.F32        S6, S21, S29
+    s2 = s2 * s2;       // 001BD9D0                 VMUL.F32        S2, S2, S2
+    s2 = s2 + s4 * s4;  // 001BD9D4                 VMLA.F32        S2, S4, S4
+    s2 = s2 + s6 * s6;  // 001BD9D8                 VMLA.F32        S2, S6, S6
+    s4 = sqrtf(s2);     // 001BD9EE
+    s0 = s17 / s22;     // 001BD9F6                 VDIV.F32        S0, S17, S22
+    // cVec32_sp_60.float_4 = s4;   //001BD9FE                 VSTR            S4, [SP,#0xF0+var_92_vec32_94+6] ; sp+0x64
+    f_sp_60[1] = s4;  // 001BD9FE                 VSTR            S4, [SP,#0xF0+var_92_vec32_94+6] ; sp+0x64
+
+    int i_r0 = 0;  // 001BD9FC
+    while (i_r0 != 2) {
+      // 001BDA04
+      char c_r2 = f_sp_60[i_r0] > s24 ? 2 : 1;
+      if (f_sp_60[i_r0] < s26) {
+        c_r2 = 0;
+      }
+      c_sp_5E[i_r0] = c_r2;  // 001BDA24
+      i_r0++;                // 001BDA26
+    }
+
+    s2 = s30 / s22;          // 001BDA2C                 VDIV.F32        S2, S30, S22
+    uint c_r2 = c_sp_5E[0];  // 001BDA30   LDRB.W          R2, [SP,#0xF0+var_92_vec32_94] ; sp_5E
+    uint c_r0 = c_sp_5E[1];  // 001BDA34   LDRB.W          R0, [SP,#0xF0+var_92_vec32_94+1] ; sp_5F
+    s4 = s28 / s22;          // 001BDA3A                 VDIV.F32        S4, S28, S22
+    uint c_r1;
+
+    if (c_r2 > c_r0) {
+      // LOGI("clamp 3");
+      s2 = -s2;                      // 001BDA40
+      s0 = -s0;                      // 001BDA46
+      d16_0 = cVec32_sp_68.float_0;  // 001BDA4A
+      d16_4 = cVec32_sp_68.float_4;
+      s4 = -s4;  // 001BDA4E
+      float d17_0 = cVec32_sp_74.float_0;
+      float d17_4 = cVec32_sp_74.float_4;
+      f_sp_50[0] = cVec32_sp_68.float_8;  // 001BDA5C
+      // f_sp_50[1] = cVec32_sp_68.float_8; //001BDA5C
+      cVec32_sp_68.float_8 = cVec32_sp_74.float_8;  // 001BDA5E
+      cVec32_sp_68.float_0 = d17_0;                 // 001BDA60
+      cVec32_sp_68.float_4 = d17_4;
+      cVec32_sp_74.float_8 = f_sp_50[0];  // 001BDA64
+      c_r1 = c_sp_5E[0];                            // 001BDA66
+      c_sp_5E[0] = c_r0 /*c_r0_sp_5F*/;             // 001BDA68
+      f_sp_48[0] = d16_0;                           // 001BDA6C
+      f_sp_48[1] = d16_4;
+      cVec32_sp_74.float_0 = d16_0;  // 001BDA70
+      cVec32_sp_74.float_4 = d16_4;
+      c_sp_5E[1] = c_r2;  // 001BDA74
+    } else {
+      c_r1 = c_sp_5E[1];  // c_r0_sp_5F;
+      c_r0 = c_sp_5E[0];  // c_r2_sp_5E;
+    }
+
+    float LR = s4;  // 001BDA7E
+
+    // float f_r5;
+    uint8 r0_v38 = 0;
+    if (c_r1 | c_r0) {
+      // 001BDA94
+      if (c_r0 == 0 && c_r1 == 1) {
+        r0_v38 = 1;
+      } else if (c_r0 == 0 && c_r1 == 2) {
+        r0_v38 = 2;
+      } else if (c_r0 == 1 && c_r1 == 1) {
+        r0_v38 = 3;
+      } else if (c_r0 == 1 && c_r1 == 2) {
+        r0_v38 = 4;
+      } else {
+        // 001BDAC0
+        r0_v38 = (c_r0 == 2 && c_r1 == 2) ? 5 : -1;
+      }
+    } else {
+      // 001BDA9E
+      r0_v38 = 0;
+    }
+
+    // 001BDAE0 loc_1BDAE0
+    i_sp_3C = r0_v38;  // 001BDAE0
+    // 001BDB0A
+    CFTTVector32 vec_2;
+    vec_2.float_0 = LR;
+    vec_2.float_4 = s0;
+    vec_2.float_8 = s2;
+    i_sp_30 = LineCircleIntersection(cVec32_sp_68, vec_2, s22, this->vec32_4, s26, &f_sp_48[0]);
+    f_sp_44 = LR;  // 001BDB06
+    // 001BDB2E
+    vec_2.float_0 = f_sp_44;
+    vec_2.float_4 = s0;
+    vec_2.float_8 = s2;
+    int ret_r0 = LineCircleIntersection(cVec32_sp_68, vec_2, s22, this->vec32_4, s24, &f_sp_50[0]);
+
+    if ((-3 + (uchar)r0_v38 )>= 2) {
+      // 001BDB48
+      if (((uchar)r0_v38) == 5) {
+        // 001BDB52
+        if ((i_sp_30 != 2) && (ret_r0 != 2)) {
+          // if (ret_r0 != 2) {
+          i_sp_24 = i_sp_24 + (((ret_r0 ^ 2) | i_sp_30) != 0);
+          /*if ((ret_r0 ^ 2) | ret_sp_30 ) {
+            i_sp_24 += 1;
+          }*/
+          //}
+        }
+      } else {
+        // 001BDB6E
+        i_sp_28 = (r0_v38 /*i_sp_3C*/ << 0x18) == 0 ? i_sp_28+1 : i_sp_28;
+      }
+    }
+  }
+
+  // 001BDB80
+  int r1_v45 = s18 <= s20 ? 1 : 0;
+  int r2_v46 = s20 <= s16 ? 1 : 0;
+  // int i_r3 = (uint32)((uint8)i_sp_24) != 4 ? 1 : -4 + (uint32)((uint8)i_sp_24);
+  uint8 i_r3 = i_sp_24 != 4 ? 1 : -4 + (uchar)i_sp_24;
+  // if ((uint32)((uint8)i_sp_28) != 4) {
+  if ((uchar)i_sp_28 != 4) {  // 001BDBB8
+    r1_v45 &= r2_v46;
+    r1_v45 &= i_sp_20;
+    if ((r1_v45 | i_r3)==0) {
+      this->f_1C = 0;
+      return ;
+    }
+  } else {
+    if (r1_v45 | i_r3) {
+      this->f_1C = 0;
+      return ;
+    }
+  }
+
+  if (r1_v45 == 0) {
+    // 001BDBD4
+    s18 = atan2f(((int)this->vec32_4.float_8) ^ 0x80000000, ((int)this->vec32_4.float_0) ^ 0x80000000);
+    s16 = 6.28318548f;  // 001BDBEA
+    s20 = s18 + s16;    // 001BDBF2
+
+    // float* ptrFloat_r4 = &cVec32_sp_80[0].float_8; //001BDBF6
+    float ptrR6_sp68[4];
+    for (int i_r5 = 0; i_r5 != 4; i_r5++) {
+      s4 = cVec32_sp_80[i_r5].float_8;  //*ptrFloat_r4;
+      s6 = cVec32_sp_80[i_r5].float_0;  //*(ptrFloat_r4 - 2);
+      s0 = this->vec32_4.float_0;
+      s2 = this->vec32_4.float_8;
+      s0 = s6 - s0;
+      s2 = s4 - s2;
+      // 001BDC20
+      s0 = atan2f(s2, s0);
+      ptrR6_sp68[i_r5] = s0 < 0.0f ? s0+s16 : s0 ;
+    }
+    s0 = -6.28318548f;  // 001BDC4C
+    int i_r0 = 0;
+    if (s18 < 0.0f) {
+      s18 = s20;  // 001BDC5A
+    }
+    while (i_r0 != 4) {
+      s2 = ptrR6_sp68[i_r0];  // 001BDC62
+      s8 = s2 + s0;
+      s4 = s2 + s16;
+      s6 = s2 - s18;
+      s10 = s8 - s18;
+      s12 = s4 - s18;
+      s4 = fabsf(s6);    // 001BDC7A
+      s10 = fabsf(s10);  // 001BDC7E
+      s6 = fabsf(s12);   // 001BDC82
+      if (s10 < s4) {
+        s2 = s8;
+        s8 = s2 + s16;
+        if (s6 < s4) {
+          s2 = s8;
+        }
+        ptrR6_sp68[i_r0] = s2;  // 001BDCB0
+      }
+      i_r0++;
+    }
+    s0 = 0.0f;  // 001BDCBA
+    i_r0 = 0;
+    while (i_r0 != 4) {
+      s2 = ptrR6_sp68[i_r0];
+      s4 = s0 > s2 ? s0 : s2;
+      s0 = s4;
+      if (s16 > s2) {
+        s16 = s2;
+      }
+      i_r0++;
+    }
+    // s2 =  6.28318548f;
+    // s4 = s0 +  6.28318548f; //s2;
+    // s2 = s16 +  6.28318548f; //s2;
+    if (s16 < 0.0f) {
+      s0 = s0 + 6.28318548f;    // s4;
+      s16 = s16 + 6.28318548f;  // s2;
+    }
+    s0 = s0 - s16;  // 001BDD12
+    Intersect(this->field_18, this->f_1C, s16, s0, this->field_18, this->f_1C);
+  }
 }
 //-------------------------------------------------------------------------------------------------
 //001BE378