/** * 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; }