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

Linux Cross Reference
Tina4/src/geomstat/constraint/state_eq.c

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

  1 /**@(#)
  2 **/
  3 #include <math.h>
  4 #include <stdio.h>
  5 #include <tina/sys.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 #include <tina/vision.h>
  9 #include <tina/visionfuncs.h>
 10 
 11 double  kalman();
 12 
 13 double  point3_equal(State * state, int i0, int i1, int *pdf)
 14 {
 15     int     df0, df1;
 16     Point3 *point0;
 17     Point3 *point1;
 18     Basis3 *basis0;
 19     Basis3 *basis1;
 20     Vec3    dp = {Vec3_id};
 21     Vector *h;
 22     Vector *h0;
 23     Vector *h1;
 24     double  lscale = state->lscale;
 25     double  accuracy = state->accuracy;
 26     double  z, eps, res, dres;
 27 
 28     point0 = (Point3 *) vector_getp(state->geom, i0);
 29     point1 = (Point3 *) vector_getp(state->geom, i1);
 30     basis0 = (Basis3 *) vector_getp(state->basis, i0);
 31     basis1 = (Basis3 *) vector_getp(state->basis, i1);
 32     df0 = geom_df(POINT3);
 33     df1 = geom_df(POINT3);
 34 
 35     dp = vec3_diff(point0->p, point1->p);
 36     eps = accuracy * lscale;
 37 
 38     h = vector_alloc(state->maxindex, ptr_v);
 39     h0 = vector_alloc(df0, double_v);
 40     vector_putp((void *) h0, h, i0);
 41     h1 = vector_alloc(df1, double_v);
 42     vector_putp((void *) h1, h, i1);
 43 
 44     z = vec3_dot(dp, basis1->ex);
 45 
 46     vector_putf(vec3_dot(basis0->ex, basis1->ex), h0, 0);
 47     vector_putf(vec3_dot(basis0->ey, basis1->ex), h0, 1);
 48     vector_putf(vec3_dot(basis0->ez, basis1->ex), h0, 2);
 49 
 50     vector_putf(-1.0, h1, 0);
 51     vector_putf(0.0, h1, 1);
 52     vector_putf(0.0, h1, 2);
 53 
 54     dres = kalman(state, z, h, eps, pdf);
 55     if (dres == 1e10)
 56         return (1e10);
 57     res = dres;
 58 
 59     z = vec3_dot(dp, basis1->ey);
 60 
 61     vector_putf(vec3_dot(basis0->ex, basis1->ey), h0, 0);
 62     vector_putf(vec3_dot(basis0->ey, basis1->ey), h0, 1);
 63     vector_putf(vec3_dot(basis0->ez, basis1->ey), h0, 2);
 64 
 65     vector_putf(0.0, h1, 0);
 66     vector_putf(-1.0, h1, 1);
 67     vector_putf(0.0, h1, 2);
 68 
 69     dres = kalman(state, z, h, eps, pdf);
 70     if (dres == 1e10)
 71         return (1e10);
 72     res += dres;
 73 
 74     z = vec3_dot(dp, basis1->ez);
 75 
 76     vector_putf(vec3_dot(basis0->ex, basis1->ez), h0, 0);
 77     vector_putf(vec3_dot(basis0->ey, basis1->ez), h0, 1);
 78     vector_putf(vec3_dot(basis0->ez, basis1->ez), h0, 2);
 79 
 80     vector_putf(0.0, h1, 0);
 81     vector_putf(0.0, h1, 1);
 82     vector_putf(-1.0, h1, 2);
 83 
 84     dres = kalman(state, z, h, eps, pdf);
 85     if (dres == 1e10)
 86         return (1e10);
 87     res += dres;
 88 
 89     vector_free(h0);
 90     vector_free(h1);
 91     vector_free(h);
 92 
 93     return (res);
 94 }
 95 
 96 double  line3_equal(State * state, int i0, int i1, int *pdf)
 97 {
 98     int     df0, df1;
 99     Line3  *line0;
100     Line3  *line1;
101     Basis3 *basis0;
102     Basis3 *basis1;
103     Vec3    dp = {Vec3_id};
104     Vector *h;
105     Vector *h0;
106     Vector *h1;
107     double  lscale = state->lscale;
108     double  accuracy = state->accuracy;
109     double  z, eps, res, dres;
110     double  exx, exy, eyx, eyy, ezz, dpz;
111 
112     line0 = (Line3 *) vector_getp(state->geom, i0);
113     line1 = (Line3 *) vector_getp(state->geom, i1);
114     basis0 = (Basis3 *) vector_getp(state->basis, i0);
115     basis1 = (Basis3 *) vector_getp(state->basis, i1);
116     df0 = geom_df(LINE3);
117     df1 = geom_df(LINE3);
118 
119     h = vector_alloc(state->maxindex, ptr_v);
120     h0 = vector_alloc(df0, double_v);
121     vector_putp((void *) h0, h, i0);
122     h1 = vector_alloc(df1, double_v);
123     vector_putp((void *) h1, h, i1);
124 
125 
126     exx = vec3_dot(basis0->ex, basis1->ex);
127     exy = vec3_dot(basis0->ex, basis1->ey);
128     eyx = vec3_dot(basis0->ey, basis1->ex);
129     eyy = vec3_dot(basis0->ey, basis1->ey);
130     ezz = vec3_dot(basis0->ez, basis1->ez);
131     dp = vec3_diff(line0->p, line1->p);
132     dpz = vec3_dot(dp, basis0->ez);
133 
134     eps = accuracy * lscale;
135     z = vec3_dot(dp, basis0->ex);
136 
137     vector_putf(1.0, h0, 0);
138     vector_putf(0.0, h0, 1);
139     vector_putf(-dpz, h0, 2);
140     vector_putf(0.0, h0, 3);
141 
142     vector_putf(-exx, h1, 0);
143     vector_putf(-exy, h1, 1);
144     vector_putf(0.0, h1, 2);
145     vector_putf(0.0, h1, 3);
146 
147     dres = kalman(state, z, h, eps, pdf);
148     if (dres == 1e10)
149         return (1e10);
150     res = dres;
151 
152     z = vec3_dot(dp, basis0->ey);
153 
154     vector_putf(0.0, h0, 0);
155     vector_putf(1.0, h0, 1);
156     vector_putf(0.0, h0, 2);
157     vector_putf(-dpz, h0, 3);
158 
159     vector_putf(-eyx, h1, 0);
160     vector_putf(-eyy, h1, 1);
161     vector_putf(0.0, h1, 2);
162     vector_putf(0.0, h1, 3);
163 
164     dres = kalman(state, z, h, eps, pdf);
165     if (dres == 1e10)
166         return (1e10);
167     res += dres;
168 
169     eps = accuracy;
170     z = -vec3_dot(basis0->ex, basis1->ez);
171 
172     vector_putf(0.0, h0, 0);
173     vector_putf(0.0, h0, 1);
174     vector_putf(ezz, h0, 2);
175     vector_putf(0.0, h0, 3);
176 
177     vector_putf(0.0, h1, 0);
178     vector_putf(0.0, h1, 1);
179     vector_putf(-exx, h1, 2);
180     vector_putf(-exy, h1, 3);
181 
182     dres = kalman(state, z, h, eps, pdf);
183     if (dres == 1e10)
184         return (1e10);
185     res += dres;
186 
187 
188     z = -vec3_dot(basis0->ey, basis1->ez);
189 
190     vector_putf(0.0, h0, 0);
191     vector_putf(0.0, h0, 1);
192     vector_putf(0.0, h0, 2);
193     vector_putf(ezz, h0, 3);
194 
195     vector_putf(0.0, h1, 0);
196     vector_putf(0.0, h1, 1);
197     vector_putf(-eyx, h1, 2);
198     vector_putf(eyy, h1, 3);
199 
200     dres = kalman(state, z, h, eps, pdf);
201     if (dres == 1e10)
202         return (1e10);
203     res += dres;
204 
205     vector_free(h0);
206     vector_free(h1);
207     vector_free(h);
208 
209     return (res);
210 }
211 
212 double  plane_equal(State * state, int i0, int i1, int *pdf)
213 {
214     int     df0, df1;
215     Plane  *plane0;
216     Plane  *plane1;
217     Basis3 *basis0;
218     Basis3 *basis1;
219     Vec3    dp = {Vec3_id};
220     Vector *h;
221     Vector *h0;
222     Vector *h1;
223     double  lscale = state->lscale;
224     double  accuracy = state->accuracy;
225     double  z, eps, res, dres;
226 
227     plane0 = (Plane *) vector_getp(state->geom, i0);
228     plane1 = (Plane *) vector_getp(state->geom, i1);
229     basis0 = (Basis3 *) vector_getp(state->basis, i0);
230     basis1 = (Basis3 *) vector_getp(state->basis, i1);
231     df0 = geom_df(PLANE);
232     df1 = geom_df(PLANE);
233 
234     h = vector_alloc(state->maxindex, ptr_v);
235     h0 = vector_alloc(df0, double_v);
236     vector_putp((void *) h0, h, i0);
237     h1 = vector_alloc(df1, double_v);
238     vector_putp((void *) h1, h, i1);
239 
240 
241     dp = vec3_diff(plane0->p, plane1->p);
242 
243     eps = accuracy * lscale;
244     z = vec3_dot(dp, basis0->ex);
245 
246     vector_putf(1.0, h0, 0);
247     vector_putf(vec3_dot(dp, basis0->ex), h0, 1);
248     vector_putf(vec3_dot(dp, basis0->ey), h0, 2);
249 
250     vector_putf(-vec3_dot(basis0->ez, basis1->ez), h1, 0);
251     vector_putf(0.0, h1, 1);
252     vector_putf(0.0, h1, 2);
253 
254     dres = kalman(state, z, h, eps, pdf);
255     if (dres == 1e10)
256         return (1e10);
257     res = dres;
258 
259     eps = accuracy;
260     dp = vec3_diff(basis0->ez, basis1->ez);
261 
262     z = vec3_dot(dp, basis0->ex);
263 
264     vector_putf(0.0, h0, 0);
265     vector_putf(1.0, h0, 1);
266     vector_putf(0.0, h0, 2);
267 
268     vector_putf(0.0, h1, 0);
269     vector_putf(-vec3_dot(basis0->ex, basis1->ex), h1, 1);
270     vector_putf(-vec3_dot(basis0->ex, basis1->ey), h1, 2);
271 
272     dres = kalman(state, z, h, eps, pdf);
273     if (dres == 1e10)
274         return (1e10);
275     res += dres;
276 
277     z = vec3_dot(dp, basis0->ey);
278 
279     vector_putf(0.0, h0, 0);
280     vector_putf(0.0, h0, 1);
281     vector_putf(1.0, h0, 2);
282 
283     vector_putf(0.0, h1, 0);
284     vector_putf(-vec3_dot(basis0->ey, basis1->ex), h1, 1);
285     vector_putf(-vec3_dot(basis0->ey, basis1->ey), h1, 2);
286 
287     dres = kalman(state, z, h, eps, pdf);
288     if (dres == 1e10)
289         return (1e10);
290     res += dres;
291 
292     vector_free(h0);
293     vector_free(h1);
294     vector_free(h);
295 
296     return (res);
297 }
298 
299 double  state_equal(State * state, int i0, int i1)
300 {
301     int     df;
302     int     type0, type1;
303     double  res;
304 
305     if (state_badpair(state, i0, i1))
306         return (-1.0);
307 
308     type0 = vector_get(state->type, i0);
309     type1 = vector_get(state->type, i1);
310     (void) geom_reorder(&i0, &i1, &type0, &type1);
311 
312     switch (OPAIR(type0, type1))
313     {
314     case OPAIR(POINT3, POINT3):
315         df = 3;
316         res = point3_equal(state, i0, i1, &df);
317         break;
318     case OPAIR(LINE3, LINE3):
319         df = 4;
320         res = line3_equal(state, i0, i1, &df);
321         break;
322     case OPAIR(PLANE, PLANE):
323         df = 3;
324         res = plane_equal(state, i0, i1, &df);
325         break;
326     default:
327         return (-1.0);
328     }
329     return (chisq(res, df));
330 }
331 

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

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.