/**
* vsg_matrix3@t@_invert:
* @mat: a #VsgMatrix3@t@
* @result: a #VsgMatrix3@t@
*
* Computes matrix inversion of @mat and stores the result in @result.
* Argument aliasing is allowed.
*
* Returns: a flag indicating success of the inversion.
*/
gboolean vsg_matrix3@t@_invert (const VsgMatrix3@t@ *mat,
VsgMatrix3@t@ *result)
{
@type@ s;
#ifdef VSG_CHECK_PARAMS
g_return_val_if_fail (mat != NULL, FALSE);
g_return_val_if_fail (result != NULL, FALSE);
#endif
s = vsg_matrix3@t@_det (mat);
if (s == 0.0)
{
g_critical ("invalid VsgMatrix3@t@ for inversion.\n");
return FALSE;
}
s = 1 / s;
vsg_matrix3@t@_set (result,
s * (C11 * C22 - C12 * C21),
s * (C21 * C02 - C22 * C01),
s * (C01 * C12 - C02 * C11),
s * (C12 * C20 - C10 * C22),
s * (C22 * C00 - C20 * C02),
s * (C02 * C10 - C00 * C12),
s * (C10 * C21 - C11 * C20),
s * (C20 * C01 - C21 * C00),
s * (C00 * C11 - C01 * C10));
return TRUE;
}
/**
* Calculate matrix determinant (naive implementation).
* @return matrix determinant.
*/
virtual real_t det() const
{
return _det(m_elements, N);
}
static void fitLine3D_wods( const Point3f * points, int count, float *weights, float *line )
{
int i;
float w0 = 0;
float x0 = 0, y0 = 0, z0 = 0;
float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0;
float dx2, dy2, dz2, dxy, dxz, dyz;
float *v;
float n;
float det[9], evc[9], evl[3];
memset( evl, 0, 3*sizeof(evl[0]));
memset( evc, 0, 9*sizeof(evl[0]));
if( weights )
{
for( i = 0; i < count; i++ )
{
float x = points[i].x;
float y = points[i].y;
float z = points[i].z;
float w = weights[i];
x2 += x * x * w;
xy += x * y * w;
xz += x * z * w;
y2 += y * y * w;
yz += y * z * w;
z2 += z * z * w;
x0 += x * w;
y0 += y * w;
z0 += z * w;
w0 += w;
}
}
else
{
for( i = 0; i < count; i++ )
{
float x = points[i].x;
float y = points[i].y;
float z = points[i].z;
x2 += x * x;
xy += x * y;
xz += x * z;
y2 += y * y;
yz += y * z;
z2 += z * z;
x0 += x;
y0 += y;
z0 += z;
}
w0 = (float) count;
}
x2 /= w0;
xy /= w0;
xz /= w0;
y2 /= w0;
yz /= w0;
z2 /= w0;
x0 /= w0;
y0 /= w0;
z0 /= w0;
dx2 = x2 - x0 * x0;
dxy = xy - x0 * y0;
dxz = xz - x0 * z0;
dy2 = y2 - y0 * y0;
dyz = yz - y0 * z0;
dz2 = z2 - z0 * z0;
det[0] = dz2 + dy2;
det[1] = -dxy;
det[2] = -dxz;
det[3] = det[1];
det[4] = dx2 + dz2;
det[5] = -dyz;
det[6] = det[2];
det[7] = det[5];
det[8] = dy2 + dx2;
// Searching for a eigenvector of det corresponding to the minimal eigenvalue
Mat _det( 3, 3, CV_32F, det );
Mat _evc( 3, 3, CV_32F, evc );
Mat _evl( 3, 1, CV_32F, evl );
eigen( _det, _evl, _evc );
i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2);
v = &evc[i * 3];
n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] );
n = (float)MAX(n, eps);
line[0] = v[0] / n;
line[1] = v[1] / n;
line[2] = v[2] / n;
line[3] = x0;
line[4] = y0;
line[5] = z0;
}
