95 lines
2.4 KiB
C
95 lines
2.4 KiB
C
|
#include "four_point_transform.h"
|
||
|
|
#include "singular_value_decomposition.h"
|
||
|
|
|
||
|
|
/* Make a 4x4 matrix that maps
|
||
|
|
* e1 -> p1
|
||
|
|
* e2 -> p3
|
||
|
|
* e3 -> p3
|
||
|
|
* (1,1,1,0) -> p4
|
||
|
|
*/
|
||
|
|
static void
|
||
|
|
unit_to (graphene_point3d_t *p1,
|
||
|
|
graphene_point3d_t *p2,
|
||
|
|
graphene_point3d_t *p3,
|
||
|
|
graphene_point3d_t *p4,
|
||
|
|
graphene_matrix_t *m)
|
||
|
|
{
|
||
|
|
graphene_vec3_t v1, v2, v3, v4;
|
||
|
|
graphene_vec4_t vv1, vv2, vv3, vv4, p;
|
||
|
|
graphene_matrix_t u, s;
|
||
|
|
float v[16] = { 0., };
|
||
|
|
double A[16];
|
||
|
|
double U[16];
|
||
|
|
double S[4];
|
||
|
|
double V[16];
|
||
|
|
double B[4];
|
||
|
|
double x[4];
|
||
|
|
int i, j;
|
||
|
|
|
||
|
|
graphene_point3d_to_vec3 (p1, &v1);
|
||
|
|
graphene_point3d_to_vec3 (p2, &v2);
|
||
|
|
graphene_point3d_to_vec3 (p3, &v3);
|
||
|
|
graphene_point3d_to_vec3 (p4, &v4);
|
||
|
|
|
||
|
|
graphene_vec4_init_from_vec3 (&vv1, &v1, 1.);
|
||
|
|
graphene_vec4_init_from_vec3 (&vv2, &v2, 1.);
|
||
|
|
graphene_vec4_init_from_vec3 (&vv3, &v3, 1.);
|
||
|
|
graphene_vec4_init_from_vec3 (&vv4, &v4, 1.);
|
||
|
|
|
||
|
|
graphene_vec4_init (&p, 0., 0., 0., 1.);
|
||
|
|
|
||
|
|
graphene_matrix_init_from_vec4 (&u, &vv1, &vv2, &vv3, &p);
|
||
|
|
|
||
|
|
/* solve x * u = vv4 */
|
||
|
|
|
||
|
|
for (i = 0; i < 4; i++)
|
||
|
|
for (j = 0; j < 4; j++)
|
||
|
|
A[j * 4 + i] = graphene_matrix_get_value (&u, i, j);
|
||
|
|
|
||
|
|
B[0] = graphene_vec4_get_x (&vv4);
|
||
|
|
B[1] = graphene_vec4_get_y (&vv4);
|
||
|
|
B[2] = graphene_vec4_get_z (&vv4);
|
||
|
|
B[3] = graphene_vec4_get_w (&vv4);
|
||
|
|
|
||
|
|
singular_value_decomposition (A, 4, 4, U, S, V);
|
||
|
|
singular_value_decomposition_solve (U, S, V, 4, 4, B, x);
|
||
|
|
|
||
|
|
v[ 0] = x[0];
|
||
|
|
v[ 5] = x[1];
|
||
|
|
v[10] = x[2];
|
||
|
|
v[15] = 1;
|
||
|
|
|
||
|
|
graphene_matrix_init_from_float (&s, (const float *)&v);
|
||
|
|
graphene_matrix_multiply (&s, &u, m);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Compute a 4x4 matrix m that maps
|
||
|
|
* p1 -> q1
|
||
|
|
* p2 -> q2
|
||
|
|
* p3 -> q3
|
||
|
|
* p4 -> q4
|
||
|
|
*
|
||
|
|
* This is not in general possible, because projective
|
||
|
|
* transforms preserve coplanarity. But in the cases we
|
||
|
|
* care about here, both sets of points are always coplanar.
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
perspective_3d (graphene_point3d_t *p1,
|
||
|
|
graphene_point3d_t *p2,
|
||
|
|
graphene_point3d_t *p3,
|
||
|
|
graphene_point3d_t *p4,
|
||
|
|
graphene_point3d_t *q1,
|
||
|
|
graphene_point3d_t *q2,
|
||
|
|
graphene_point3d_t *q3,
|
||
|
|
graphene_point3d_t *q4,
|
||
|
|
graphene_matrix_t *m)
|
||
|
|
{
|
||
|
|
graphene_matrix_t a, a_inv, b;
|
||
|
|
|
||
|
|
unit_to (p1, p2, p3, p4, &a);
|
||
|
|
unit_to (q1, q2, q3, q4, &b);
|
||
|
|
|
||
|
|
graphene_matrix_inverse (&a, &a_inv);
|
||
|
|
graphene_matrix_multiply (&a_inv, &b, m);
|
||
|
|
}
|