// 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);
}
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);
}
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);
}
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);
}
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);
}
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");
}