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