~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

# Linux Cross ReferenceTina4/src/math/geom/proj2.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

```  1 /**@(#)Image point corresponding to projective point etc
2 **/
3
4 #include <math.h>
5 #include <stdio.h>
6 #include <tina/sys.h>
7 #include <tina/math.h>
8 #include <tina/mathfuncs.h>
9
10 /** image point corresponding to projective point **/
11 Vec2    vec2_of_proj2(Vec3 v)
12 {
13     double  z = vec3_z(v);
14
15     return (vec2(vec3_x(v) / z, vec3_y(v) / z));
16 }
17
18 /** (unit) projective point corresponding to image point **/
19 Vec3    proj2_of_vec2(Vec2 v)
20 {
21     return (vec3(vec2_x(v), vec2_y(v), 1.0));
22 }
23
24 /** result of applying projective transform to projective point **/
25 Vec3    proj2_rectify(Mat3 m, Vec3 v)
26 {
27     return (vec3_unit(mat3_vprod(m, v)));
28 }
29
30 /** result of applying projective transform to image point **/
31 Vec2    vec2_rectify(Mat3 m, Vec2 v)
32 {
33     Vec3    w = {Vec3_id};
34
35     w = mat3_vprod(m, vec3(vec2_x(v), vec2_y(v), 1.0));
36     return (vec2(vec3_x(w) / vec3_z(w), vec3_y(w) / vec3_z(w)));
37 }
38
39 /** projective coordinates of projective line joining two points **/
40 Vec3    proj2_join(Vec3 p, Vec3 q)
41 {
42     return (vec3_unitcross(p, q));
43 }
44
45 /** projective coordinates of intersection of two lines **/
46 Vec3    proj2_intersect(Vec3 p, Vec3 q)
47 {
48     return (vec3_unitcross(p, q));
49 }
50
51 /** temporary (not so stable) **/
52 static Vec3 solve(Mat3 m, Vec3 y)
53 {
54     return (mat3_vprod(mat3_inverse(m), y));
55 }
56
57 Mat3    proj2_to_frame(Vec3 p00, Vec3 p10, Vec3 p01, Vec3 p11)
58 {
59     Mat3    m = {Mat3_id};
60     Vec3    px = {Vec3_id};
61     Vec3    py = {Vec3_id};
62     Vec3    pz = {Vec3_id};
63     Vec3    v = {Vec3_id};
64
65     /** for stability **/
66     p00 = vec3_unit(p00);
67     p01 = vec3_unit(p01);
68     p10 = vec3_unit(p10);
69     p11 = vec3_unit(p11);
70
71     /** images of (1 0 0), (0 1 0), and (0 0 1) **/
72     px = proj2_intersect(proj2_join(p00, p10), proj2_join(p01, p11));
73     py = proj2_intersect(proj2_join(p00, p01), proj2_join(p10, p11));
74     pz = vec3_unit(p00);
75
76     /** found m up to scale factor on each column **/
77     m = mat3_of_cols(px, py, pz);
78
79     /** now make (1 1 1) go to p11 **/
80     v = vec3_unit(solve(m, p11));
81     m = mat3_of_cols(vec3_times(vec3_x(v), px),
82                      vec3_times(vec3_y(v), py),
83                      vec3_times(vec3_z(v), pz));
84
85     return (m);
86 }
87
88 Mat3    proj2_between(Vec2 p00, Vec2 p10, Vec2 p01, Vec2 p11, Vec2 q00, Vec2 q10, Vec2 q01, Vec2 q11)
89 {
90     Mat3    mr = {Mat3_id};
91     Mat3    ms = {Mat3_id};
92     Mat3    mrs = {Mat3_id};
93     Vec3    r00 = {Vec3_id};
94     Vec3    r10 = {Vec3_id};
95     Vec3    r01 = {Vec3_id};
96     Vec3    r11 = {Vec3_id};
97     Vec3    s00 = {Vec3_id};
98     Vec3    s10 = {Vec3_id};
99     Vec3    s01 = {Vec3_id};
100     Vec3    s11 = {Vec3_id};
101
102     r00 = proj2_of_vec2(p00);
103     r10 = proj2_of_vec2(p10);
104     r01 = proj2_of_vec2(p01);
105     r11 = proj2_of_vec2(p11);
106
107     s00 = proj2_of_vec2(q00);
108     s10 = proj2_of_vec2(q10);
109     s01 = proj2_of_vec2(q01);
110     s11 = proj2_of_vec2(q11);
111
112     mr = proj2_to_frame(r00, r10, r01, r11);
113     ms = proj2_to_frame(s00, s10, s01, s11);
114     mrs = mat3_prod(ms, mat3_inverse(mr));
115     return (mrs);
116 }
117
118 Mat3    proj2_between_proj2(Vec3 p00, Vec3 p10, Vec3 p01, Vec3 p11, Vec3 q00, Vec3 q10, Vec3 q01, Vec3 q11)
119 {
120     Mat3    mp = {Mat3_id};
121     Mat3    mq = {Mat3_id};
122
123     mp = proj2_to_frame(p00, p10, p01, p11);
124     mq = proj2_to_frame(q00, q10, q01, q11);
125     return (mat3_prod(mq, mat3_inverse(mp)));
126 }
127
128 /**
129 least squares replacement for geometrical method above
130 **/
131
132 /**
133 proj_between_x1 is unstable if zx compoment of
134 projection is small
135 use proj_between_ls below which does all three and gets best
136 **/
137
138 Mat3    proj_x1(int n, Vec3 * p, Vec3 * q, double *badness)
139 {
140     Mat3    m = {Mat3_id};
141     Matrix *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
142     Vector *b = vector_alloc(2 * n, double_v);
143     Vector *c;
144     int     i;
145
146     for (i = 0; i < n; i++)
147     {
148         double  xp = vec3_x(p[i]), xq = vec3_x(q[i]);
149         double  yp = vec3_y(p[i]), yq = vec3_y(q[i]);
150         double  zp = vec3_z(p[i]), zq = vec3_z(q[i]);
151
152         matrix_putf(zq * yp, a, 2 * i, 0);
153         matrix_putf(zq * zp, a, 2 * i, 1);
154         matrix_putf(zq * xp, a, 2 * i, 2);
155         matrix_putf(-xq * yp, a, 2 * i, 6);
156         matrix_putf(-xq * zp, a, 2 * i, 7);
157         vector_putf(xq * xp, b, 2 * i);
158
159         matrix_putf(zq * yp, a, 2 * i + 1, 3);
160         matrix_putf(zq * zp, a, 2 * i + 1, 4);
161         matrix_putf(zq * xp, a, 2 * i + 1, 5);
162         matrix_putf(-yq * yp, a, 2 * i + 1, 6);
163         matrix_putf(-yq * zp, a, 2 * i + 1, 7);
164         vector_putf(yq * xp, b, 2 * i + 1);
165     }
166
167     if ((c = matrix_cholesky_least_square(a, b)) == NULL)
168     {
170         return (mat3_zero());
171     }
172     mat3_xy(m) = vector_getf(c, 0);
173     mat3_xz(m) = vector_getf(c, 1);
174     mat3_xx(m) = vector_getf(c, 2);
175     mat3_yy(m) = vector_getf(c, 3);
176     mat3_yz(m) = vector_getf(c, 4);
177     mat3_yx(m) = vector_getf(c, 5);
178     mat3_zy(m) = vector_getf(c, 6);
179     mat3_zz(m) = vector_getf(c, 7);
180     mat3_zx(m) = 1.0;
181
182     matrix_free(a);
183     vector_free(b);
184     vector_free(c);
185
187     return (m);
188 }
189
190 Mat3    proj_y1(int n, Vec3 * p, Vec3 * q, double *badness)
191 {
192     Mat3    m = {Mat3_id};
193     Matrix *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
194     Vector *b = vector_alloc(2 * n, double_v);
195     Vector *c;
196     int     i;
197
198     for (i = 0; i < n; i++)
199     {
200         double  xp = vec3_x(p[i]), xq = vec3_x(q[i]);
201         double  yp = vec3_y(p[i]), yq = vec3_y(q[i]);
202         double  zp = vec3_z(p[i]), zq = vec3_z(q[i]);
203
204         matrix_putf(zq * zp, a, 2 * i, 0);
205         matrix_putf(zq * xp, a, 2 * i, 1);
206         matrix_putf(zq * yp, a, 2 * i, 2);
207         matrix_putf(-xq * zp, a, 2 * i, 6);
208         matrix_putf(-xq * xp, a, 2 * i, 7);
209         vector_putf(xq * yp, b, 2 * i);
210
211         matrix_putf(zq * zp, a, 2 * i + 1, 3);
212         matrix_putf(zq * xp, a, 2 * i + 1, 4);
213         matrix_putf(zq * yp, a, 2 * i + 1, 5);
214         matrix_putf(-yq * zp, a, 2 * i + 1, 6);
215         matrix_putf(-yq * xp, a, 2 * i + 1, 7);
216         vector_putf(yq * yp, b, 2 * i + 1);
217     }
218
219     if ((c = matrix_cholesky_least_square(a, b)) == NULL)
220     {
222         return (mat3_zero());
223     }
224     mat3_xz(m) = vector_getf(c, 0);
225     mat3_xx(m) = vector_getf(c, 1);
226     mat3_xy(m) = vector_getf(c, 2);
227     mat3_yz(m) = vector_getf(c, 3);
228     mat3_yx(m) = vector_getf(c, 4);
229     mat3_yy(m) = vector_getf(c, 5);
230     mat3_zz(m) = vector_getf(c, 6);
231     mat3_zx(m) = vector_getf(c, 7);
232     mat3_zy(m) = 1.0;
233
234     matrix_free(a);
235     vector_free(b);
236     vector_free(c);
237
239     return (m);
240 }
241
242 Mat3    proj_z1(int n, Vec3 * p, Vec3 * q, double *badness)
243 {
244     Mat3    m = {Mat3_id};
245     Matrix *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
246     Vector *b = vector_alloc(2 * n, double_v);
247     Vector *c;
248     int     i;
249
250     for (i = 0; i < n; i++)
251     {
252         double  xp = vec3_x(p[i]), xq = vec3_x(q[i]);
253         double  yp = vec3_y(p[i]), yq = vec3_y(q[i]);
254         double  zp = vec3_z(p[i]), zq = vec3_z(q[i]);
255
256         matrix_putf(zq * xp, a, 2 * i, 0);
257         matrix_putf(zq * yp, a, 2 * i, 1);
258         matrix_putf(zq * zp, a, 2 * i, 2);
259         matrix_putf(-xq * xp, a, 2 * i, 6);
260         matrix_putf(-xq * yp, a, 2 * i, 7);
261         vector_putf(xq * zp, b, 2 * i);
262
263         matrix_putf(zq * xp, a, 2 * i + 1, 3);
264         matrix_putf(zq * yp, a, 2 * i + 1, 4);
265         matrix_putf(zq * zp, a, 2 * i + 1, 5);
266         matrix_putf(-yq * xp, a, 2 * i + 1, 6);
267         matrix_putf(-yq * yp, a, 2 * i + 1, 7);
268         vector_putf(yq * zp, b, 2 * i + 1);
269     }
270
271     if ((c = matrix_cholesky_least_square(a, b)) == NULL)
272     {
274         return (mat3_zero());
275     }
276     mat3_xx(m) = vector_getf(c, 0);
277     mat3_xy(m) = vector_getf(c, 1);
278     mat3_xz(m) = vector_getf(c, 2);
279     mat3_yx(m) = vector_getf(c, 3);
280     mat3_yy(m) = vector_getf(c, 4);
281     mat3_yz(m) = vector_getf(c, 5);
282     mat3_zx(m) = vector_getf(c, 6);
283     mat3_zy(m) = vector_getf(c, 7);
284     mat3_zz(m) = 1.0;
285
286     matrix_free(a);
287     vector_free(b);
288     vector_free(c);
289
291     return (m);
292 }
293
294 Mat3    proj_between_ls(int n, Vec3 * p, Vec3 * q)
295 {
296     Mat3    mx = {Mat3_id};
297     Mat3    my = {Mat3_id};
298     Mat3    mz = {Mat3_id};
299     double  bx, by, bz, b;
300
301     mx = proj_x1(n, p, q, &bx);
302     my = proj_y1(n, p, q, &by);
303     mz = proj_z1(n, p, q, &bz);
304
305     b = MIN3(bx, by, bz);
306     if (b == bz)
307         return (mz);
308     if (b == by)
309         return (my);
310     return (mx);
311 }
312 ```

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~