void calcObb(Obb& _obb, const void* _vertices, uint32_t _numVertices, uint32_t _stride, uint32_t _steps)
{
Aabb aabb;
calcAabb(aabb, _vertices, _numVertices, _stride);
float minArea = calcAreaAabb(aabb);
Obb best;
aabbToObb(best, aabb);
float angleStep = float(M_PI_2/_steps);
float ax = 0.0f;
float mtx[16];
for (uint32_t ii = 0; ii < _steps; ++ii)
{
float ay = 0.0f;
for (uint32_t jj = 0; jj < _steps; ++jj)
{
float az = 0.0f;
for (uint32_t kk = 0; kk < _steps; ++kk)
{
mtxRotateXYZ(mtx, ax, ay, az);
float mtxT[16];
mtxTranspose(mtxT, mtx);
calcAabb(aabb, mtxT, _vertices, _numVertices, _stride);
float area = calcAreaAabb(aabb);
if (area < minArea)
{
minArea = area;
aabbTransformToObb(best, aabb, mtx);
}
az += angleStep;
}
ay += angleStep;
}
ax += angleStep;
}
memcpy(&_obb, &best, sizeof(Obb) );
}
static MTXMatrix _getEigenvaluesIteration (MTXMatrix matrix) {
int i, j, n = 0;
int rotation_m_len = ((int) pow(matrix->rows, 2) - matrix->rows) / 2;
MTXMatrix aux = NULL, Q = NULL, R = NULL, result = NULL;
MTXMatrix rotation_matrices[rotation_m_len];
/* Calculate rotations matrices */
for (i = 0; i < matrix->rows; i ++)
for (j = 0; j < matrix->columns; j ++)
if (j < i) {
rotation_matrices[n] = mtxGetRotationMatrix(matrix, i, j);
n ++;
}
/* Begin calculate R matrix */
R = mtxNew(matrix->rows, matrix->columns);
for (i = n - 1; i >= 0; i --) {
aux = R;
R = mtxMul(R, rotation_matrices[i]);
mtxDestroy(&aux);
}
aux = R;
R = mtxMul(R, matrix);
mtxDestroy(&aux);
/* Begin calculate Q matrix */
Q = mtxNew(matrix->rows, matrix->columns);
for (i = 0; i < n; i ++) {
aux = Q;
Q = mtxMul(aux, mtxTranspose(rotation_matrices[i]));
mtxDestroy(&aux);
}
/* Destroy rotations matrices */
for (i = 0; i < rotation_m_len; i ++) {
aux = rotation_matrices[i];
mtxDestroy(&aux);
}
result = mtxMul(R, Q);
mtxDestroy(&R);
mtxDestroy(&Q);
return result;
}
void mtxInvert(float* mtx, const float* src)
{
float tmp[16];
float val, val2, val_inv, zero, one;
int i, j, i4, i8, i12, ind;
mtxTranspose(tmp, src);
LoadIdentity(mtx);
for(i = 0; i != 4; i++)
{
val = tmp[(i << 2) + i];
ind = i;
i4 = i + 4;
i8 = i + 8;
i12 = i + 12;
for (j = i + 1; j != 4; j++)
{
if(fabsf(tmp[(i << 2) + j]) > fabsf(val))
{
ind = j;
val = tmp[(i << 2) + j];
}
}
if(ind != i)
{
val2 = mtx[i];
mtx[i] = mtx[ind];
mtx[ind] = val2;
val2 = tmp[i];
tmp[i] = tmp[ind];
tmp[ind] = val2;
ind += 4;
val2 = mtx[i4];
mtx[i4] = mtx[ind];
mtx[ind] = val2;
val2 = tmp[i4];
tmp[i4] = tmp[ind];
tmp[ind] = val2;
ind += 4;
val2 = mtx[i8];
mtx[i8] = mtx[ind];
mtx[ind] = val2;
val2 = tmp[i8];
tmp[i8] = tmp[ind];
tmp[ind] = val2;
ind += 4;
val2 = mtx[i12];
mtx[i12] = mtx[ind];
mtx[ind] = val2;
val2 = tmp[i12];
tmp[i12] = tmp[ind];
tmp[ind] = val2;
}
if(val == zero)
{
LoadIdentity(mtx);
return;
}
val_inv = one / val;
tmp[i] *= val_inv;
mtx[i] *= val_inv;
tmp[i4] *= val_inv;
mtx[i4] *= val_inv;
tmp[i8] *= val_inv;
mtx[i8] *= val_inv;
tmp[i12] *= val_inv;
mtx[i12] *= val_inv;
if(i != 0)
{
val = tmp[i << 2];
tmp[0] -= tmp[i] * val;
mtx[0] -= mtx[i] * val;
tmp[4] -= tmp[i4] * val;
mtx[4] -= mtx[i4] * val;
tmp[8] -= tmp[i8] * val;
mtx[8] -= mtx[i8] * val;
tmp[12] -= tmp[i12] * val;
mtx[12] -= mtx[i12] * val;
}
if(i != 1)
{
val = tmp[(i << 2) + 1];
tmp[1] -= tmp[i] * val;
mtx[1] -= mtx[i] * val;
tmp[5] -= tmp[i4] * val;
mtx[5] -= mtx[i4] * val;
tmp[9] -= tmp[i8] * val;
mtx[9] -= mtx[i8] * val;
tmp[13] -= tmp[i12] * val;
mtx[13] -= mtx[i12] * val;
}
if(i != 2)
{
val = tmp[(i << 2) + 2];
tmp[2] -= tmp[i] * val;
mtx[2] -= mtx[i] * val;
tmp[6] -= tmp[i4] * val;
mtx[6] -= mtx[i4] * val;
tmp[10] -= tmp[i8] * val;
mtx[10] -= mtx[i8] * val;
tmp[14] -= tmp[i12] * val;
mtx[14] -= mtx[i12] * val;
}
if(i != 3)
{
val = tmp[(i << 2) + 3];
tmp[3] -= tmp[i] * val;
mtx[3] -= mtx[i] * val;
tmp[7] -= tmp[i4] * val;
mtx[7] -= mtx[i4] * val;
tmp[11] -= tmp[i8] * val;
mtx[11] -= mtx[i8] * val;
tmp[15] -= tmp[i12] * val;
mtx[15] -= mtx[i12] * val;
}
}
}
