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

Linux Cross Reference
Tina6/tina-libs/tina/geometry/geomTrans_rot.c

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

  1 /**********
  2  * 
  3  * This file is part of the TINA Open Source Image Analysis Environment
  4  * henceforth known as TINA
  5  *
  6  * TINA is free software; you can redistribute it and/or modify
  7  * it under the terms of the GNU General Public License as 
  8  * published by the Free Software Foundation.
  9  *
 10  * TINA is distributed in the hope that it will be useful,
 11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13  * GNU General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU General Public License
 16  * along with TINA; if not, write to the Free Software Foundation, Inc., 
 17  * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 18  *
 19  * ANY users of TINA who require exemption from the existing licence must
 20  * negotiate a new licence with Dr. Neil.A.Thacker, the sole agent for
 21  * the University of Manchester.
 22  *
 23  **********
 24  * 
 25  * Program :    TINA
 26  * File    :  $Source: /home/tina/cvs/tina-libs/tina/geometry/geomTrans_rot.c,v $
 27  * Date    :  $Date: 2002/12/09 11:51:23 $
 28  * Version :  $Revision: 1.1.1.1 $
 29  * CVS Id  :  $Id: geomTrans_rot.c,v 1.1.1.1 2002/12/09 11:51:23 cvstina Exp $
 30  *
 31  * Notes :
 32  *
 33  *********
 34 */
 35 
 36 
 37 #include "geomTrans_rot.h"
 38 
 39 #if HAVE_CONFIG_H
 40   #include <config.h>
 41 #endif
 42 
 43 #include <stdio.h>
 44 #include <math.h>
 45 #include <tina/sys/sysDef.h>
 46 #include <tina/sys/sysPro.h>
 47 #include <tina/math/mathDef.h>
 48 #include <tina/math/mathPro.h>
 49 #include <tina/geometry/geomDef.h>
 50 #include <tina/geometry/geom_PointDef.h>
 51 #include <tina/geometry/geom_LineDef.h>
 52 #include <tina/geometry/geom_CurveDef.h>
 53 #include <tina/geometry/geom_PlaneDef.h>
 54 
 55 
 56 static int rot_matrix_get(Matrix * M, Vec3 v1, Vec3 v2, double w);
 57 static Mat3 rot_compute(Matrix * M, int *rot_error);
 58 
 59 /* compute rotation from pure vector information */
 60 Mat3    mlist_comp_rot_vec3(List * match_list, int *rot_error)
 61 {
 62     Matrix *A = matrix_alloc(4, 4, matrix_full, float_v);
 63     Matrix *B = matrix_alloc(4, 4, matrix_full, float_v);
 64     Mat3    rot = {Mat3_id};
 65     List   *lptr;
 66 
 67     for (lptr = match_list; lptr != NULL; lptr = lptr->next)
 68     {
 69         Match  *mptr = (Match *) lptr->to;
 70         Vec3    v1 = {Vec3_id};
 71         Vec3    v2 = {Vec3_id};
 72 
 73         switch (mptr->type)
 74         {
 75         default:
 76             continue;           /* to next match */
 77         case LINE3:
 78             v1 = ((Line3 *) mptr->to1)->v;
 79             v2 = ((Line3 *) mptr->to2)->v;
 80             break;
 81         case CONIC3:
 82             v1 = ((Conic3 *) mptr->to1)->ez;
 83             v2 = ((Conic3 *) mptr->to2)->ez;
 84             break;
 85         case PLANE:
 86             v1 = ((Plane *) mptr->to1)->n;
 87             v2 = ((Plane *) mptr->to2)->n;
 88             break;
 89         }
 90 
 91         if (rot_matrix_get(A, v1, v2, mptr->weight))
 92             (void) fmatrix_add_inplace(B, A);
 93     }
 94 
 95     rot = rot_compute(B, rot_error);
 96     matrix_free(A);
 97     matrix_free(B);
 98 
 99     return (rot);
100 }
101 
102 /* compute rotation from 3D point position information */
103 Mat3    mlist_comp_rot_pos3(List * match_list, Vec3 c1, Vec3 c2, int *rot_error)
104 
105 /* centroids */
106 
107 {
108     Matrix *A = matrix_alloc(4, 4, matrix_full, float_v);
109     Matrix *B = matrix_alloc(4, 4, matrix_full, float_v);
110     Mat3    rot = {Mat3_id};
111     List   *lptr;
112 
113     for (lptr = match_list; lptr != NULL; lptr = lptr->next)
114     {
115         Match  *mptr = (Match *) lptr->to;
116         Vec3    v1 = {Vec3_id};
117         Vec3    v2 = {Vec3_id};
118         Point3 *p1;
119         Point3 *p2;
120 
121         if (mptr->type != POINT3)
122             continue;
123 
124         p1 = (Point3 *) mptr->to1;
125         p2 = (Point3 *) mptr->to2;
126 
127         v1 = vec3_unit(vec3_diff(p1->p, c1));
128         v2 = vec3_unit(vec3_diff(p2->p, c2));
129 
130         if (rot_matrix_get(A, v1, v2, mptr->weight))
131             (void) fmatrix_add_inplace(B, A);
132     }
133 
134     rot = rot_compute(B, rot_error);
135     matrix_free(A);
136     matrix_free(B);
137 
138     return (rot);
139 }
140 
141 /* compute rotation from all information */
142 Mat3    mlist_comp_rot_all(List * match_list, int *rot_error)
143 {
144     Matrix *A = matrix_alloc(4, 4, matrix_full, float_v);
145     Matrix *B = matrix_alloc(4, 4, matrix_full, float_v);
146     Mat3    rot = {Mat3_id};
147     List   *lptr1;
148     List   *lptr2;
149 
150     for (lptr1 = match_list; lptr1 != NULL; lptr1 = lptr1->next)
151     {
152         Match  *mptr1 = (Match *) lptr1->to;
153         Vec3    v1 = {Vec3_id};
154         Vec3    v2 = {Vec3_id};
155         Vec3    p1 = {Vec3_id};
156         Vec3    p2 = {Vec3_id};
157 
158         switch (mptr1->type)
159         {
160         default:
161             continue;
162         case POINT3:
163             break;
164         case LINE3:
165             v1 = ((Line3 *) mptr1->to1)->v;
166             v2 = ((Line3 *) mptr1->to2)->v;
167             break;
168         case CONIC3:
169             v1 = ((Conic3 *) mptr1->to1)->ez;
170             v2 = ((Conic3 *) mptr1->to2)->ez;
171             break;
172         case PLANE:
173             v1 = ((Plane *) mptr1->to1)->n;
174             v2 = ((Plane *) mptr1->to2)->n;
175             break;
176         }
177 
178         if (mptr1->type != POINT3 && rot_matrix_get(A, v1, v2, mptr1->weight))
179             (void) fmatrix_add_inplace(B, A);
180 
181         /* consider all pair-wise vectors */
182 
183         switch (mptr1->type)
184         {
185         default:
186             continue;
187         case POINT3:
188             p1 = ((Point3 *) mptr1->to1)->p;
189             p2 = ((Point3 *) mptr1->to2)->p;
190             break;
191         case CONIC3:
192             p1 = ((Conic3 *) mptr1->to1)->origin;
193             p2 = ((Conic3 *) mptr1->to2)->origin;
194             break;
195         }
196 
197         for (lptr2 = lptr1->next; lptr2 != NULL; lptr2 = lptr2->next)
198         {
199             Match  *mptr2 = (Match *) lptr2->to;
200 
201             switch (mptr2->type)
202             {
203             default:
204                 continue;
205             case POINT3:
206                 v1 = vec3_diff(p1, ((Point3 *) mptr2->to1)->p);
207                 v2 = vec3_diff(p2, ((Point3 *) mptr2->to2)->p);
208                 break;
209             case CONIC3:
210                 v1 = vec3_diff(p1, ((Conic3 *) mptr2->to1)->origin);
211                 v2 = vec3_diff(p2, ((Conic3 *) mptr2->to2)->origin);
212                 break;
213             }
214             v1 = vec3_unit(v1);
215             v2 = vec3_unit(v2);
216             if (rot_matrix_get(A, v1, v2, (mptr1->weight + mptr2->weight) * 0.5))
217                 (void) fmatrix_add_inplace(B, A);
218         }
219     }
220 
221     rot = rot_compute(B, rot_error);
222     matrix_free(A);
223     matrix_free(B);
224 
225     return (rot);
226 }
227 
228 static Mat3 rot_compute(Matrix * M, int *rot_error)
229 {
230     Matrix *evec;
231     Vector *eval;
232     Vector *quat;
233     double  theta, sin_theta;
234     float  *el;
235     Vec3    axis = {Vec3_id};
236 
237     if (M == NULL || M->m != 4 || M->n != 4)
238     {
239         *rot_error = 1;
240         return (mat3_unit());
241     }
242     evec = matrix_alloc(4, 4, matrix_full, float_v);
243     eval = vector_alloc(4, float_v);
244 
245     if (!matrix_eigen_sym(M, eval, evec))
246     {
247         matrix_free(evec);
248         vector_free(eval);
249         *rot_error = 1;
250         return (mat3_unit());
251     }
252     *rot_error = 0;
253     quat = matrix_col_vector(evec, 0);
254     el = quat->data;
255     if (el[0] < 0)
256         vector_minus_inplace(quat);
257 
258     theta = tina_acos(el[0]);
259     sin_theta = sin(theta);
260     if (fabs(sin_theta) < 0.000000001)
261         axis = vec3_ex();
262     else
263         axis = vec3_unit(vec3(el[1] / sin_theta, el[2] / sin_theta, el[3] / sin_theta));
264     matrix_free(evec);
265     vector_free(eval);
266     vector_free(quat);
267     return (rot3(theta * 2, axis));
268 }
269 
270 static int rot_matrix_get(Matrix * M, Vec3 v1, Vec3 v2, double w)
271 /* of type float */
272 
273 /* weight factor */
274 {
275     double  xY, yX, xZ, zX, yZ, zY;
276     double  xpXs, ypYs, zpZs;
277     double  xmXs, ymYs, zmZs;
278     double  w2 = 2 * w;
279     float **el;
280 
281     if (M->vtype != float_v || M->shape != matrix_full)
282         return (0);
283 
284     if (M->m != 4 || M->n != 4)
285         return (0);
286 
287     el = M->el.float_v;
288 
289     xpXs = sqr(vec3_x(v1) + vec3_x(v2));
290     ypYs = sqr(vec3_y(v1) + vec3_y(v2));
291     zpZs = sqr(vec3_z(v1) + vec3_z(v2));
292     xmXs = sqr(vec3_x(v1) - vec3_x(v2));
293     ymYs = sqr(vec3_y(v1) - vec3_y(v2));
294     zmZs = sqr(vec3_z(v1) - vec3_z(v2));
295 
296     xY = vec3_x(v1) * vec3_y(v2);
297     yX = vec3_y(v1) * vec3_x(v2);
298     xZ = vec3_x(v1) * vec3_z(v2);
299     zX = vec3_z(v1) * vec3_x(v2);
300     yZ = vec3_y(v1) * vec3_z(v2);
301     zY = vec3_z(v1) * vec3_y(v2);
302 
303     el[0][0] = (float)((xmXs + ymYs + zmZs) * w);
304     el[1][1] = (float)((xmXs + ypYs + zpZs) * w);
305     el[2][2] = (float)((xpXs + ymYs + zpZs) * w);
306     el[3][3] = (float)((xpXs + ypYs + zmZs) * w);
307     el[0][1] = el[1][0] = (float)(w2 * (zY - yZ));
308     el[0][2] = el[2][0] = (float)(w2 * (xZ - zX));
309     el[0][3] = el[3][0] = (float)(w2 * (yX - xY));
310     el[1][2] = el[2][1] = (float)(-w2 * (xY + yX));
311     el[1][3] = el[3][1] = (float)(-w2 * (xZ + zX));
312     el[2][3] = el[3][2] = (float)(-w2 * (yZ + zY));
313     return (1);
314 }
315 

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