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

Linux Cross Reference
Tina4/src/geomstat/constraint/state_on.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  point_on_line(State * state, int i0, int i1, int *pdf)
 14 {
 15     int     df0, df1;
 16     Point3 *point;
 17     Line3  *line;
 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     point = (Point3 *) vector_getp(state->geom, i0);
 29     line = (Line3 *) 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(LINE3);
 34 
 35     dp = vec3_diff(point->p, line->p);
 36 
 37     h = vector_alloc(state->maxindex, ptr_v);
 38     h0 = vector_alloc(df0, double_v);
 39     vector_putp((void *) h0, h, i0);
 40     h1 = vector_alloc(df1, double_v);
 41     vector_putp((void *) h1, h, i1);
 42 
 43     eps = accuracy * lscale;
 44     z = vec3_dot(basis1->ex, dp);
 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(-vec3_dot(dp, basis1->ez), h1, 2);
 53     vector_putf(0.0, h1, 3);
 54 
 55     dres = kalman(state, z, h, eps, pdf);
 56     if (dres == 1e10)
 57         return (1e10);
 58     res = dres;
 59 
 60     z = vec3_dot(basis1->ey, dp);
 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     vector_putf(-vec3_dot(dp, basis1->ez), h1, 3);
 69 
 70     dres = kalman(state, z, h, eps, pdf);
 71     if (dres == 1e10)
 72         return (1e10);
 73     res += dres;
 74 
 75     vector_free(h0);
 76     vector_free(h1);
 77     vector_free(h);
 78 
 79     return (res);
 80 }
 81 
 82 double  point_on_plane(State * state, int i0, int i1, int *pdf)
 83 {
 84     int     df0, df1;
 85     Point3 *point;
 86     Plane  *plane;
 87     Basis3 *basis0;
 88     Basis3 *basis1;
 89     Vec3    dp = {Vec3_id};
 90     Vector *h;
 91     Vector *h0;
 92     Vector *h1;
 93     double  lscale = state->lscale;
 94     double  accuracy = state->accuracy;
 95     double  z, eps, res, dres;
 96 
 97     point = (Point3 *) vector_getp(state->geom, i0);
 98     plane = (Plane *) vector_getp(state->geom, i1);
 99     basis0 = (Basis3 *) vector_getp(state->basis, i0);
100     basis1 = (Basis3 *) vector_getp(state->basis, i1);
101     df0 = geom_df(POINT3);
102     df1 = geom_df(PLANE);
103 
104     dp = vec3_diff(point->p, plane->p);
105 
106     h = vector_alloc(state->maxindex, ptr_v);
107     h0 = vector_alloc(df0, double_v);
108     vector_putp((void *) h0, h, i0);
109     h1 = vector_alloc(df1, double_v);
110     vector_putp((void *) h1, h, i1);
111 
112     eps = accuracy * lscale;
113     z = vec3_dot(dp, basis1->ez);
114 
115     vector_putf(vec3_dot(basis0->ex, basis1->ez), h0, 0);
116     vector_putf(vec3_dot(basis0->ey, basis1->ez), h0, 1);
117     vector_putf(vec3_dot(basis0->ez, basis1->ez), h0, 2);
118 
119     vector_putf(-1.0, h1, 0);
120     vector_putf(vec3_dot(dp, basis1->ex), h1, 1);
121     vector_putf(vec3_dot(dp, basis1->ey), h1, 2);
122 
123     dres = kalman(state, z, h, eps, pdf);
124     if (dres == 1e10)
125         return (1e10);
126     res = dres;
127 
128     vector_free(h0);
129     vector_free(h1);
130     vector_free(h);
131 
132     return (res);
133 }
134 
135 double  coplanar_2_lines(State * state, int i0, int i1, int *pdf)
136 {
137     int     df0, df1;
138     Line3  *line0;
139     Line3  *line1;
140     Basis3 *basis0;
141     Basis3 *basis1;
142     Vec3    dp = {Vec3_id};
143     Vector *h;
144     Vector *h0;
145     Vector *h1;
146     double  lscale = state->lscale;
147     double  accuracy = state->accuracy;
148     double  z, eps, res;
149 
150     line0 = (Line3 *) vector_getp(state->geom, i0);
151     line1 = (Line3 *) vector_getp(state->geom, i1);
152     basis0 = (Basis3 *) vector_getp(state->basis, i0);
153     basis1 = (Basis3 *) vector_getp(state->basis, i1);
154     df0 = geom_df(LINE3);
155     df1 = geom_df(LINE3);
156 
157     dp = vec3_diff(line0->p, line1->p);
158 
159     h = vector_alloc(state->maxindex, ptr_v);
160     h0 = vector_alloc(df0, double_v);
161     vector_putp((void *) h0, h, i0);
162     h1 = vector_alloc(df1, double_v);
163     vector_putp((void *) h1, h, i1);
164 
165     eps = accuracy * lscale;
166     z = vec3_dot(dp, vec3_cross(basis0->ez, basis1->ez));
167 
168     vector_putf(-vec3_dot(basis0->ey, basis1->ez), h0, 0);
169     vector_putf(vec3_dot(basis0->ex, basis1->ez), h0, 1);
170     vector_putf(vec3_dot(dp, vec3_cross(basis0->ex, basis1->ez)), h0, 2);
171     vector_putf(vec3_dot(dp, vec3_cross(basis0->ey, basis1->ez)), h0, 3);
172 
173     vector_putf(-vec3_dot(basis0->ez, basis1->ey), h1, 0);
174     vector_putf(vec3_dot(basis0->ez, basis1->ex), h1, 1);
175     vector_putf(vec3_dot(dp, vec3_cross(basis0->ez, basis1->ex)), h1, 2);
176     vector_putf(vec3_dot(dp, vec3_cross(basis0->ez, basis1->ey)), h1, 3);
177 
178     res = kalman(state, z, h, eps, pdf);
179     vector_free(h0);
180     vector_free(h1);
181     vector_free(h);
182     return (res);
183 }
184 
185 double  line_on_plane(State * state, int i0, int i1, int *pdf)
186 {
187     int     df0, df1;
188     Line3  *line;
189     Plane  *plane;
190     Basis3 *basis0;
191     Basis3 *basis1;
192     Vec3    dp = {Vec3_id};
193     Vector *h;
194     Vector *h0;
195     Vector *h1;
196     double  lscale = state->lscale;
197     double  accuracy = state->accuracy;
198     double  z, eps, res, dres;
199 
200     line = (Line3 *) vector_getp(state->geom, i0);
201     plane = (Plane *) vector_getp(state->geom, i1);
202     basis0 = (Basis3 *) vector_getp(state->basis, i0);
203     basis1 = (Basis3 *) vector_getp(state->basis, i1);
204     df0 = geom_df(LINE3);
205     df1 = geom_df(PLANE);
206 
207     dp = vec3_diff(line->p, plane->p);
208 
209     h = vector_alloc(state->maxindex, ptr_v);
210     h0 = vector_alloc(df0, double_v);
211     vector_putp((void *) h0, h, i0);
212     h1 = vector_alloc(df1, double_v);
213     vector_putp((void *) h1, h, i1);
214 
215     eps = accuracy * lscale;
216     z = vec3_dot(dp, basis1->ez);
217 
218     vector_putf(vec3_dot(basis0->ex, basis1->ez), h0, 0);
219     vector_putf(vec3_dot(basis0->ey, basis1->ez), h0, 1);
220     vector_putf(0.0, h0, 2);
221     vector_putf(0.0, h0, 3);
222 
223     vector_putf(-1.0, h1, 0);
224     vector_putf(vec3_dot(dp, basis1->ex), h1, 1);
225     vector_putf(vec3_dot(dp, basis1->ey), h1, 2);
226 
227     dres = kalman(state, z, h, eps, pdf);
228     if (dres == 1e10)
229         return (1e10);
230     res = dres;
231 
232     eps = accuracy;
233     z = vec3_dot(basis0->ez, basis1->ez);
234 
235     vector_putf(0.0, h0, 0);
236     vector_putf(0.0, h0, 1);
237     vector_putf(vec3_dot(basis0->ex, basis1->ez), h0, 2);
238     vector_putf(vec3_dot(basis0->ey, basis1->ez), h0, 3);
239 
240     vector_putf(0.0, h1, 0);
241     vector_putf(vec3_dot(basis0->ez, basis1->ex), h1, 1);
242     vector_putf(vec3_dot(basis0->ez, basis1->ey), h1, 2);
243 
244     dres = kalman(state, z, h, eps, pdf);
245     if (dres == 1e10)
246         return (1e10);
247     res += dres;
248 
249     vector_free(h0);
250     vector_free(h1);
251     vector_free(h);
252 
253     return (res);
254 }
255 
256 double  state_on(State * state, int i0, int i1)
257 {
258     int     df;
259     int     type0, type1;
260     double  res;
261 
262     if (state_badpair(state, i0, i1))
263         return (-1.0);
264 
265     type0 = vector_get(state->type, i0);
266     type1 = vector_get(state->type, i1);
267     (void) geom_reorder(&i0, &i1, &type0, &type1);
268 
269     switch (OPAIR(type0, type1))
270     {
271     case OPAIR(POINT3, LINE3):
272         df = 2;
273         res = point_on_line(state, i0, i1, &df);
274         break;
275     case OPAIR(POINT3, PLANE):
276         df = 1;
277         res = point_on_plane(state, i0, i1, &df);
278         break;
279     case OPAIR(LINE3, LINE3):
280         df = 1;
281         res = coplanar_2_lines(state, i0, i1, &df);
282         break;
283     case OPAIR(LINE3, PLANE):
284         df = 2;
285         res = line_on_plane(state, i0, i1, &df);
286         break;
287     default:
288         return (-1.0);
289     }
290     return (chisq(res, df));
291 }
292 

~ [ 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.