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;
}
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;
}
}END_TEST
START_TEST(test_correct_code_matInverse)
{
fl64 ** A;
fl64 ** Ainv;
fl64 Sol[4][4] = { { -1, 0, 2, -1 }, { 0.39, -0.01, -0.97, 0.79 }, {
0.34, -0.06, 0.18, -0.26 }, { 0.05, 0.05, -0.15, 0.05 } };
fl64 size = 4;
si32 retval = 0;
si32 i = 0, j = 0;
fl64 A1[4] = { 1, 2, 3, 4 };
fl64 A2[4] = { 3, 3, 2, 23 };
fl64 A3[4] = { 2, 3, 2, 3 };
fl64 A4[4] = { 2, 4, 1, 2 };
A = (fl64**) malloc(size * sizeof(fl64*));
A[0] = A1;
A[1] = A2;
A[2] = A3;
A[3] = A4;
Ainv = (fl64**) malloc(size * sizeof(fl64*));
for (i = 0; i < size; i++) {
Ainv[i] = (fl64*) malloc(size * sizeof(fl64));
}
retval = matInverse(Ainv, A, size);
ck_assert_int_eq(retval, EXIT_SUCCESS);
for (i = 0; i < size; i++) {
for (j = 0; j < size; j++) {
ck_assert(Ainv[i][j]-Sol[i][j]<DBL_EPSILON);
}
}
free(A);
for (i = 0; i < size; i++) {
free(Ainv[i]);
}
free(Ainv);
}END_TEST
ray Mouse::getRay(const Camera & camera, float win_width, float win_height)
{
ray myRay;
float x = (2.0f * *mouseX) / win_width - 1.0f;
float y = 1.0f - (2.0f * *mouseY) / win_height;
float z = -1.0f;
vec rayNds = getVec(x, y, z);
float rayClip[] = {rayNds.x, rayNds.y, z, 1.0};
float rayEye[4];
float tempInverse[16];
float rayWorld[4];
matInverse(&tempInverse[0]);
matMultVec4fUtil(rayEye, rayClip, &tempInverse[0]);
rayEye[2] = z;
rayEye[3] = 0.0;
matMultVec4fUtil(rayWorld, rayEye, &tempInverse[0]);
vec4 rayWor = getVec4(rayWorld[0], rayWorld[1], rayWorld[2], rayWorld[3]);
// don't forget to normalise the vector at some point
rayWor = vec4Normalize(rayWor);
myRay.origin.x = camera.position.x;
myRay.origin.y = camera.position.y;
myRay.origin.z = camera.position.z;
myRay.direction.x = rayWor.x;
myRay.direction.y = rayWor.y;
myRay.direction.z = rayWor.z;
myRay.direction.w = rayWor.w;
return myRay;
}
void kfUpdate(float *xl, float *omega) {
static float phi = 0;
static float theta = 0;
static float psi = 0;
float dphi, dtheta, dP;
float sin_theta;
float cos_phi = cos(phi);
float sin_phi = sin(phi);
float cos_theta = cos(theta);
float tan_theta = tan(theta);
float a_values[] = { -wy*cos_phi*tan_theta + wz*sin_phi*tan_theta,
(-wy*sin_phi + wz*cos_phi) / (cos_theta*cos_theta),
wy*sin_phi - wz*cos_phi,
0 };
float c_values[6];
float h_values[3];
matAssignValues(A, a_values);
dphi = wx - wy*sin_phi*tan_theta - wz*cos_phi*tan_theta;
dtheta = -wy*cos_phi - wz*sin_phi;
phi = phi + dphi * TIME_STEP;
theta = theta + dtheta * TIME_STEP;
// computing dP = APA' + Q
matTranspose(temp2x2, A); // A'
matDotProduct(temp2x2, P, temp2x2); // PA'
matDotProduct(temp2x2, A, temp2x2); // APA'
matAdd(dP, temp2x2, Q); // APA' + Q
// computing P = P + dt*dP
matScale(temp2x2, dP, TIME_STEP); // dt*dP
matAdd(P, P, temp2x2); // P + dt*dP
cos_phi = cos(phi);
sin_phi = sin(phi);
cos_theta = cos(theta);
sin_theta = sin(theta);
c_values = {0, cos_theta,
-cos_theta*cos_phi, sin_theta*sin_phi,
cos_theta*sin_phi, sin_theta*cos_phi }
matAssignValues(C, c_values);
// L = PC'(R + CPC')^-1
matTranspose(temp2x3, C); // C'
matDotProduct(temp2x3, P, temp2x3); // PC'
matDotProduct(temp3x3, C, temp2x3); // CPC'
matAdd(temp3x3, R, temp3x3); // R + CPC'
matInverse(temp3x3, temp3x3); // (R + CPC')^-1
matDotProduct(L, temp2x3, temp3x3); // PC'(R + CPC')^-1
// P = (I - LC)P
matDotProduct(temp2x2, L, C); // LC
matSub(temp2x2, I, temp2x2); // I - LC
matDotProduct(P, temp2x2, P); // (I - LC)P
h_values = {sin_theta, -cos_theta*sin_phi, -cos_theta*cos_phi};
matAssignValues(H, h_values);
/*
ph = ph + dot(L[0], self.ab - h)
th = th + dot(L[1], self.ab - h)
*/
// change the values so that they stay between -PI and +PI
phi = ((phi + PI) % (2*PI)) - PI;
theta = ((theta + PI) % (2*PI)) - PI;
psidot = wy * sin(ph) / cos(th) + * cos(ph) / cos(th);
psi += psidot * TIME_STEP;
}
