1 | #include "four_point_transform.h" |
2 | #include "singular_value_decomposition.h" |
3 | |
4 | /* Make a 4x4 matrix that maps |
5 | * e1 -> p1 |
6 | * e2 -> p3 |
7 | * e3 -> p3 |
8 | * (1,1,1,0) -> p4 |
9 | */ |
10 | static void |
11 | unit_to (graphene_point3d_t *p1, |
12 | graphene_point3d_t *p2, |
13 | graphene_point3d_t *p3, |
14 | graphene_point3d_t *p4, |
15 | graphene_matrix_t *m) |
16 | { |
17 | graphene_vec3_t v1, v2, v3, v4; |
18 | graphene_vec4_t vv1, vv2, vv3, vv4, p; |
19 | graphene_matrix_t u, s; |
20 | float v[16] = { 0., }; |
21 | double A[16]; |
22 | double U[16]; |
23 | double S[4]; |
24 | double V[16]; |
25 | double B[4]; |
26 | double x[4]; |
27 | int i, j; |
28 | |
29 | graphene_point3d_to_vec3 (p: p1, v: &v1); |
30 | graphene_point3d_to_vec3 (p: p2, v: &v2); |
31 | graphene_point3d_to_vec3 (p: p3, v: &v3); |
32 | graphene_point3d_to_vec3 (p: p4, v: &v4); |
33 | |
34 | graphene_vec4_init_from_vec3 (v: &vv1, src: &v1, w: 1.); |
35 | graphene_vec4_init_from_vec3 (v: &vv2, src: &v2, w: 1.); |
36 | graphene_vec4_init_from_vec3 (v: &vv3, src: &v3, w: 1.); |
37 | graphene_vec4_init_from_vec3 (v: &vv4, src: &v4, w: 1.); |
38 | |
39 | graphene_vec4_init (v: &p, x: 0., y: 0., z: 0., w: 1.); |
40 | |
41 | graphene_matrix_init_from_vec4 (m: &u, v0: &vv1, v1: &vv2, v2: &vv3, v3: &p); |
42 | |
43 | /* solve x * u = vv4 */ |
44 | |
45 | for (i = 0; i < 4; i++) |
46 | for (j = 0; j < 4; j++) |
47 | A[j * 4 + i] = graphene_matrix_get_value (m: &u, row: i, col: j); |
48 | |
49 | B[0] = graphene_vec4_get_x (v: &vv4); |
50 | B[1] = graphene_vec4_get_y (v: &vv4); |
51 | B[2] = graphene_vec4_get_z (v: &vv4); |
52 | B[3] = graphene_vec4_get_w (v: &vv4); |
53 | |
54 | singular_value_decomposition (A, nrows: 4, ncols: 4, U, S, V); |
55 | singular_value_decomposition_solve (U, S, V, nrows: 4, ncols: 4, B, x); |
56 | |
57 | v[ 0] = x[0]; |
58 | v[ 5] = x[1]; |
59 | v[10] = x[2]; |
60 | v[15] = 1; |
61 | |
62 | graphene_matrix_init_from_float (m: &s, v: (const float *)&v); |
63 | graphene_matrix_multiply (a: &s, b: &u, res: m); |
64 | } |
65 | |
66 | /* Compute a 4x4 matrix m that maps |
67 | * p1 -> q1 |
68 | * p2 -> q2 |
69 | * p3 -> q3 |
70 | * p4 -> q4 |
71 | * |
72 | * This is not in general possible, because projective |
73 | * transforms preserve coplanarity. But in the cases we |
74 | * care about here, both sets of points are always coplanar. |
75 | */ |
76 | void |
77 | perspective_3d (graphene_point3d_t *p1, |
78 | graphene_point3d_t *p2, |
79 | graphene_point3d_t *p3, |
80 | graphene_point3d_t *p4, |
81 | graphene_point3d_t *q1, |
82 | graphene_point3d_t *q2, |
83 | graphene_point3d_t *q3, |
84 | graphene_point3d_t *q4, |
85 | graphene_matrix_t *m) |
86 | { |
87 | graphene_matrix_t a, a_inv, b; |
88 | |
89 | unit_to (p1, p2, p3, p4, m: &a); |
90 | unit_to (p1: q1, p2: q2, p3: q3, p4: q4, m: &b); |
91 | |
92 | graphene_matrix_inverse (m: &a, res: &a_inv); |
93 | graphene_matrix_multiply (a: &a_inv, b: &b, res: m); |
94 | } |
95 | |