CIKContext::AddAllLocks

/* CIKContext::AddAllLocks(Vector*, Quaternion*) */

void __thiscall CIKContext::AddAllLocks(CIKContext *this,Vector *param_1,Quaternion *param_2)

{
  matrix3x4_t *pmVar1;
  char cVar2;
  matrix3x4_t *pmVar3;
  int iVar4;
  int iVar5;
  int iVar6;
  CStudioHdr *pCVar7;
  int *piVar8;
  int iVar9;
  int local_30;
  undefined4 local_2c;
  undefined4 local_28;
  undefined4 local_24;
  undefined4 local_20;
  
  if (*(int *)(**(int **)(this + 0xff8) + 0x11c) == 0) {
    return;
  }
  do {
    pmVar3 = g_MatrixPool;
    if (g_MatrixPool == (matrix3x4_t *)0x0) {
      pmVar3 = (matrix3x4_t *)memalign(0x10,0x1800);
      if (((uint)pmVar3 & 0xf) != 0) {
        raise(5);
      }
      break;
    }
    cVar2 = ThreadInterlockedAssignIf64
                      (&g_MatrixPool,*(undefined4 *)g_MatrixPool,DAT_01047874 + -1,g_MatrixPool,
                       DAT_01047874);
  } while (cVar2 == '\0');
  local_2c = 0;
  local_28 = 0;
  local_24 = 0;
  local_20 = 0;
  iVar9 = *(int *)(**(int **)(this + 0xff8) + 0x11c);
  iVar6 = *(int *)(this + 0x101c);
  iVar4 = 0;
  if (iVar9 != 0) {
    CUtlVector<ikcontextikrule_t,CUtlMemory<ikcontextikrule_t,int>>::GrowVector
              ((CUtlVector<ikcontextikrule_t,CUtlMemory<ikcontextikrule_t,int>> *)(this + 0x1010),
               iVar9);
    CUtlVector<ikcontextikrule_t,CUtlMemory<ikcontextikrule_t,int>>::ShiftElementsRight
              ((CUtlVector<ikcontextikrule_t,CUtlMemory<ikcontextikrule_t,int>> *)(this + 0x1010),
               iVar6,iVar9);
    iVar4 = *(int *)(**(int **)(this + 0xff8) + 0x11c);
  }
  iVar6 = iVar6 * 0x84;
  memset((void *)(*(int *)(this + 0x1010) + iVar6),0,iVar4 * 0x84);
  pCVar7 = *(CStudioHdr **)(this + 0xff8);
  iVar9 = *(int *)pCVar7;
  if (0 < *(int *)(iVar9 + 0x11c)) {
    local_30 = 0;
    do {
      iVar9 = local_30 * 0x10 + *(int *)(iVar9 + 0x120) + iVar9;
      iVar4 = *(int *)(iVar9 + 0x38 + *(int *)(iVar9 + 0xc));
      if ((*(uint *)(this + 0x105c) & *(uint *)(*(int *)(pCVar7 + 0x2c) + iVar4 * 4)) != 0) {
        ::BuildBoneChain(pCVar7,(matrix3x4_t *)(this + 0x1024),param_1,param_2,iVar4,pmVar3,
                         (CBoneBitList *)&local_2c);
        iVar5 = iVar6 + *(int *)(this + 0x1010);
        *(int *)(iVar5 + 8) = local_30;
        *(int *)(iVar5 + 0x10) = local_30;
        *(undefined4 *)(iVar5 + 4) = 2;
        MatrixAngles(pmVar3 + iVar4 * 0x30,(Quaternion *)(iVar5 + 0x2c),(Vector *)(iVar5 + 0x20));
        piVar8 = (int *)(*(int *)(iVar9 + 0xc) + iVar9);
        if ((float)piVar8[1] * (float)piVar8[1] + (float)piVar8[3] * (float)piVar8[3] +
            (float)piVar8[2] * (float)piVar8[2] <= 0.0) {
          *(undefined4 *)(iVar5 + 0x6c) = 0;
          *(undefined4 *)(iVar5 + 0x70) = 0;
          *(undefined4 *)(iVar5 + 0x74) = 0;
          pCVar7 = *(CStudioHdr **)(this + 0xff8);
        }
        else {
          VectorRotate((float *)(piVar8 + 1),pmVar3 + *piVar8 * 0x30,(float *)(iVar5 + 0x6c));
          iVar9 = *(int *)(iVar9 + 0x1c + *(int *)(iVar9 + 0xc));
          *(undefined4 *)(iVar5 + 0x78) = *(undefined4 *)(pmVar3 + iVar9 * 0x30 + 0xc);
          *(undefined4 *)(iVar5 + 0x7c) = *(undefined4 *)(pmVar3 + iVar9 * 0x30 + 0x1c);
          *(undefined4 *)(iVar5 + 0x80) = *(undefined4 *)(pmVar3 + iVar9 * 0x30 + 0x2c);
          pCVar7 = *(CStudioHdr **)(this + 0xff8);
        }
      }
      local_30 = local_30 + 1;
      iVar6 = iVar6 + 0x84;
      iVar9 = *(int *)pCVar7;
    } while (local_30 < *(int *)(iVar9 + 0x11c));
  }
  do {
    iVar9 = DAT_01047874;
    pmVar1 = g_MatrixPool;
    iVar6 = DAT_01047874 + 0x10001;
    *(matrix3x4_t **)pmVar3 = g_MatrixPool;
    cVar2 = ThreadInterlockedAssignIf64(&g_MatrixPool,pmVar3,iVar6,pmVar1,iVar9);
  } while (cVar2 == '\0');
  return;
}