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) );
}
