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

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

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