bool Matrix3x3::setAffineFromThreePoints(const Point3D &p1, const Point3D &p2, const Point3D &p3, const Point3D &q1, const Point3D &q2, const Point3D &q3) { Matrix3x3 invHp; Matrix3x3 Hp(p1, p2, p3); double detHp = matDeterminant(Hp); if (detHp == 0.) { return false; } Matrix3x3 Hq(q1, q2, q3); double detHq = matDeterminant(Hq); if (detHq == 0.) { return false; } invHp = matInverse(Hp, detHp); *this = matMul(Hq, invHp); return true; }
bool Matrix3x3::setHomographyFromFourPoints(const Point3D &p1, const Point3D &p2, const Point3D &p3, const Point3D &p4, const Point3D &q1, const Point3D &q2, const Point3D &q3, const Point3D &q4) { Matrix3x3 invHp; Matrix3x3 Hp( crossprod( crossprod(p1, p2), crossprod(p3, p4) ), crossprod( crossprod(p1, p3), crossprod(p2, p4) ), crossprod( crossprod(p1, p4), crossprod(p2, p3) ) ); double detHp = matDeterminant(Hp); if (detHp == 0.) { return false; } Matrix3x3 Hq( crossprod( crossprod(q1, q2), crossprod(q3, q4) ), crossprod( crossprod(q1, q3), crossprod(q2, q4) ), crossprod( crossprod(q1, q4), crossprod(q2, q3) ) ); double detHq = matDeterminant(Hq); if (detHq == 0.) { return false; } invHp = matInverse(Hp, detHp); *this = matMul(Hq, invHp); return true; }
}END_TEST START_TEST(test_correct_code_matDeterminant) { fl64 ** A; fl64 det = 0; fl64 size = 3; si32 retval = 0; fl64 A1[3] = { 8, 1, 6 }; fl64 A2[3] = { 3, 5, 7 }; fl64 A3[3] = { 4, 9, 2 }; A = (fl64**) malloc(3 * sizeof(fl64*)); A[0] = A1; A[1] = A2; A[2] = A3; retval = matDeterminant(&det, A, size); ck_assert_int_eq(retval, EXIT_SUCCESS); ck_assert((det - (-360)) < DBL_EPSILON); free(A); }END_TEST
Matrix3x3 matInverse(const Matrix3x3 & M) { return matScaleAdjoint( M, 1. / matDeterminant(M) ); }