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

Linux Cross Reference
Tina4/src/vision/transform/trnslation.c

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

  1 /**@(#)
  2 **/
  3 #include <tina/sys.h>
  4 #include <tina/sysfuncs.h>
  5 #include <tina/math.h>
  6 #include <tina/mathfuncs.h>
  7 #include <tina/vision.h>
  8 
  9 static Mat3 trans_mat3_get(Vec3 p1, Vec3 v1, Vec3 p2, Vec3 v2, double weight)
 10 /* BUG p1 & p2 unused */
 11 
 12 
 13 {
 14     Mat3    V = {Mat3_id};
 15     int     i, j;
 16 
 17     for (i = 0; i < 3; ++i)
 18         V.el[i][i] = (float)((2 - v1.el[i] * v1.el[i] - v2.el[i] * v2.el[i]) * weight);
 19 
 20     for (i = 0; i < 3; ++i)
 21         for (j = i + 1; j < 3; ++j)
 22         {
 23             float   val;
 24 
 25             val = (float)((-v1.el[i] * v1.el[j] - v2.el[i] * v2.el[j]) * weight);
 26             V.el[i][j] = V.el[j][i] = val;
 27         }
 28 
 29     return (V);
 30 }
 31 
 32 Vec3    mlist_comp_translate_vec3(List * match_list, Mat3 R)
 33 {
 34     Mat3    P = {Mat3_id};
 35     Mat3    V = {Mat3_id};
 36     Vec3    Y = {Vec3_id};
 37     List   *lptr;
 38 
 39     Y = vec3_zero();
 40     P = mat3_zero();
 41 
 42     for (lptr = match_list; lptr != NULL; lptr = lptr->next)
 43     {
 44         Match  *mptr = (Match *) lptr->to;
 45         Vec3    p1 = {Vec3_id};
 46         Vec3    p2 = {Vec3_id};
 47         Vec3    v1 = {Vec3_id};
 48         Vec3    v2 = {Vec3_id};
 49         Vec3    diff = {Vec3_id};
 50 
 51         switch (mptr->type)
 52         {
 53         case LINE3:
 54             v1 = ((Line3 *) mptr->to1)->v;
 55             p1 = ((Line3 *) mptr->to1)->p;
 56             v2 = ((Line3 *) mptr->to2)->v;
 57             p2 = ((Line3 *) mptr->to2)->p;
 58             break;
 59         case CONIC3:
 60             v1 = ((Conic3 *) mptr->to1)->ez;
 61             p1 = ((Conic3 *) mptr->to1)->origin;
 62             v2 = ((Conic3 *) mptr->to2)->ez;
 63             p2 = ((Conic3 *) mptr->to2)->origin;
 64             break;
 65         default:
 66             continue;
 67         }
 68 
 69         p1 = mat3_vprod(R, p1);
 70         v1 = mat3_vprod(R, v1);
 71         diff = vec3_diff(p2, p1);
 72         V = trans_mat3_get(p1, v1, p2, v2, mptr->weight);
 73         P = mat3_sum(P, V);
 74         Y = vec3_sum(Y, mat3_vprod(V, diff));
 75     }
 76 
 77     return (mat3_vprod(mat3_inverse(P), Y));
 78 }
 79 
 80 Vec3    mlist_comp_translate_all(List * match_list, Mat3 R)
 81 {
 82     Mat3    P = {Mat3_id};
 83     Mat3    V = {Mat3_id};
 84     Vec3    Y = {Vec3_id};
 85     List   *lptr1;
 86     List   *lptr2;
 87 
 88     Y = vec3_zero();
 89     P = mat3_zero();
 90 
 91     for (lptr1 = match_list; lptr1 != NULL; lptr1 = lptr1->next)
 92     {
 93         Match  *mptr1 = (Match *) lptr1->to;
 94         Vec3    p1 = {Vec3_id};
 95         Vec3    p2 = {Vec3_id};
 96         Vec3    v1 = {Vec3_id};
 97         Vec3    v2 = {Vec3_id};
 98         Vec3    diff = {Vec3_id};
 99 
100         switch (mptr1->type)
101         {
102         case POINT3:
103             break;
104         case LINE3:
105             v1 = ((Line3 *) mptr1->to1)->v;
106             p1 = ((Line3 *) mptr1->to1)->p;
107             v2 = ((Line3 *) mptr1->to2)->v;
108             p2 = ((Line3 *) mptr1->to2)->p;
109             break;
110         case CONIC3:
111             v1 = ((Conic3 *) mptr1->to1)->ez;
112             p1 = ((Conic3 *) mptr1->to1)->origin;
113             v2 = ((Conic3 *) mptr1->to2)->ez;
114             p2 = ((Conic3 *) mptr1->to2)->origin;
115             break;
116         default:
117             continue;
118         }
119 
120         if (mptr1->type != POINT3)
121         {
122             p1 = mat3_vprod(R, p1);
123             v1 = mat3_vprod(R, v1);
124             diff = vec3_diff(p2, p1);
125             V = trans_mat3_get(p1, v1, p2, v2, mptr1->weight);
126             P = mat3_sum(P, V);
127             Y = vec3_sum(Y, mat3_vprod(V, diff));
128         }
129         switch (mptr1->type)
130         {
131         default:
132             continue;
133         case POINT3:
134             p1 = ((Point3 *) mptr1->to1)->p;
135             p2 = ((Point3 *) mptr1->to2)->p;
136             break;
137         case CONIC3:
138             p1 = ((Conic3 *) mptr1->to1)->origin;
139             p2 = ((Conic3 *) mptr1->to2)->origin;
140             break;
141         }
142 
143         for (lptr2 = lptr1->next; lptr2 != NULL; lptr2 = lptr2->next)
144         {
145             Match  *mptr2 = (Match *) lptr2->to;
146 
147             switch (mptr2->type)
148             {
149             default:
150                 continue;
151             case POINT3:
152                 v1 = vec3_diff(((Point3 *) mptr2->to1)->p, p1);
153                 v2 = vec3_diff(((Point3 *) mptr2->to2)->p, p2);
154                 break;
155             case CONIC3:
156                 v1 = vec3_diff(((Conic3 *) mptr2->to1)->origin, p1);
157                 v2 = vec3_diff(((Conic3 *) mptr2->to2)->origin, p2);
158                 break;
159             }
160             v1 = vec3_unit(v1);
161             v2 = vec3_unit(v2);
162 
163             p1 = mat3_vprod(R, p1);
164             v1 = mat3_vprod(R, v1);
165             diff = vec3_diff(p2, p1);
166             V = trans_mat3_get(p1, v1, p2, v2, (mptr1->weight + mptr2->weight) * 0.5);
167             P = mat3_sum(P, V);
168             Y = vec3_sum(Y, mat3_vprod(V, diff));
169         }
170     }
171 
172     return (mat3_vprod(mat3_inverse(P), Y));
173 }
174 
175 Vec3    mlist_comp_translate_pos3(List * match_list, Mat3 R)
176 {
177     Vec3    T = {Vec3_id};
178     List   *lptr;
179     float   tot_weight = (float)0.0;
180 
181     T = vec3_zero();
182 
183     for (lptr = match_list; lptr != NULL; lptr = lptr->next)
184     {
185         Match  *mptr = (Match *) lptr->to;
186         Vec3    p1 = {Vec3_id};
187         Vec3    p2 = {Vec3_id};
188         Vec3    diff = {Vec3_id};
189 
190         switch (mptr->type)
191         {
192         default:
193             continue;
194         case POINT3:
195             p1 = ((Point3 *) mptr->to1)->p;
196             p2 = ((Point3 *) mptr->to2)->p;
197             break;
198         case CONIC3:
199             p1 = ((Conic3 *) mptr->to1)->origin;
200             p2 = ((Conic3 *) mptr->to2)->origin;
201             break;
202         }
203 
204         diff = vec3_diff(p2, mat3_vprod(R, p1));
205         T = vec3_sum(T, vec3_times(mptr->weight, diff));
206         tot_weight += mptr->weight;
207     }
208 
209     return (vec3_times(1 / tot_weight, T));
210 }
211 

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