Exemple #1
0
// find closest 3D point from from a set of 3D lines
void find_point_opt(type_sight *sights_list, int N,
		    double *point_opt, double *outerr)
{
	double prod1[3][3];
	double prod2[3];
	double Id[3][3];
	double mat_sum[3][3]={{0,0,0},
					   {0,0,0},
					   {0,0,0}};
	double prod_sum[3]={0,0,0};
	
	for(int i=0; i<N; i++)
	  {
	    OUTER_PRODUCT_3X3(prod1,sights_list[i].v,sights_list[i].v);
	    IDENTIFY_MATRIX_3X3(Id);
	    ACCUM_SCALE_MATRIX_3X3(Id,-1,prod1); // Now Id actually contains a mat diff
	    ACCUM_SCALE_MATRIX_3X3(mat_sum,1,Id); // accumulates it into matrix 'mat_sum'
	    MAT_DOT_VEC_3X3(prod2,Id,sights_list[i].s); // Id still contains a mat diff
	    VEC_ACCUM(prod_sum,1,prod2); // accumulates the above result
	  }
	
	double mat_sum_inv[3][3];
	double det;
	INVERT_3X3(mat_sum_inv,det,mat_sum);
	
	// find the optimal point
	MAT_DOT_VEC_3X3(point_opt,mat_sum_inv,prod_sum);
}
Exemple #2
0
void kalman_update_3x3(kalman_state_n* state, float* measurement,float** A, float** H)
{
    float** auxMat;
    auxMat=(float**)malloc(3*sizeof(float*));
    for (int i=0; i<3; i++) auxMat[i]=malloc(3*sizeof(float));
    float* auxVet;
    auxVet=(float*)malloc(3*sizeof(float));
   
    float pTrsp[3][3],Atrsp[3][3];
    
    /*predicted p*/
    TRANSPOSE_MATRIX_3X3(Atrsp, A);
    MATRIX_PRODUCT_3X3(auxMat, state->p, Atrsp);
    MATRIX_PRODUCT_3X3(state->p, A, auxMat);
    ACCUM_SCALE_MATRIX_3X3(state->p, 1, state->q)
    /*predicted x*/
    MAT_DOT_VEC_3X3(state->x, A, state->x);
//    state->x=xaux;

    /*kalman gain*/
    float Htrsp[3][3], S[3][3], Sinv[3][3], B[3][3], det;
    TRANSPOSE_MATRIX_3X3(Htrsp, H);
    TRANSPOSE_MATRIX_3X3(pTrsp, state->p);
    MATRIX_PRODUCT_3X3(auxMat, pTrsp, Htrsp);
    MATRIX_PRODUCT_3X3(S, H, auxMat);
    ACCUM_SCALE_MATRIX_3X3(S, 1.0, state->r);
 
    MATRIX_PRODUCT_3X3(B, H, pTrsp);
 
    INVERT_3X3(Sinv, det, S);
    MATRIX_PRODUCT_3X3(auxMat, Sinv, B);
    TRANSPOSE_MATRIX_3X3(state->k, auxMat);
//    MAT_PRINT_3X3(state->k);

    /*estimated x*/    
    MAT_DOT_VEC_3X3(auxVet, H, state->x);
    VEC_DIFF(auxVet, measurement, auxVet);
    MAT_DOT_VEC_3X3(auxVet, state->k, auxVet);
    VEC_SUM(state->x, state->x, auxVet);
    
    /*estimated P*/
    MATRIX_PRODUCT_3X3(auxMat, H, state->p);
    MATRIX_PRODUCT_3X3(auxMat, state->k, auxMat);
    ACCUM_SCALE_MATRIX_3X3(state->p, -1.0, auxMat);
   
    /*out*/
    MAT_DOT_VEC_3X3(measurement, H, state->x);
    
    for (int i=0; i<3; i++) free(auxMat[i]);
    free(auxMat);
    free(auxVet);
}
Exemple #3
0
void
extrusion_round_or_cut_join (int ncp,	/* number of contour points */
                           gleDouble contour[][2],	/* 2D contour */
                           gleDouble cont_normal[][2],/* 2D normal vecs */
                           gleDouble up[3],	/* up vector for contour */
                           int npoints,		/* numpoints in poly-line */
                           gleDouble point_array[][3],	/* polyline */
                           float color_array[][3],	/* color of polyline */
                           gleDouble xform_array[][2][3])   /* 2D contour xforms */
{
   int i, j;
   int inext, inextnext;
   gleDouble m[4][4];
   gleDouble tube_len, seg_len;
   gleDouble diff[3];
   gleDouble bi_0[3], bi_1[3];		/* bisecting plane */
   gleDouble bisector_0[3], bisector_1[3];		/* bisecting plane */
   gleDouble cut_0[3], cut_1[3];	/* cutting planes */
   gleDouble lcut_0[3], lcut_1[3];	/* cutting planes */
   int valid_cut_0, valid_cut_1;	/* flag -- cut vector is valid */
   gleDouble end_point_0[3], end_point_1[3];
   gleDouble torsion_point_0[3], torsion_point_1[3];
   gleDouble isect_point[3];
   gleDouble origin[3], neg_z[3];
   gleDouble yup[3];		/* alternate up vector */
   gleDouble *front_cap, *back_cap;	/* arrays containing the end caps */
   gleDouble *front_loop, *back_loop; /* arrays containing the tube ends */
   double *front_norm, *back_norm; /* arrays containing normal vecs */
   double *norm_loop=0x0, *tmp; /* normal vectors, cast into 3d from 2d */
   int *front_is_trimmed, *back_is_trimmed;   /* T or F */
   float *front_color, *back_color;  /* pointers to segment colors */
   gleCapCallback cap_callback = 0x0 ;  /* function callback to draw cap */
   gleCapCallback tmp_cap_callback = 0x0;  /* function callback to draw cap */
   int join_style_is_cut;      /* TRUE if join style is cut */
   double dot;                  /* partial dot product */
   char *mem_anchor;
   int first_time = TRUE;
   gleDouble *cut_vec;

   /* create a local, block scope copy of of the join style.
    * this will alleviate wasted cycles and register write-backs */
   /* choose the right callback, depending on the choosen join style */
   if (__TUBE_CUT_JOIN) {
      join_style_is_cut = TRUE;
      cap_callback =  draw_cut_style_cap_callback;
   } else {
      join_style_is_cut = FALSE;
      cap_callback =  draw_round_style_cap_callback;
   }

   /* By definition, the contour passed in has its up vector pointing in
    * the y direction */
   if (up == NULL) {
      yup[0] = 0.0;
      yup[1] = 1.0;
      yup[2] = 0.0;
   } else {
      VEC_COPY (yup, up);
   }

   /* ========== "up" vector sanity check ========== */
   (void) up_sanity_check (yup, npoints, point_array);

   /* the origin is at the origin */
   origin [0] = 0.0;
   origin [1] = 0.0;
   origin [2] = 0.0;

   /* and neg_z is at neg z */
   neg_z[0] = 0.0;
   neg_z[1] = 0.0;
   neg_z[2] = 1.0;

   /* malloc the data areas that we'll need to store the end-caps */
   mem_anchor = malloc (4 * 3*ncp*sizeof(gleDouble)
                      + 2 * 3*ncp*sizeof(double)
                      + 2 * 1*ncp*sizeof(int));
   front_norm = (double *) mem_anchor;
   back_norm = front_norm + 3*ncp;
   front_loop = (gleDouble *) (back_norm + 3*ncp);
   back_loop = front_loop + 3*ncp;
   front_cap = back_loop + 3*ncp;
   back_cap  = front_cap + 3*ncp;
   front_is_trimmed = (int *) (back_cap + 3*ncp);
   back_is_trimmed = front_is_trimmed + ncp;

   /* ======================================= */

   /* |-|-|-|-|-|-|-|-| SET UP FOR FIRST SEGMENT |-|-|-|-|-|-|-| */

   /* ignore all segments of zero length */
   i = 1;
   inext = i;
   FIND_NON_DEGENERATE_POINT (inext, npoints, seg_len, diff, point_array);
   tube_len = seg_len;	/* store for later use */

   /* may as well get the normals set up now */
   if (cont_normal != NULL) {
      if (xform_array == NULL) {
         norm_loop = front_norm;
         back_norm = norm_loop;
         for (j=0; j<ncp; j++) {
            norm_loop[3*j] = cont_normal[j][0];
            norm_loop[3*j+1] = cont_normal[j][1];
            norm_loop[3*j+2] = 0.0;
         }
      } else {
         for (j=0; j<ncp; j++) {
            NORM_XFORM_2X2 ( (&front_norm[3*j]),
                              xform_array[inext-1],
                              cont_normal [j]);
            front_norm[3*j+2] = 0.0;
            back_norm[3*j+2] = 0.0;
         }
      }
   } else {
      front_norm = back_norm = norm_loop = NULL;
   }

   /* get the bisecting plane */
   bisecting_plane (bi_0, point_array[i-1],
                          point_array[i],
                          point_array[inext]);

   /* compute cutting plane */
   CUTTING_PLANE (valid_cut_0, cut_0, point_array[i-1],
                         point_array[i],
                         point_array[inext]);

   /* reflect the up vector in the bisecting plane */
   VEC_REFLECT (yup, yup, bi_0);

   /* |-|-|-|-|-|-|-|-| START LOOP OVER SEGMENTS |-|-|-|-|-|-|-| */

   /* draw tubing, not doing the first segment */
   while (inext<npoints-1) {

      inextnext = inext;
      /* ignore all segments of zero length */
      FIND_NON_DEGENERATE_POINT (inextnext, npoints,
                                 seg_len, diff, point_array);

      /* get the far bisecting plane */
      bisecting_plane (bi_1, point_array[i],
                             point_array[inext],
                             point_array[inextnext]);


      /* compute cutting plane */
      CUTTING_PLANE (valid_cut_1, cut_1, point_array[i],
                            point_array[inext],
                            point_array[inextnext]);

      /* rotate so that z-axis points down v2-v1 axis,
       * and so that origen is at v1 */
      uviewpoint (m, point_array[i], point_array[inext], yup);
      PUSHMATRIX ();
      MULTMATRIX (m);

      /* rotate the cutting planes into the local coordinate system */
      MAT_DOT_VEC_3X3 (lcut_0, m, cut_0);
      MAT_DOT_VEC_3X3 (lcut_1, m, cut_1);

      /* rotate the bisecting planes into the local coordinate system */
      MAT_DOT_VEC_3X3 (bisector_0, m, bi_0);
      MAT_DOT_VEC_3X3 (bisector_1, m, bi_1);


      neg_z[2] = -tube_len;

      /* draw the tube */
      /* --------- START OF TMESH GENERATION -------------- */
      for (j=0; j<ncp; j++) {

         /* set up the endpoints for segment clipping */
         if (xform_array == NULL) {
            VEC_COPY_2 (end_point_0, contour[j]);
            VEC_COPY_2 (end_point_1, contour[j]);
            VEC_COPY_2 (torsion_point_0, contour[j]);
            VEC_COPY_2 (torsion_point_1, contour[j]);
         } else {
            /* transform the contour points with the local xform */
            MAT_DOT_VEC_2X3 (end_point_0,
                             xform_array[inext-1], contour[j]);
            MAT_DOT_VEC_2X3 (torsion_point_0,
                             xform_array[inext], contour[j]);
            MAT_DOT_VEC_2X3 (end_point_1,
                             xform_array[inext], contour[j]);
            MAT_DOT_VEC_2X3 (torsion_point_1,
                             xform_array[inext-1], contour[j]);

            /* if there are normals and there are affine xforms,
             * then compute local coordinate system normals.
             * Set up the back normals. (The front normals we inherit
             * from previous pass through the loop).  */
            if (cont_normal != NULL) {
               /* do up the normal vectors with the inverse transpose */
               NORM_XFORM_2X2 ( (&back_norm[3*j]),
                                xform_array[inext],
                                cont_normal [j]);
            }
         }
         end_point_0 [2] = 0.0;
         torsion_point_0 [2] = 0.0;
         end_point_1 [2] = - tube_len;
         torsion_point_1 [2] = - tube_len;

         /* The two end-points define a line.  Intersect this line
          * against the clipping plane defined by the PREVIOUS
          * tube segment.  */

         /* if this and the last tube are co-linear, don't cut the angle
          * if you do, a divide by zero will result.  This and last tube
          * are co-linear when the cut vector is of zero length */
         if (valid_cut_0 && join_style_is_cut) {
             INNERSECT (isect_point,  /* isect point (returned) */
                       origin,		/* point on intersecting plane */
                       lcut_0,		/* normal vector to plane */
                       end_point_0,	/* point on line */
                       end_point_1);	/* another point on the line */

            /* determine whether the raw end of the extrusion would have
             * been cut, by checking to see if the raw and is on the
             * far end of the half-plane defined by the cut vector.
             * If the raw end is not "cut", then it is "trimmed".
             */
            if (lcut_0[2] < 0.0) { VEC_SCALE (lcut_0, -1.0, lcut_0); }
            dot = lcut_0[0] * end_point_0[0];
            dot += lcut_0[1] * end_point_0[1];

            VEC_COPY ((&front_loop[3*j]), isect_point);
         } else {
            /* actual value of dot not interseting; need
             * only be positive so that if test below failes */
            dot = 1.0;
            VEC_COPY ((&front_loop[3*j]), end_point_0);
         }

         INNERSECT (isect_point, 	/* intersection point (returned) */
                    origin,		/* point on intersecting plane */
                    bisector_0,		/* normal vector to plane */
                    end_point_0,	/* point on line */
                    torsion_point_1);	/* another point on the line */

         /* trim out interior of intersecting tube */
         /* ... but save the untrimmed version for drawing the endcaps */
         /* ... note that cap contains valid data ONLY when is_trimmed
          * is TRUE. */
         if ((dot <= 0.0) || (isect_point[2] < front_loop[3*j+2])) {
/*
         if ((dot <= 0.0) || (front_loop[3*j+2] > 0.0)) {
*/
            VEC_COPY ((&front_cap[3*j]), (&front_loop [3*j]));
            VEC_COPY ((&front_loop[3*j]), isect_point);
            front_is_trimmed[j] = TRUE;
         } else {
            front_is_trimmed[j] = FALSE;
         }

         /* if intersection is behind the end of the segment,
          * truncate to the end of the segment
          * Note that coding front_loop [3*j+2] = -tube_len;
          * doesn't work when twists are involved, */
         if (front_loop[3*j+2] < -tube_len) {
            VEC_COPY( (&front_loop[3*j]), end_point_1);
         }

         /* --------------------------------------------------- */
         /* The two end-points define a line.  We did one endpoint
          * above. Now do the other.Intersect this line
          * against the clipping plane defined by the NEXT
          * tube segment.  */

         /* if this and the last tube are co-linear, don't cut the angle
          * if you do, a divide by zero will result.  This and last tube
          * are co-linear when the cut vector is of zero length */
         if (valid_cut_1 && join_style_is_cut) {
            INNERSECT (isect_point,  /* isect point (returned) */
                       neg_z,		/* point on intersecting plane */
                       lcut_1,		/* normal vector to plane */
                       end_point_1,	/* point on line */
                       end_point_0);	/* another point on the line */

            if (lcut_1[2] > 0.0) { VEC_SCALE (lcut_1, -1.0, lcut_1); }
            dot = lcut_1[0] * end_point_1[0];
            dot += lcut_1[1] * end_point_1[1];


            VEC_COPY ((&back_loop[3*j]), isect_point);
         } else {
            /* actual value of dot not interseting; need
             * only be positive so that if test below failes */
            dot = 1.0;
            VEC_COPY ((&back_loop[3*j]), end_point_1);
         }

         INNERSECT (isect_point, 	/* intersection point (returned) */
                    neg_z,		/* point on intersecting plane */
                    bisector_1,		/* normal vector to plane */
                    torsion_point_0,	/* point on line */
                    end_point_1);	/* another point on the line */

         /* cut out interior of intersecting tube */
         /* ... but save the uncut version for drawing the endcaps */
         /* ... note that cap contains valid data ONLY when is
          *_trimmed is TRUE. */
/*
        if ((dot <= 0.0) || (back_loop[3*j+2] < -tube_len)) {
*/
        if ((dot <= 0.0) || (isect_point[2] > back_loop[3*j+2])) {
            VEC_COPY ((&back_cap[3*j]), (&back_loop [3*j]));
            VEC_COPY ((&back_loop[3*j]), isect_point);
            back_is_trimmed[j] = TRUE;
         } else {
            back_is_trimmed[j] = FALSE;
         }

         /* if intersection is behind the end of the segment,
          * truncate to the end of the segment
          * Note that coding back_loop [3*j+2] = 0.0;
          * doesn't work when twists are involved, */
         if (back_loop[3*j+2] > 0.0) {
            VEC_COPY( (&back_loop[3*j]), end_point_0);
         }
      }

      /* --------- END OF TMESH GENERATION -------------- */

      /* |||||||||||||||||| START SEGMENT DRAW |||||||||||||||||||| */
      /* There are six different cases we can have for presence and/or
       * absecnce of colors and normals, and for interpretation of
       * normals. The blechy set of nested if statements below
       * branch to each of the six cases */
      if (xform_array == NULL) {
         if (color_array == NULL) {
            if (cont_normal == NULL) {
               draw_segment_plain (ncp, (gleVector *) front_loop, (gleVector *) back_loop, inext, seg_len);
            } else
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_segment_facet_n (ncp, (gleVector *) front_loop, (gleVector *) back_loop, (gleVector *) norm_loop,
                                     inext, seg_len);
            } else {
               draw_segment_edge_n (ncp, (gleVector *) front_loop, (gleVector *) back_loop, (gleVector *) norm_loop,
                                    inext, seg_len);
            }
         } else {
            if (cont_normal == NULL) {
               draw_segment_color (ncp, (gleVector *) front_loop, (gleVector *) back_loop,
                                   color_array[inext-1],
                                   color_array[inext], inext, seg_len);
            } else
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_segment_c_and_facet_n (ncp,
                                   (gleVector *) front_loop, (gleVector *) back_loop, (gleVector *) norm_loop,
                                   color_array[inext-1],
                                   color_array[inext], inext, seg_len);
            } else {
               draw_segment_c_and_edge_n (ncp,
                                   (gleVector *) front_loop, (gleVector *) back_loop, (gleVector *) norm_loop,
                                   color_array[inext-1],
                                   color_array[inext], inext, seg_len);
             }
          }
      } else {
         if (color_array == NULL) {
            if (cont_normal == NULL) {
               draw_segment_plain (ncp, (gleVector *) front_loop, (gleVector *) back_loop, inext, seg_len);
            } else
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_binorm_segment_facet_n (ncp, (gleVector *) front_loop, (gleVector *) back_loop,
                                                 (gleVector *) front_norm, (gleVector *) back_norm,
                                                 inext, seg_len);
            } else {
               draw_binorm_segment_edge_n (ncp, (gleVector *) front_loop, (gleVector *) back_loop,
                                                (gleVector *) front_norm, (gleVector *) back_norm,
                                                inext, seg_len);
            }
         } else {
            if (cont_normal == NULL) {
               draw_segment_color (ncp, (gleVector *) front_loop, (gleVector *) back_loop,
                                   color_array[inext-1],
                                   color_array[inext], inext, seg_len);
            } else
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_binorm_segment_c_and_facet_n (ncp,
                                   (gleVector *) front_loop, (gleVector *) back_loop,
                                   (gleVector *) front_norm, (gleVector *) back_norm,
                                   color_array[inext-1],
                                   color_array[inext], inext, seg_len);
            } else {
               draw_binorm_segment_c_and_edge_n (ncp,
                                   (gleVector *) front_loop, (gleVector *) back_loop,
                                   (gleVector *) front_norm, (gleVector *) back_norm,
                                   color_array[inext-1],
                                   color_array[inext], inext, seg_len);
             }
          }
      }
      /* |||||||||||||||||| END SEGMENT DRAW |||||||||||||||||||| */

      /* v^v^v^v^v^v^v^v^v  BEGIN END CAPS v^v^v^v^v^v^v^v^v^v^v^v */

      /* if end caps are required, draw them. But don't draw any
       * but the very first and last caps */
      if (first_time) {
         first_time = FALSE;
         tmp_cap_callback = cap_callback;
         cap_callback = null_cap_callback;
         if (__TUBE_DRAW_CAP) {
            if (color_array != NULL) C3F (color_array[inext-1]);
            draw_angle_style_front_cap (ncp, bisector_0,
                                       (gleDouble (*)[3]) front_loop);
         }
      }
      /* v^v^v^v^v^v^v^v^v  END END CAPS v^v^v^v^v^v^v^v^v^v^v^v */

      /* $$$$$$$$$$$$$$$$ BEGIN -1, FILLET & JOIN DRAW $$$$$$$$$$$$$$$$$ */
      /*
       * Now, draw the fillet triangles, and the join-caps.
       */
      if (color_array != NULL) {
         front_color = color_array[inext-1];
         back_color = color_array[inext];
      } else {
         front_color = NULL;
         back_color = NULL;
      }

      if (cont_normal == NULL) {
         /* the flag valid-cut is true if the cut vector has a valid
          * value (i.e. if a degenerate case has not occured).
          */
         if (valid_cut_0) {
            cut_vec = lcut_0;
         } else {
            cut_vec = NULL;
         }
         draw_fillets_and_join_plain (ncp,
                                  (gleVector *) front_loop,
                                  (gleVector *) front_cap,
                                  front_is_trimmed,
                                  origin,
                                  bisector_0,
                                  front_color,
                                  back_color,
                                  cut_vec,
                                  TRUE,
                                  cap_callback);

         /* v^v^v^v^v^v^v^v^v  BEGIN END CAPS v^v^v^v^v^v^v^v^v^v^v^v */
         if (inext == npoints-2) {
            if (__TUBE_DRAW_CAP) {
               if (color_array != NULL) C3F (color_array[inext]);
               draw_angle_style_back_cap (ncp, bisector_1,
                                           (gleDouble (*)[3]) back_loop);
               cap_callback = null_cap_callback;
            }
         } else {
            /* restore ability to draw cap */
            cap_callback = tmp_cap_callback;
         }
         /* v^v^v^v^v^v^v^v^v  END END CAPS v^v^v^v^v^v^v^v^v^v^v^v */

         /* the flag valid-cut is true if the cut vector has a valid
          * value (i.e. if a degenerate case has not occured).
          */
         if (valid_cut_1) {
            cut_vec = lcut_1;
         } else {
            cut_vec = NULL;
         }
         draw_fillets_and_join_plain (ncp,
                                  (gleVector *) back_loop,
                                  (gleVector *) back_cap,
                                  back_is_trimmed,
                                  neg_z,
                                  bisector_1,
                                  back_color,
                                  front_color,
                                  cut_vec,
                                  FALSE,
                                  cap_callback);
      } else {

         /* the flag valid-cut is true if the cut vector has a valid
          * value (i.e. if a degenerate case has not occured).
          */
         if (valid_cut_0) {
            cut_vec = lcut_0;
         } else {
            cut_vec = NULL;
         }
         draw_fillets_and_join_n_norms (ncp,
                                  (gleVector *) front_loop,
                                  (gleVector *) front_cap,
                                  front_is_trimmed,
                                  origin,
                                  bisector_0,
                                  (gleVector *) front_norm,
                                  front_color,
                                  back_color,
                                  cut_vec,
                                  TRUE,
                                  cap_callback);

         /* v^v^v^v^v^v^v^v^v  BEGIN END CAPS v^v^v^v^v^v^v^v^v^v^v^v */
         if (inext == npoints-2) {
            if (__TUBE_DRAW_CAP) {
               if (color_array != NULL) C3F (color_array[inext]);
               draw_angle_style_back_cap (ncp, bisector_1,
                                         (gleDouble (*)[3]) back_loop);
               cap_callback = null_cap_callback;
            }
         } else {
            /* restore ability to draw cap */
            cap_callback = tmp_cap_callback;
         }
         /* v^v^v^v^v^v^v^v^v  END END CAPS v^v^v^v^v^v^v^v^v^v^v^v */

         /* the flag valid-cut is true if the cut vector has a valid
          * value (i.e. if a degenerate case has not occured).
          */
         if (valid_cut_1) {
            cut_vec = lcut_1;
         } else {
            cut_vec = NULL;
         }
         draw_fillets_and_join_n_norms (ncp,
                                  (gleVector *) back_loop,
                                  (gleVector *) back_cap,
                                  back_is_trimmed,
                                  neg_z,
                                  bisector_1,
                                  (gleVector *) back_norm,
                                  back_color,
                                  front_color,
                                  cut_vec,
                                  FALSE,
                                  cap_callback);
      }

      /* $$$$$$$$$$$$$$$$ END FILLET & JOIN DRAW $$$$$$$$$$$$$$$$$ */

      /* pop this matrix, do the next set */
      POPMATRIX ();

      /* slosh stuff over to next vertex */
      tmp = front_norm;
      front_norm = back_norm;
      back_norm = tmp;

      tube_len = seg_len;
      i = inext;
      inext = inextnext;
      VEC_COPY (bi_0, bi_1);
      VEC_COPY (cut_0, cut_1);
      valid_cut_0 = valid_cut_1;

      /* reflect the up vector in the bisecting plane */
      VEC_REFLECT (yup, yup, bi_0);
   }
   /* |-|-|-|-|-|-|-|-| END LOOP OVER SEGMENTS |-|-|-|-|-|-|-| */

   free (mem_anchor);

}
Exemple #4
0
void draw_round_style_cap_callback (int ncp,
                                  double cap[][3],
                                  float face_color[3],
                                  gleDouble cut[3],
                                  gleDouble bi[3],
                                  double norms[][3],
                                  int frontwards)
{
   double axis[3];
   double xycut[3];
   double theta;
   double *last_contour, *next_contour;
   double *last_norm, *next_norm;
   double *cap_z;
   double *tmp;
   char *malloced_area;
   int i, j, k;
   double m[4][4];

   if (face_color != NULL) C3F (face_color);

   /* ------------ start setting up rotation matrix ------------- */
   /* if the cut vector is NULL (this should only occur in
    * a degenerate case), then we can't draw anything. return. */
   if (cut == NULL) return;

   /* make sure that the cut vector points inwards */
   if (cut[2] > 0.0) {
      VEC_SCALE (cut, -1.0, cut);
   }

   /* make sure that the bi vector points outwards */
   if (bi[2] < 0.0) {
      VEC_SCALE (bi, -1.0, bi);
   }

   /* determine the axis we are to rotate about to get bi-contour.
    * Note that the axis will always lie in the x-y plane */
   VEC_CROSS_PRODUCT (axis, cut, bi);

   /* reverse the cut vector for the back cap -- 
    * need to do this to get angle right */
   if (!frontwards) {
      VEC_SCALE (cut, -1.0, cut);
   }

   /* get angle to rotate by -- arccos of dot product of cut with cut
    * projected into the x-y plane */
   xycut [0] = 0.0;
   xycut [1] = 0.0;
   xycut [2] = 1.0;
   VEC_PERP (xycut, cut, xycut);
   VEC_NORMALIZE (xycut);
   VEC_DOT_PRODUCT (theta, xycut, cut);

   theta = acos (theta);

   /* we'll tesselate round joins into a number of teeny pieces */
   theta /= (double) __ROUND_TESS_PIECES;

   /* get the matrix */
   urot_axis_d (m, theta, axis);

   /* ------------ done setting up rotation matrix ------------- */

   /* This malloc is a fancy version of:
    * last_contour = (double *) malloc (3*ncp*sizeof(double);
    * next_contour = (double *) malloc (3*ncp*sizeof(double);
    */
   malloced_area = malloc ((4*3+1) *ncp*sizeof (double));
   last_contour = (double *) malloced_area;
   next_contour = last_contour +  3*ncp;
   cap_z = next_contour + 3*ncp;
   last_norm = cap_z + ncp;
   next_norm = last_norm + 3*ncp;

   /* make first copy of contour */
   if (frontwards) {
      for (j=0; j<ncp; j++) {
         last_contour[3*j] = cap[j][0];
         last_contour[3*j+1] = cap[j][1];
         last_contour[3*j+2] = cap_z[j] = cap[j][2];
      }

      if (norms != NULL) {
         for (j=0; j<ncp; j++) {
            VEC_COPY ((&last_norm[3*j]), norms[j]);
         }
      }
   } else {
      /* in order for backfacing polygon removal to work correctly, have
       * to have the sense in which the joins are drawn to be reversed 
       * for the back cap.  This can be done by reversing the order of
       * the contour points.  Normals are a bit trickier, since the 
       * reversal is off-by-one for facet normals as compared to edge 
       * normals. */
      for (j=0; j<ncp; j++) {
         k = ncp - j - 1;
         last_contour[3*k] = cap[j][0];
         last_contour[3*k+1] = cap[j][1];
         last_contour[3*k+2] = cap_z[k] = cap[j][2];
      }

      if (norms != NULL) {
         if (__TUBE_DRAW_FACET_NORMALS) {
            for (j=0; j<ncp-1; j++) {
               k = ncp - j - 2;
               VEC_COPY ((&last_norm[3*k]), norms[j]);
            }
         } else {
            for (j=0; j<ncp; j++) {
               k = ncp - j - 1;
               VEC_COPY ((&last_norm[3*k]), norms[j]);
            }
         }
      }
   }

   /* &&&&&&&&&&&&&& start drawing cap &&&&&&&&&&&&& */

   for (i=0; i<__ROUND_TESS_PIECES; i++) {
      for (j=0; j<ncp; j++) {
         next_contour [3*j+2] -= cap_z[j];
         last_contour [3*j+2] -= cap_z[j];
         MAT_DOT_VEC_3X3 ( (&next_contour[3*j]), m, (&last_contour[3*j]));
         next_contour [3*j+2] += cap_z[j];
         last_contour [3*j+2] += cap_z[j];
      }

      if (norms != NULL) {
         for (j=0; j<ncp; j++) {
            MAT_DOT_VEC_3X3 ( (&next_norm[3*j]), m, (&last_norm[3*j]));
         }
      }

      /* OK, now render it all */
      if (norms == NULL) {
         draw_segment_plain (ncp, (gleVector *) next_contour, 
                                  (gleVector *) last_contour, 0, 0.0);
      } else
      if (__TUBE_DRAW_FACET_NORMALS) {
         draw_binorm_segment_facet_n (ncp, 
                               (gleVector *) next_contour, 
                               (gleVector *) last_contour,
                               (gleVector *) next_norm, 
                               (gleVector *) last_norm, 0, 0.0);
      } else {
         draw_binorm_segment_edge_n (ncp,
                               (gleVector *) next_contour, 
                               (gleVector *) last_contour,
                               (gleVector *) next_norm, 
                               (gleVector *) last_norm, 0, 0.0);
     }

      /* swap contours */
      tmp = next_contour;
      next_contour = last_contour;
      last_contour = tmp;

      tmp = next_norm;
      next_norm = last_norm;
      last_norm = tmp;
   }
   /* &&&&&&&&&&&&&& end drawing cap &&&&&&&&&&&&& */

   /* Thou shalt not leak memory */
   free (malloced_area);
}
void extrusion_angle_join (int ncp,		/* number of contour points */
                           gleDouble contour[][2],	/* 2D contour */
                           gleDouble cont_normal[][2], /* 2D normal vecs */
                           gleDouble up[3],	/* up vector for contour */
                           int npoints,		/* numpoints in poly-line */
                           gleDouble point_array[][3],	/* polyline */
                           float color_array[][3],	/* color of polyline */
                           gleDouble xform_array[][2][3])  /* 2D contour xforms */
{
   int i, j;
   int inext, inextnext;
   gleDouble m[4][4];
   gleDouble len;
   gleDouble len_seg;
   gleDouble diff[3];
   gleDouble bi_0[3], bi_1[3];		/* bisecting plane */
   gleDouble bisector_0[3], bisector_1[3];	/* bisecting plane */
   gleDouble end_point_0[3], end_point_1[3]; 
   gleDouble origin[3], neg_z[3];
   gleDouble yup[3];		/* alternate up vector */
   gleDouble *front_loop, *back_loop;   /* contours in 3D */
   char * mem_anchor;
   double *norm_loop; 
   double *front_norm, *back_norm, *tmp; /* contour normals in 3D */
   int first_time;

   /* By definition, the contour passed in has its up vector pointing in
    * the y direction */
   if (up == NULL) {
      yup[0] = 0.0;
      yup[1] = 1.0;
      yup[2] = 0.0;
   } else {
      VEC_COPY(yup, up);
   }

   /* ========== "up" vector sanity check ========== */
   (void) up_sanity_check (yup, npoints, point_array);

   /* the origin is at the origin */
   origin [0] = 0.0;
   origin [1] = 0.0;
   origin [2] = 0.0;

   /* and neg_z is at neg z */
   neg_z[0] = 0.0;
   neg_z[1] = 0.0;
   neg_z[2] = 1.0;

   /* ignore all segments of zero length */
   i = 1;
   inext = i;
   FIND_NON_DEGENERATE_POINT (inext, npoints, len, diff, point_array);
   len_seg = len;	/* store for later use */

   /* get the bisecting plane */
   bisecting_plane (bi_0, point_array[0], 
                          point_array[1], 
                          point_array[inext]);
   /* reflect the up vector in the bisecting plane */
   VEC_REFLECT (yup, yup, bi_0);

   /* malloc the storage we'll need for relaying changed contours to the
    * drawing routines. */
   mem_anchor =  malloc (2 * 3 * ncp * sizeof(double)
                      +  2 * 3 * ncp * sizeof(gleDouble));
   front_loop = (gleDouble *) mem_anchor;
   back_loop = front_loop + 3 * ncp;
   front_norm = (double *) (back_loop + 3 * ncp);
   back_norm = front_norm + 3 * ncp;
   norm_loop = front_norm;

   /* may as well get the normals set up now */
   if (cont_normal != NULL) {
      if (xform_array == NULL) {
         for (j=0; j<ncp; j++) {
            norm_loop[3*j] = cont_normal[j][0];
            norm_loop[3*j+1] = cont_normal[j][1];
            norm_loop[3*j+2] = 0.0;
         }
      } else {
         for (j=0; j<ncp; j++) {
            NORM_XFORM_2X2 ( (&front_norm[3*j]),
                              xform_array[inext-1],
                              cont_normal [j]);
            front_norm[3*j+2] = 0.0;
            back_norm[3*j+2] = 0.0;
         }
      }
   }

   first_time = TRUE;
   /* draw tubing, not doing the first segment */
   while (inext<npoints-1) {

      inextnext = inext;
      /* ignore all segments of zero length */
      FIND_NON_DEGENERATE_POINT (inextnext, npoints, len, diff, point_array);

      /* get the next bisecting plane */
      bisecting_plane (bi_1, point_array[i], 
                             point_array[inext], 
                             point_array[inextnext]);  

      /* rotate so that z-axis points down v2-v1 axis, 
       * and so that origen is at v1 */
      uviewpoint (m, point_array[i], point_array[inext], yup);
      PUSHMATRIX ();
      MULTMATRIX (m);

      /* rotate the bisecting planes into the local coordinate system */
      MAT_DOT_VEC_3X3 (bisector_0, m, bi_0);
      MAT_DOT_VEC_3X3 (bisector_1, m, bi_1);

      neg_z[2] = -len_seg;

      /* draw the tube */
      /* --------- START OF TMESH GENERATION -------------- */
      for (j=0; j<ncp; j++) {

         /* if there are normals, and there are either affine xforms, OR
          * path-edge normals need to be drawn, then compute local
          * coordinate system normals. 
          */
         if (cont_normal != NULL) {
          
            /* set up the back normals. (The front normals we inherit
             * from previous pass through the loop) */
            if (xform_array != NULL) {
               /* do up the normal vectors with the inverse transpose */
               NORM_XFORM_2X2 ( (&back_norm[3*j]),
                                 xform_array[inext],
                                 cont_normal [j]);
            }

            /* Note that if the xform array is NULL, then normals are
             * constant, and are set up outside of the loop.
             */

            /* 
             * if there are normal vectors, and the style calls for it, 
             * then we want to project the normal vectors into the
             * bisecting plane. (This style is needed to make toroids, etc. 
             * look good: Without this, segmentation artifacts show up
             * under lighting.
             */
            if (__TUBE_DRAW_PATH_EDGE_NORMALS) {
               /* Hmm, if no affine xforms, then we haven't yet set
                * back vector. So do it. */
               if (xform_array == NULL) {
                  back_norm[3*j] = cont_normal[j][0];
                  back_norm[3*j+1] = cont_normal[j][1];
               }
   
               /* now, start with a fresh normal (z component equal to
                * zero), project onto bisecting plane (by computing 
                * perpendicular componenet to bisect vector, and renormalize 
                * (since projected vector is not of unit length */ 
               front_norm[3*j+2] = 0.0;
               VEC_PERP ((&front_norm[3*j]), (&front_norm[3*j]), bisector_0);
               VEC_NORMALIZE ((&front_norm[3*j]));
    
               back_norm[3*j+2] = 0.0;
               VEC_PERP ((&back_norm[3*j]), (&back_norm[3*j]), bisector_1);
               VEC_NORMALIZE ((&back_norm[3*j]));
            } 
         } 

         /* Next, we want to define segements. We find the endpoints of
          * the segments by intersecting the contour with the bisecting
          * plane.  If there is no local affine transform, this is easy.
          *
          * If there is an affine tranform, then we want to remove the
          * torsional component, so that the intersection points won't
          * get twisted out of shape.  We do this by applying the
          * local affine transform to the entire coordinate system.
          */
         if (xform_array == NULL) {
            end_point_0 [0] = contour[j][0];
            end_point_0 [1] = contour[j][1];
   
            end_point_1 [0] = contour[j][0];
            end_point_1 [1] = contour[j][1];
         } else {
            /* transform the contour points with the local xform */
            MAT_DOT_VEC_2X3 (end_point_0,
                             xform_array[inext-1], contour[j]);
            MAT_DOT_VEC_2X3 (end_point_1,
                             xform_array[inext-1], contour[j]);
         }

         end_point_0 [2] = 0.0;
         end_point_1 [2] = - len_seg;

         /* The two end-points define a line.  Intersect this line
          * against the clipping plane defined by the PREVIOUS
          * tube segment.  */

         INNERSECT ((&front_loop[3*j]), /* intersection point (returned) */
                    origin,		/* point on intersecting plane */
                    bisector_0,		/* normal vector to plane */
                    end_point_0,	/* point on line */
                    end_point_1);	/* another point on the line */	

         /* The two end-points define a line.  Intersect this line
          * against the clipping plane defined by the NEXT
          * tube segment.  */

         /* if there's an affine coordinate change, be sure to use it */
         if (xform_array != NULL) {
            /* transform the contour points with the local xform */
            MAT_DOT_VEC_2X3 (end_point_0,
                             xform_array[inext], contour[j]);
            MAT_DOT_VEC_2X3 (end_point_1,
                             xform_array[inext], contour[j]);
         }

         INNERSECT ((&back_loop[3*j]),	/* intersection point (returned) */
                    neg_z,		/* point on intersecting plane */
                    bisector_1,		/* normal vector to plane */
                    end_point_0,	/* point on line */
                    end_point_1);	/* another point on the line */	

      }

      /* --------- END OF TMESH GENERATION -------------- */

      /* v^v^v^v^v^v^v^v^v  BEGIN END CAPS v^v^v^v^v^v^v^v^v^v^v^v */

      /* if end caps are required, draw them. But don't draw any 
       * but the very first and last caps */
      if (__TUBE_DRAW_CAP) {
         if (first_time) {
            if (color_array != NULL) C3F (color_array[inext-1]);
            first_time = FALSE;
            draw_angle_style_front_cap (ncp, bisector_0, (gleVector *) front_loop);
         }
         if (inext == npoints-2) {
            if (color_array != NULL) C3F (color_array[inext]);
            draw_angle_style_back_cap (ncp, bisector_1, (gleVector *) back_loop);
         }
      }
      /* v^v^v^v^v^v^v^v^v  END END CAPS v^v^v^v^v^v^v^v^v^v^v^v */

      /* |||||||||||||||||| START SEGMENT DRAW |||||||||||||||||||| */
      /* There are six different cases we can have for presence and/or
       * absecnce of colors and normals, and for interpretation of
       * normals. The blechy set of nested if statements below
       * branch to each of the six cases */
      if ((xform_array == NULL) && (!__TUBE_DRAW_PATH_EDGE_NORMALS)) {
         if (color_array == NULL) {
            if (cont_normal == NULL) {
               draw_segment_plain (ncp, (gleVector *) front_loop,
                                        (gleVector *) back_loop, inext, len_seg);
            } else
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_segment_facet_n (ncp, (gleVector *) front_loop,
                                          (gleVector *) back_loop, 
                                          (gleVector *) norm_loop, inext, len_seg);
            } else {
               draw_segment_edge_n (ncp, (gleVector *) front_loop, 
                                         (gleVector *) back_loop, 
                                         (gleVector *) norm_loop, inext, len_seg);
            }
         } else {
            if (cont_normal == NULL) {
               draw_segment_color (ncp, (gleVector *) front_loop, 
                                        (gleVector *) back_loop, 
                                   color_array[inext-1],
                                   color_array[inext], inext, len_seg);
            } else
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_segment_c_and_facet_n (ncp, 
                                   (gleVector *) front_loop, 
                                   (gleVector *) back_loop, 
                                   (gleVector *) norm_loop,
                                   color_array[inext-1],
                                   color_array[inext], inext, len_seg);
            } else {
               draw_segment_c_and_edge_n (ncp, 
                                   (gleVector *) front_loop, 
                                   (gleVector *) back_loop, 
                                   (gleVector *) norm_loop,
                                   color_array[inext-1],
                                   color_array[inext], inext, len_seg);
             }
          }
      } else {
         if (color_array == NULL) {
            if (cont_normal == NULL) {
               draw_segment_plain (ncp, (gleVector *) front_loop, 
                                        (gleVector *)  back_loop, inext, len_seg);
            } else 
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_binorm_segment_facet_n (ncp, (gleVector *) front_loop, 
                                                 (gleVector *) back_loop,
                                                 (gleVector *) front_norm, 
                                                 (gleVector *) back_norm,
                                                 inext, len_seg);
            } else {
               draw_binorm_segment_edge_n (ncp, (gleVector *) front_loop, 
                                                (gleVector *) back_loop,
                                                (gleVector *) front_norm,
                                                (gleVector *) back_norm,
                                                inext, len_seg);
            }
         } else {
            if (cont_normal == NULL) {
               draw_segment_color (ncp, (gleVector *) front_loop, 
                                        (gleVector *) back_loop, 
                                   color_array[inext-1],
                                   color_array[inext], inext, len_seg);
            } else
            if (__TUBE_DRAW_FACET_NORMALS) {
               draw_binorm_segment_c_and_facet_n (ncp, 
                                   (gleVector *) front_loop, 
                                   (gleVector *) back_loop, 
                                   (gleVector *) front_norm, 
                                   (gleVector *) back_norm, 
                                   color_array[inext-1],
                                   color_array[inext], inext, len_seg);
            } else {
               draw_binorm_segment_c_and_edge_n (ncp, 
                                   (gleVector *) front_loop, 
                                   (gleVector *) back_loop, 
                                   (gleVector *) front_norm, 
                                   (gleVector *) back_norm, 
                                   color_array[inext-1],
                                   color_array[inext], inext, len_seg);
             }
          }
      }
      /* |||||||||||||||||| END SEGMENT DRAW |||||||||||||||||||| */

      /* pop this matrix, do the next set */
      POPMATRIX ();

      /* bump everything to the next vertex */
      len_seg = len;
      i = inext;
      inext = inextnext;
      VEC_COPY (bi_0, bi_1);

      /* trade norm loops */
      tmp = front_norm;
      front_norm = back_norm;
      back_norm = tmp;

      /* reflect the up vector in the bisecting plane */
      VEC_REFLECT (yup, yup, bi_0);
   }

   /* be sure to free it all up */
   free (mem_anchor);

}
Exemple #6
0
void PositLoop(int NbPts, double **centeredImage, double** homogeneousWorldPts, double**objectMat, double f,double center[2], double** RotIn, double* TransIn,double** Rot, double* Trans){
    
    int i,j;
    double deltaX, deltaY,delta=0;
    double delta1,delta2;
    int count=0;
    bool converged= false;
    double Er,Erhvmax,Er1,Erhvmax1,Er2,Erhvmax2;
    long int Epr,Epr1,Epr2;
    double r3T[4];
    double a[3],b[3];
    
    
    
    
    /* allocation for Rot1 and Rot2*/
    double** Rot1;
    double** Rot2;
    Rot1=(double**)malloc(3*sizeof(double*));
    Rot2=(double**)malloc(3*sizeof(double*));
    for (i=0; i<3; i++) {
        Rot1[i]=(double*)malloc(3*sizeof(double));
        Rot2[i]=(double*)malloc(3*sizeof(double));
    }
    
    
    /* allocation for centeredImageAux*/
    double** centeredImageAux;
    centeredImageAux=(double **)malloc(NbPts* sizeof(double *));
    for (i=0;i<NbPts;i++) centeredImageAux[i]=(double *)malloc(2 * sizeof(double));
    /* end alloc*/
    
    /* allocation for wk*/
    double* wk;
    wk=(double *)malloc(NbPts*sizeof(double));
    /* end alloc*/
    
    /* initializaton for Rot and Trans*/
    for (i=0;i<3; i++) {
        for (j=0;j<3; j++) Rot[i][j]=RotIn[i][j];
        Trans[i]=TransIn[i];
    }
    
    /* initialization for imageCenteredAux*/
    for (i=0;i<NbPts; i++) {
        b[0]=homogeneousWorldPts[i][0];
        b[1]=homogeneousWorldPts[i][1];
        b[2]=homogeneousWorldPts[i][2];
        MAT_DOT_VEC_3X3(a, Rot, b);
        VEC_SUM(b,a,Trans);
        centeredImageAux[i][0]=b[0]/b[2];
        centeredImageAux[i][1]=b[1]/b[2];
    }
    
    
    count=0;
    while (!converged) {
        
        
        PositBranches(NbPts, centeredImageAux, homogeneousWorldPts, objectMat, Rot1, Rot2, Trans);
        
        delta1=0;
        delta2=0;
        for (i=0; i<NbPts; i++) {
            
            
            //Calculo error de proyeccion de rotacion1//
            b[0]=homogeneousWorldPts[i][0];
            b[1]=homogeneousWorldPts[i][1];
            b[2]=homogeneousWorldPts[i][2];
            MAT_DOT_VEC_3X3(a, Rot1, b);
            VEC_SUM(b,a,Trans);
            centeredImageAux[i][0]=b[0]/b[2];
            centeredImageAux[i][1]=b[1]/b[2];
            delta1+=pow((centeredImage[i][0]-centeredImageAux[i][0]),2)+pow((centeredImage[i][1]-centeredImageAux[i][1]),2);
            
            //Calculo error de proyeccion de rotacion2//
            b[0]=homogeneousWorldPts[i][0];
            b[1]=homogeneousWorldPts[i][1];
            b[2]=homogeneousWorldPts[i][2];
            MAT_DOT_VEC_3X3(a, Rot2, b);
            VEC_SUM(b,a,Trans);
            centeredImageAux[i][0]=b[0]/b[2];
            centeredImageAux[i][1]=b[1]/b[2];
            delta2+=pow((centeredImage[i][0]-centeredImageAux[i][0]),2)+pow((centeredImage[i][1]-centeredImageAux[i][1]),2);
        }
        
        if (delta1>delta2) {
            for (i=0;i<3;i++)
            {
                for (j=0;j<3;j++) Rot[i][j]=Rot2[i][j];
            }
            
        }
        else {
            for (i=0;i<3;i++)
            {
                for (j=0;j<3;j++) Rot[i][j]=Rot1[i][j];
            }
        }
       
//        MAT_PRINT_3X3(Rot);
//        VEC_PRINT(Trans);
        
        r3T[0]=Rot[2][0];
        r3T[1]=Rot[2][1];
        r3T[2]=Rot[2][2];
        r3T[3]=Trans[2];
        
        /* copmute wk, and the difference between images*/
        delta=0;
        for (i=0;i<NbPts;i++){
            wk[i]=0;
            deltaX=0;
            deltaY=0;
            for (j=0;j<4;j++){
                wk[i]+=homogeneousWorldPts[i][j]*r3T[j]/Trans[2];
            }
            b[0]=homogeneousWorldPts[i][0];
            b[1]=homogeneousWorldPts[i][1];
            b[2]=homogeneousWorldPts[i][2];
            MAT_DOT_VEC_3X3(a, Rot, b);
            VEC_SUM(b,a,Trans);
            centeredImageAux[i][0]=b[0]/b[2];
            centeredImageAux[i][1]=b[1]/b[2];
            deltaX-=centeredImageAux[i][0];
            deltaY-=centeredImageAux[i][1];
            centeredImageAux[i][0]=wk[i]*centeredImage[i][0];
            centeredImageAux[i][1]=wk[i]*centeredImage[i][1];
            deltaX+=centeredImageAux[i][0];
            deltaY+=centeredImageAux[i][1];
            delta+=deltaX*deltaX+deltaY*deltaY;
        }
        //        printf("puntos imagen\n");
        //        for (i=0; i<NbPts; i++) {
        //            printf("\n%f\t%f\n",centeredImageAux[i][0],centeredImageAux[i][1]);
        //        }
        //        printf("\nwk en iteracion %d\n",count);
        //        for (i=0; i<NbPts; i++) printf("%f\t",wk[i]);
        //        printf("\n");
        delta=sqrt(delta);
        converged=(count>0 && delta<0.001) || (count>100);
        count+=1;
        
        if (count>10&&delta>1) Rot[0][0]=2; /* if pose doesn't converge after 20 iteration, discard the pose, the value for count and delta is arbitrary*/
        
        //        printf("\nRotacion en iteracion %d: \n",count-1);
        //        printf("%f\t %f\t %f\n",Rot[0][0],Rot[0][1],Rot[0][2]);
        //        printf("%f\t %f\t %f\n",Rot[1][0],Rot[1][1],Rot[1][2]);
        //        printf("%f\t %f\t %f\n",Rot[2][0],Rot[2][1],Rot[2][2]);
        //        printf("Traslacion en iteracion %d: \n",count-1);
        //        printf("%f\t %f\t %f\n",Trans[0],Trans[1],Trans[2]);
        
        
        
        
    }
    
    /* deallocation*/
    for (i=0;i<3;i++){
        free(Rot1[i]);
        free(Rot2[i]);
    }
    free(Rot1);
    free(Rot2);
    for (i=0;i<NbPts;i++){
        free(centeredImageAux[i]);
    }
    free(centeredImageAux);
    free(wk);
    
}
Exemple #7
0
void kalman_sensors_update(kalman_state_n* state, float* measurement,float** A, float** H)
{

    float **S,**Sinv;
    S=(float**)malloc(6*sizeof(float*));
    for (int i=0; i<6; i++) S[i]=(float *)malloc(6*sizeof(float));
    Sinv=(float**)malloc(6*sizeof(float*));
    for (int i=0; i<6; i++) Sinv[i]=(float *)malloc(6*sizeof(float));

    float auxVec3[3],auxVec6[6],auxMat3x6[3][6],auxMat6x3[6][3],auxMat3x3[3][3];
    float Atrsp[3][3];
//    printf("\n \n ARRANCA KALMAN\n\n");
    /*predicted p*/
    TRANSPOSE_MATRIX_3X3(Atrsp, A);
    MATRIX_PRODUCT_3X3(auxMat3x3, state->p, Atrsp);
    MATRIX_PRODUCT_3X3(state->p, A, auxMat3x3);
    ACCUM_SCALE_MATRIX_3X3(state->p, 1, state->q)
    /*predicted x*/
    MAT_DOT_VEC_3X3(state->x, A, state->x);
//    VEC_PRINT(state->x);
    /*kalman gain*/
//    printf("\nARRANCA KALMAN GAIN\n");
    float Htrsp[3][6];
    TRANSPOSE_MATRIX_6X3(Htrsp, H);
//    MAT_PRINT_3X6(Htrsp);
    MATRIX_PRODUCT_3X3x3X6(auxMat3x6, state->p, Htrsp);
//    MAT_PRINT_3X6(auxMat3x6);
    MATRIX_PRODUCT_6X3x3X6(S, H, auxMat3x6);
    ACCUM_SCALE_MATRIX_6X6(S, 1.0, state->r);
//    MAT_PRINT_6X6(state->r);
//    MAT_PRINT_6X6(S);
    PseudoInverseGen(S, 6, 6, Sinv);
//    MAT_PRINT_6X6(Sinv);
    MATRIX_PRODUCT_3X6x6X6(state->k, auxMat3x6, Sinv);
//    MAT_PRINT_3X6(state->k);
//    printf("\TERMINA KALMAN GAIN\n");
    
    /*estimated x*/
//    VEC_PRINT(state->x);
    MAT_DOT_VEC_6X3(auxVec6, H, state->x);
//    VEC_PRINT_6(auxVec6);
    VEC_DIFF_6(auxVec6, measurement, auxVec6);
//    VEC_PRINT_6(measurement);
//    VEC_PRINT_6(auxVec6);
//    MAT_PRINT_3X6(state->k);
    MAT_DOT_VEC_3X6(auxVec3, state->k, auxVec6);
//    VEC_PRINT(auxVec3);
    VEC_SUM(state->x, state->x, auxVec3);
//    VEC_PRINT(state->x);
    
    /*estimated P*/
    MATRIX_PRODUCT_6X3x3X3(auxMat6x3, H, state->p);
    MATRIX_PRODUCT_3X6x6X3(auxMat3x3, state->k, auxMat6x3);
    ACCUM_SCALE_MATRIX_3X3(state->p, -1.0, auxMat3x3);
    
    for (int i=0; i<6; i++) {
        free(S[i]);
        free(Sinv[i]);
    }
    free(S);
    free(Sinv);
//    printf("\n \n TERMINA KALMAN\n\n");
}