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

Linux Cross Reference
Tina4/src/geomstat/constraint/state_trns.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/sysfuncs.h>
  7 #include <tina/math.h>
  8 #include <tina/mathfuncs.h>
  9 #include <tina/vision.h>
 10 #include <tina/visionfuncs.h>
 11 
 12 double  kalman();
 13 
 14 double  line3_transf(State * state, int i0, int i1, int i2, int *pdf)
 15 {
 16     int     df0, df1, df2;
 17     Line3  *line0;
 18     Line3  *line1;
 19     Transf3 *transf;
 20     Basis3 *basis0;
 21     Basis3 *basis1;
 22     Vector *h;
 23     Vector *h0;
 24     Vector *h1;
 25     Vector *h2;
 26     double  lscale = state->lscale;
 27     double  accuracy = state->accuracy;
 28     double  z, eps, res, dres;
 29     double  exx, exy, eyx, eyy, ezz, dpz;
 30     Vec3    p = {Vec3_id};
 31     Vec3    dp = {Vec3_id};
 32     Vec3    c = {Vec3_id};
 33     Vec3    ex = {Vec3_id};
 34     Vec3    ey = {Vec3_id};
 35     Vec3    ez = {Vec3_id};
 36     Vec3    rex = {Vec3_id};
 37     Vec3    rey = {Vec3_id};
 38     Vec3    rez = {Vec3_id};
 39     Vec3    t = {Vec3_id};
 40     Mat3    R = {Mat3_id};
 41 
 42     line0 = (Line3 *) vector_getp(state->geom, i0);
 43     line1 = (Line3 *) vector_getp(state->geom, i1);
 44     transf = (Transf3 *) vector_getp(state->geom, i2);
 45     basis0 = (Basis3 *) vector_getp(state->basis, i0);
 46     basis1 = (Basis3 *) vector_getp(state->basis, i1);
 47     df0 = geom_df(LINE3);
 48     df1 = geom_df(LINE3);
 49     df2 = geom_df(TRANSF3);
 50 
 51     h = vector_alloc(state->maxindex, ptr_v);
 52     h0 = vector_alloc(df0, double_v);
 53     vector_putp((void *) h0, h, i0);
 54     h1 = vector_alloc(df1, double_v);
 55     vector_putp((void *) h1, h, i1);
 56     h2 = vector_alloc(df2, double_v);
 57     vector_putp((void *) h2, h, i2);
 58 
 59     R = transf->T.R;
 60     t = transf->T.t;
 61     p = vec3_sum(t, mat3_vprod(R, line1->p));
 62     ex = mat3_vprod(R, basis1->ex);
 63     ey = mat3_vprod(R, basis1->ey);
 64     ez = mat3_vprod(R, basis1->ez);
 65     rex = vec3_ex();
 66     rey = vec3_ey();
 67     rez = vec3_ez();
 68     exx = vec3_dot(basis0->ex, ex);
 69     exy = vec3_dot(basis0->ex, ey);
 70     eyx = vec3_dot(basis0->ey, ex);
 71     eyy = vec3_dot(basis0->ey, ey);
 72     ezz = vec3_dot(basis0->ez, ez);
 73     dp = vec3_diff(line0->p, p);
 74     dpz = vec3_dot(dp, basis0->ez);
 75 
 76     eps = accuracy * lscale;
 77 
 78     z = vec3_dot(dp, basis0->ex);
 79     c = vec3_cross(p, basis0->ex);
 80 
 81     vector_putf(1.0, h0, 0);
 82     vector_putf(0.0, h0, 1);
 83     vector_putf(-dpz, h0, 2);
 84     vector_putf(0.0, h0, 3);
 85 
 86     vector_putf(-exx, h1, 0);
 87     vector_putf(-exy, h1, 1);
 88     vector_putf(0.0, h1, 2);
 89     vector_putf(0.0, h1, 3);
 90 
 91     vector_putf(-vec3_dot(rex, c), h2, 0);
 92     vector_putf(-vec3_dot(rey, c), h2, 1);
 93     vector_putf(-vec3_dot(rez, c), h2, 2);
 94     vector_putf(-vec3_dot(rex, basis0->ex), h2, 3);
 95     vector_putf(-vec3_dot(rey, basis0->ex), h2, 4);
 96     vector_putf(-vec3_dot(rez, basis0->ex), h2, 5);
 97 
 98     dres = kalman(state, z, h, eps, pdf);
 99     if (dres == 1e10)
100         return (1e10);
101     res = dres;
102 
103     z = vec3_dot(dp, basis0->ey);
104     c = vec3_cross(p, basis0->ey);
105 
106     vector_putf(0.0, h0, 0);
107     vector_putf(1.0, h0, 1);
108     vector_putf(0.0, h0, 2);
109     vector_putf(-dpz, h0, 3);
110 
111     vector_putf(-eyx, h1, 0);
112     vector_putf(-eyy, h1, 1);
113     vector_putf(0.0, h1, 2);
114     vector_putf(0.0, h1, 3);
115 
116     vector_putf(-vec3_dot(rex, c), h2, 0);
117     vector_putf(-vec3_dot(rey, c), h2, 1);
118     vector_putf(-vec3_dot(rez, c), h2, 2);
119     vector_putf(-vec3_dot(rex, basis0->ey), h2, 3);
120     vector_putf(-vec3_dot(rey, basis0->ey), h2, 4);
121     vector_putf(-vec3_dot(rez, basis0->ey), h2, 5);
122 
123     dres = kalman(state, z, h, eps, pdf);
124     if (dres == 1e10)
125         return (1e10);
126     res += dres;
127 
128     eps = accuracy;
129 
130     z = -vec3_dot(ez, basis0->ex);
131     c = vec3_cross(ez, basis0->ex);
132 
133     vector_putf(0.0, h0, 0);
134     vector_putf(0.0, h0, 1);
135     vector_putf(ezz, h0, 2);
136     vector_putf(0.0, h0, 3);
137 
138     vector_putf(0.0, h1, 0);
139     vector_putf(0.0, h1, 1);
140     vector_putf(-exx, h1, 2);
141     vector_putf(-exy, h1, 3);
142 
143     vector_putf(-vec3_dot(rex, c), h2, 0);
144     vector_putf(-vec3_dot(rey, c), h2, 1);
145     vector_putf(-vec3_dot(rez, c), h2, 2);
146     vector_putf(0.0, h2, 3);
147     vector_putf(0.0, h2, 4);
148     vector_putf(0.0, h2, 5);
149 
150     dres = kalman(state, z, h, eps, pdf);
151     if (dres == 1e10)
152         return (1e10);
153     res += dres;
154 
155     z = -vec3_dot(ez, basis0->ey);
156     c = vec3_cross(ez, basis0->ey);
157 
158     vector_putf(0.0, h0, 0);
159     vector_putf(0.0, h0, 1);
160     vector_putf(0.0, h0, 2);
161     vector_putf(ezz, h0, 3);
162 
163     vector_putf(0.0, h1, 0);
164     vector_putf(0.0, h1, 1);
165     vector_putf(-eyx, h1, 2);
166     vector_putf(-eyy, h1, 3);
167 
168     vector_putf(-vec3_dot(rex, c), h2, 0);
169     vector_putf(-vec3_dot(rey, c), h2, 1);
170     vector_putf(-vec3_dot(rez, c), h2, 2);
171     vector_putf(0.0, h2, 3);
172     vector_putf(0.0, h2, 4);
173     vector_putf(0.0, h2, 5);
174 
175     dres = kalman(state, z, h, eps, pdf);
176     if (dres == 1e10)
177         return (1e10);
178     res += dres;
179 
180     vector_free(h0);
181     vector_free(h1);
182     vector_free(h2);
183     vector_free(h);
184 
185     return (res);
186 }
187 
188 double  state_transf(State * state, int i0, int i1, int i2)
189 {
190     int     df;
191     int     type0, type1, type2;
192     double  res;
193 
194     if ((i0 < 0 || i0 > state->maxindex) ||
195         (i1 < 0 || i1 > state->maxindex) ||
196         (i2 < 0 || i2 > state->maxindex))
197     {
198         error("state_transf: bad index", non_fatal);
199         return (-1.0);
200     }
201     type0 = vector_get(state->type, i0);
202     type1 = vector_get(state->type, i1);
203     type2 = vector_get(state->type, i2);
204     if (type2 != TRANSF3)
205         error("state_transf: bad type", non_fatal);
206 
207     (void) geom_reorder(&i0, &i1, &type0, &type1);
208     switch (OPAIR(type0, type1))
209     {
210     case OPAIR(LINE3, LINE3):
211         df = 4;
212         res = line3_transf(state, i0, i1, i2, &df);
213         break;
214     default:
215         error("state_transf: bad type", non_fatal);
216         return (-1.0);
217     }
218     return (chisq(res, df));
219 }
220 

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