Studio_SolveIK

/* Studio_SolveIK(int, int, int, Vector&, Vector&, Vector&, matrix3x4_t*) */

undefined4
Studio_SolveIK(int param_1,int param_2,int param_3,Vector *param_4,Vector *param_5,Vector *param_6,
              matrix3x4_t *param_7)

{
  float fVar1;
  float fVar2;
  float fVar3;
  int iVar4;
  float *pfVar6;
  matrix3x4_t *pmVar7;
  matrix3x4_t *pmVar8;
  float fVar9;
  float fVar10;
  float fVar11;
  undefined1 auVar12 [16];
  float fVar13;
  float fVar14;
  float fVar15;
  float fVar16;
  float fVar17;
  float fVar18;
  float fVar19;
  float local_b4;
  float local_94;
  float local_90;
  float local_8c;
  float local_88;
  float local_84;
  float local_80;
  float local_7c;
  float local_78;
  float local_74;
  float local_70;
  float local_6c;
  float local_68;
  float local_64;
  float local_60;
  float local_58;
  float local_54;
  float local_4c;
  float local_48;
  float local_40 [4];
  float local_30;
  float local_2c;
  float local_28;
  float local_24;
  float local_20;
  int iVar5;
  
  pmVar8 = param_7 + param_1 * 0x30;
  fVar1 = *(float *)(pmVar8 + 0xc);
  pmVar7 = param_7 + param_2 * 0x30;
  fVar2 = *(float *)(pmVar8 + 0x1c);
  fVar3 = *(float *)(pmVar8 + 0x2c);
  fVar11 = *(float *)(param_7 + param_3 * 0x30 + 0x1c);
  fVar17 = *(float *)(param_7 + param_3 * 0x30 + 0xc);
  local_94 = *(float *)param_4 - fVar1;
  fVar14 = *(float *)(param_7 + param_3 * 0x30 + 0x2c);
  local_7c = *(float *)param_5 - fVar1;
  local_90 = *(float *)(param_4 + 4) - fVar2;
  local_8c = *(float *)(param_4 + 8) - fVar3;
  local_74 = *(float *)(param_5 + 8) - fVar3;
  local_78 = *(float *)(param_5 + 4) - fVar2;
  fVar15 = *(float *)(pmVar7 + 0xc) - fVar1;
  fVar13 = fVar17 - *(float *)(pmVar7 + 0xc);
  fVar18 = fVar14 - *(float *)(pmVar7 + 0x2c);
  fVar9 = SQRT((*(float *)(pmVar7 + 0x1c) - fVar2) * (*(float *)(pmVar7 + 0x1c) - fVar2) +
               fVar15 * fVar15 +
               (*(float *)(pmVar7 + 0x2c) - fVar3) * (*(float *)(pmVar7 + 0x2c) - fVar3));
  fVar15 = fVar11 - *(float *)(pmVar7 + 0x1c);
  fVar13 = SQRT(fVar15 * fVar15 + fVar13 * fVar13 + fVar18 * fVar18);
  fVar18 = local_90 * local_90 + local_94 * local_94 + local_8c * local_8c;
  fVar15 = fVar13;
  if (fVar9 <= fVar13) {
    fVar15 = fVar9;
  }
  fVar10 = SQRT(fVar18);
  fVar19 = fVar13 + fVar9;
  fVar16 = fVar10 - fVar15;
  if (fVar10 - fVar15 <= fVar19) {
    fVar16 = fVar19;
  }
  fVar16 = fVar16 * 100.0;
  local_84 = *(float *)(param_6 + 4) * fVar16 + local_78;
  local_80 = *(float *)(param_6 + 8) * fVar16 + local_74;
  local_88 = *(float *)param_6 * fVar16 + local_7c;
  fVar19 = fVar19 * 0.9998;
  if (fVar19 < fVar10) {
    VectorNormalize((Vector *)&local_94);
    local_94 = local_94 * fVar19;
    local_90 = local_90 * fVar19;
    local_8c = fVar19 * local_8c;
    fVar18 = local_90 * local_90 + local_94 * local_94 + local_8c * local_8c;
    fVar10 = SQRT(fVar18);
  }
  fVar19 = ABS(fVar9 - fVar13) * 1.15;
  if (fVar19 <= fVar15 * 0.15) {
    fVar19 = fVar15 * 0.15;
  }
  local_b4 = local_94;
  if (fVar10 < fVar19) {
    local_94 = fVar17 - fVar1;
    local_90 = fVar11 - fVar2;
    local_8c = fVar14 - fVar3;
    VectorNormalize((Vector *)&local_94);
    fVar10 = local_94 * fVar19;
    local_90 = local_90 * fVar19;
    local_8c = fVar19 * local_8c;
    fVar18 = local_90 * local_90 + fVar10 * fVar10 + local_8c * local_8c;
    local_b4 = fVar10;
  }
  auVar12 = rsqrtss(ZEXT416((uint)fVar10),ZEXT416((uint)fVar18));
  fVar11 = auVar12._0_4_;
  fVar18 = (fVar18 * fVar11 * fVar11 + -3.0) * fVar11 * -0.5;
  fVar15 = local_b4 * fVar18;
  fVar19 = local_90 * fVar18;
  fVar18 = fVar18 * local_8c;
  local_40[1] = fVar19;
  local_40[2] = fVar18;
  fVar11 = local_84 * fVar19 + fVar15 * local_88 + local_80 * fVar18;
  fVar17 = local_88 - fVar11 * fVar15;
  local_30 = local_84 - fVar11 * fVar19;
  fVar14 = local_80 - fVar11 * fVar18;
  fVar11 = fVar17 * fVar17 + fVar14 * fVar14 + local_30 * local_30;
  auVar12 = rsqrtss(ZEXT416((uint)(local_30 * local_30)),ZEXT416((uint)fVar11));
  fVar16 = auVar12._0_4_;
  local_2c = (fVar11 * fVar16 * fVar16 + -3.0) * fVar16 * -0.5;
  fVar17 = fVar17 * local_2c;
  local_30 = local_30 * local_2c;
  local_2c = local_2c * fVar14;
  fVar14 = local_2c * fVar19 - local_30 * fVar18;
  local_24 = fVar17 * fVar18 - local_2c * fVar15;
  local_20 = local_30 * fVar15 - fVar17 * fVar19;
  iVar5 = 0;
  pfVar6 = &local_64;
  fVar11 = fVar15;
  local_40[0] = fVar15;
  local_40[3] = fVar17;
  local_28 = fVar14;
  while( true ) {
    iVar4 = iVar5 + 1;
    *pfVar6 = fVar11;
    pfVar6[1] = fVar17;
    pfVar6[2] = fVar14;
    if (iVar4 == 3) break;
    fVar11 = local_40[iVar4];
    fVar17 = local_40[iVar5 + 4];
    fVar14 = local_40[iVar5 + 7];
    iVar5 = iVar4;
    pfVar6 = pfVar6 + 3;
  }
  local_70 = fVar19 * local_90 + fVar15 * local_b4 + fVar18 * local_8c;
  local_6c = local_30 * local_90 + local_2c * local_8c + local_40[3] * local_b4;
  local_68 = local_24 * local_90 + local_28 * local_b4 + local_20 * local_8c;
  fVar17 = SQRT(local_68 * local_68 + local_6c * local_6c + local_70 * local_70);
  fVar11 = ((fVar9 * fVar9 - fVar13 * fVar13) / fVar17 + fVar17) * 0.5;
  fVar14 = SQRT(fVar9 * fVar9 - fVar11 * fVar11);
  local_7c = local_64 * fVar11 + local_60 * fVar14;
  local_78 = local_58 * fVar11 + local_54 * fVar14;
  local_74 = local_4c * fVar11 + fVar14 * local_48;
  if ((fVar17 - fVar13 < fVar11) && (fVar11 < fVar9)) {
    local_94 = local_b4;
    Studio_AlignIKMatrix(pmVar8,(Vector *)&local_7c);
    local_70 = local_94 - local_7c;
    local_6c = local_90 - local_78;
    local_68 = local_8c - local_74;
    Studio_AlignIKMatrix(pmVar7,(Vector *)&local_70);
    *(float *)(pmVar7 + 0xc) = local_7c + fVar1;
    *(float *)(pmVar7 + 0x1c) = local_78 + fVar2;
    *(float *)(pmVar7 + 0x2c) = local_74 + fVar3;
    *(float *)(param_7 + param_3 * 0x30 + 0xc) = local_94 + fVar1;
    *(float *)(param_7 + param_3 * 0x30 + 0x1c) = local_90 + fVar2;
    *(float *)(param_7 + param_3 * 0x30 + 0x2c) = local_8c + fVar3;
    return 1;
  }
  return 0;
}