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

Linux Cross Reference
Tina4/src/geomstat/constraint/state_par.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  par_line3s(State * state, int i0, int i1, int *pdf)
 14 {
 15     int     df0, df1;
 16     Basis3 *basis0;
 17     Basis3 *basis1;
 18     Vector *h;
 19     Vector *h0;
 20     Vector *h1;
 21     double  accuracy = state->accuracy;
 22     double  z, eps, res, dres;
 23     double  exx, exy, eyx, eyy, ezz;
 24 
 25     basis0 = (Basis3 *) vector_getp(state->basis, i0);
 26     basis1 = (Basis3 *) vector_getp(state->basis, i1);
 27     df0 = geom_df(LINE3);
 28     df1 = geom_df(LINE3);
 29 
 30     h = vector_alloc(state->maxindex, ptr_v);
 31     h0 = vector_alloc(df0, double_v);
 32     vector_putp((void *) h0, h, i0);
 33     h1 = vector_alloc(df1, double_v);
 34     vector_putp((void *) h1, h, i1);
 35 
 36 
 37     exx = vec3_dot(basis0->ex, basis1->ex);
 38     exy = vec3_dot(basis0->ex, basis1->ey);
 39     eyx = vec3_dot(basis0->ey, basis1->ex);
 40     eyy = vec3_dot(basis0->ey, basis1->ey);
 41     ezz = vec3_dot(basis0->ez, basis1->ez);
 42 
 43     eps = accuracy;
 44     z = vec3_dot(basis0->ez, basis1->ex);
 45 
 46     vector_putf(0.0, h0, 0);
 47     vector_putf(0.0, h0, 1);
 48     vector_putf(exx, h0, 2);
 49     vector_putf(eyx, h0, 3);
 50 
 51     vector_putf(0.0, h1, 0);
 52     vector_putf(0.0, h1, 1);
 53     vector_putf(-ezz, h1, 2);
 54     vector_putf(0.0, h1, 3);
 55 
 56     dres = kalman(state, z, h, eps, pdf);
 57     if (dres == 1e10)
 58         return (1e10);
 59     res = dres;
 60 
 61 
 62     z = vec3_dot(basis0->ez, basis1->ey);
 63 
 64     vector_putf(0.0, h0, 0);
 65     vector_putf(0.0, h0, 1);
 66     vector_putf(exy, h0, 2);
 67     vector_putf(eyy, h0, 3);
 68 
 69     vector_putf(0.0, h1, 0);
 70     vector_putf(0.0, h1, 1);
 71     vector_putf(0.0, h1, 2);
 72     vector_putf(-ezz, h1, 3);
 73 
 74     dres = kalman(state, z, h, eps, pdf);
 75     if (dres == 1e10)
 76         return (1e10);
 77     res += dres;
 78 
 79     vector_free(h0);
 80     vector_free(h1);
 81     vector_free(h);
 82 
 83     return (res);
 84 }
 85 
 86 double  par_line3_plane(State * state, int i0, int i1, int *pdf)
 87 {
 88     int     df0, df1;
 89     Basis3 *basis0;
 90     Basis3 *basis1;
 91     Vector *h;
 92     Vector *h0;
 93     Vector *h1;
 94     double  accuracy = state->accuracy;
 95     double  z, eps, res, dres;
 96 
 97     basis0 = (Basis3 *) vector_getp(state->basis, i0);
 98     basis1 = (Basis3 *) vector_getp(state->basis, i1);
 99     df0 = geom_df(LINE3);
100     df1 = geom_df(PLANE);
101 
102     h = vector_alloc(state->maxindex, ptr_v);
103     h0 = vector_alloc(df0, double_v);
104     vector_putp((void *) h0, h, i0);
105     h1 = vector_alloc(df1, double_v);
106     vector_putp((void *) h1, h, i1);
107 
108 
109     eps = accuracy;
110     z = vec3_dot(basis0->ex, basis1->ez);
111 
112     vector_putf(0.0, h0, 0);
113     vector_putf(0.0, h0, 1);
114     vector_putf(vec3_dot(basis0->ex, basis1->ez), h0, 2);
115     vector_putf(vec3_dot(basis0->ey, basis1->ez), h0, 3);
116 
117     vector_putf(0.0, h1, 0);
118     vector_putf(vec3_dot(basis0->ez, basis1->ex), h1, 1);
119     vector_putf(vec3_dot(basis0->ez, basis1->ey), h1, 2);
120 
121     dres = kalman(state, z, h, eps, pdf);
122     if (dres == 1e10)
123         return (1e10);
124     res = dres;
125 
126     vector_free(h0);
127     vector_free(h1);
128     vector_free(h);
129 
130     return (res);
131 }
132 
133 double  par_planes(State * state, int i0, int i1, int *pdf)
134 {
135     int     df0, df1;
136     Basis3 *basis0;
137     Basis3 *basis1;
138     Vector *h;
139     Vector *h0;
140     Vector *h1;
141     double  accuracy = state->accuracy;
142     double  z, eps, res, dres;
143     double  exx, exy, eyx, eyy, ezz;
144 
145     basis0 = (Basis3 *) vector_getp(state->basis, i0);
146     basis1 = (Basis3 *) vector_getp(state->basis, i1);
147     df0 = geom_df(PLANE);
148     df1 = geom_df(PLANE);
149 
150     exx = vec3_dot(basis0->ex, basis1->ex);
151     exy = vec3_dot(basis0->ex, basis1->ey);
152     eyx = vec3_dot(basis0->ey, basis1->ex);
153     eyy = vec3_dot(basis0->ey, basis1->ey);
154     ezz = vec3_dot(basis0->ez, basis1->ez);
155 
156 
157     h = vector_alloc(state->maxindex, ptr_v);
158     h0 = vector_alloc(df0, double_v);
159     vector_putp((void *) h0, h, i0);
160     h1 = vector_alloc(df1, double_v);
161     vector_putp((void *) h1, h, i1);
162     eps = accuracy;
163     z = vec3_dot(basis0->ez, basis1->ex);
164 
165     vector_putf(0.0, h0, 0);
166     vector_putf(exx, h0, 1);
167     vector_putf(eyx, h0, 2);
168 
169     vector_putf(0.0, h1, 0);
170     vector_putf(-ezz, h1, 1);
171     vector_putf(0.0, h1, 2);
172 
173     dres = kalman(state, z, h, eps, pdf);
174     if (dres == 1e10)
175         return (1e10);
176     res = dres;
177 
178 
179     z = vec3_dot(basis0->ez, basis1->ey);
180 
181     vector_putf(0.0, h0, 0);
182     vector_putf(exy, h0, 1);
183     vector_putf(eyy, h0, 2);
184 
185     vector_putf(0.0, h1, 0);
186     vector_putf(0.0, h1, 1);
187     vector_putf(-ezz, h1, 2);
188 
189     dres = kalman(state, z, h, eps, pdf);
190     if (dres == 1e10)
191         return (1e10);
192     res += dres;
193 
194     vector_free(h0);
195     vector_free(h1);
196     vector_free(h);
197 
198     return (res);
199 }
200 
201 double  state_par(State * state, int i0, int i1)
202 {
203     int     df;
204     int     type0, type1;
205     double  res;
206 
207     if (state_badpair(state, i0, i1))
208         return (-1.0);
209 
210     type0 = vector_get(state->type, i0);
211     type1 = vector_get(state->type, i1);
212     (void) geom_reorder(&i0, &i1, &type0, &type1);
213 
214     switch (OPAIR(type0, type1))
215     {
216     case OPAIR(LINE3, LINE3):
217         df = 2;
218         res = par_line3s(state, i0, i1, &df);
219         break;
220     case OPAIR(LINE3, PLANE):
221         df = 2;
222         res = par_line3_plane(state, i0, i1, &df);
223         break;
224     case OPAIR(PLANE, PLANE):
225         df = 2;
226         res = par_planes(state, i0, i1, &df);
227         break;
228     default:
229         return (-1.0);
230     }
231     return (chisq(res, df));
232 }
233 

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