quadrilateral quadrilateralNew(double vertices[QL_NB_VERTICES][2]) {
quadrilateral ql;
double sidelength;
double side[2];
VEC_ZERO_2(ql.center);
ql.perimeter = 0;
for (int i = 0; i < QL_NB_VERTICES; i++) {
VEC_COPY_2(ql.vertices[i], vertices[i]);
ql.center[0] += (1.0 / QL_NB_VERTICES) * (ql.vertices[i][0]);
ql.center[1] += (1.0 / QL_NB_VERTICES) * (ql.vertices[i][1]);
VEC_DIFF_2(side, ql.vertices[i], vertices[(i+1)%QL_NB_VERTICES]);
//VEC_ANGLE_2(ql.dirs_deg[i],side);
VEC_LENGTH_2(sidelength, side);
ql.perimeter += sidelength;
}
ql.id = 0; //constructed - not assigned
return ql;
}
int getMarkerVertices(markerQr marker, double **imgPts) {
int contador = 0;
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
for (int j = 0; j < QLSET_NB_QLS; j++) {
for (int k = 0; k < QL_NB_VERTICES; k++) {
VEC_COPY_2(imgPts[contador], marker.qlSet[i].ql[j].vertices[k]);
contador++;
}
}
}
return 0;
}
int orderQlSetArr3(quadrilateralSet *qlSet) {
quadrilateralSet qlSetTemp[MRKR_NB_QLSETS];
double vect[MRKR_NB_QLSETS][2];
double leng[MRKR_NB_QLSETS];
double dirs[2][2];
int ref_index = -1;
/*compute the 3 posible principal directions*/
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
VEC_DIFF_2(vect[i], qlSet[i].center,
qlSet[(i+1)%MRKR_NB_QLSETS].center);
VEC_LENGTH_2(leng[i], vect[i]);
VEC_NORMALIZE_2(vect[i]);
}
/*find out y direction by minimum distance*/
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
if ((leng[i] < leng[(i + 1) % MRKR_NB_QLSETS])
&& (leng[i] < leng[(i + 2) % MRKR_NB_QLSETS])) {
VEC_COPY_2(dirs[1], vect[i]);
VEC_NORMALIZE_2(dirs[1]);
ref_index = i;
break;
}
}
double c[3];
double a[3] = {vect[ref_index][0],vect[ref_index][1],0};
double b[3] = {vect[(ref_index+1)%MRKR_NB_QLSETS][0],vect[(ref_index+1)%MRKR_NB_QLSETS][1],0};
VEC_CROSS_PRODUCT(c,a,b);
if (c[2]<0){
qlSetTemp[0] = qlSet[(ref_index + 0) % MRKR_NB_QLSETS];
qlSetTemp[1] = qlSet[(ref_index + 2) % MRKR_NB_QLSETS];
qlSetTemp[2] = qlSet[(ref_index + 1) % MRKR_NB_QLSETS];
} else {
qlSetTemp[0] = qlSet[(ref_index + 1) % MRKR_NB_QLSETS];
qlSetTemp[1] = qlSet[(ref_index + 2) % MRKR_NB_QLSETS];
qlSetTemp[2] = qlSet[(ref_index + 0) % MRKR_NB_QLSETS];
}
for (int i = 0; i < MRKR_NB_QLSETS; i++)
qlSet[i] = qlSetTemp[i];
return 0;
}
int orderQlSetArr(quadrilateralSet *qlSet) {
quadrilateralSet qlSetTemp[MRKR_NB_QLSETS];
double vect[MRKR_NB_QLSETS][2];
double leng[MRKR_NB_QLSETS];
double dirs[2][2];
int ref_index = -1;
/*compute the 3 posible principal directions*/
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
VEC_DIFF_2(vect[i], qlSet[i].center,
qlSet[(i+1)%MRKR_NB_QLSETS].center);
VEC_LENGTH_2(leng[i], vect[i]);
VEC_NORMALIZE_2(vect[i]);
}
/*find out y direction by minimum distance*/
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
if ((leng[i] < leng[(i + 1) % MRKR_NB_QLSETS])
&& (leng[i] < leng[(i + 2) % MRKR_NB_QLSETS])) {
VEC_COPY_2(dirs[1], vect[i]);
VEC_NORMALIZE_2(dirs[1]);
ref_index = i;
break;
}
}
double cos_angle1, cos_angle2;
/*find out origin by widest angle*/
VEC_DOT_PRODUCT_2(cos_angle1, dirs[1], vect[(ref_index+1)%MRKR_NB_QLSETS]);
VEC_DOT_PRODUCT_2(cos_angle2, dirs[1], vect[(ref_index+2)%MRKR_NB_QLSETS]);
if (fabs(cos_angle1) < fabs(cos_angle2)) {
qlSetTemp[0] = qlSet[(ref_index + 1) % MRKR_NB_QLSETS];
qlSetTemp[1] = qlSet[(ref_index + 2) % MRKR_NB_QLSETS];
qlSetTemp[2] = qlSet[(ref_index) % MRKR_NB_QLSETS];
} else {
qlSetTemp[0] = qlSet[(ref_index) % MRKR_NB_QLSETS];
qlSetTemp[1] = qlSet[(ref_index + 2) % MRKR_NB_QLSETS];
qlSetTemp[2] = qlSet[(ref_index + 1) % MRKR_NB_QLSETS];
}
for (int i = 0; i < MRKR_NB_QLSETS; i++)
qlSet[i] = qlSetTemp[i];
return 0;
}
int orderMarkerVertices(markerQr *marker) {
double px, py;
double vect[2];
double temp_vertices[4][2];
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
for (int j = 0; j < QLSET_NB_QLS; j++) {
/*FIXME: implement circular shift for less general but better performance (?) */
for (int k = 0; k < QL_NB_VERTICES; k++) {
if ((marker->qlSet[i].ql[j].id) == -1) {
VEC_COPY_2(temp_vertices[k],
marker->qlSet[i].ql[j].vertices[k]);
} else {
VEC_DIFF_2(vect, marker->qlSet[i].ql[j].vertices[k],
marker->qlSet[i].center);
VEC_DOT_PRODUCT_2(px, vect, marker->directions[0]);
VEC_DOT_PRODUCT_2(py, vect, marker->directions[1]);
if (px >= 0 && py >= 0) {
VEC_COPY_2(temp_vertices[0],
marker->qlSet[i].ql[j].vertices[k]);
} else if (px >= 0 && py <= 0) {
VEC_COPY_2(temp_vertices[1],
marker->qlSet[i].ql[j].vertices[k]);
} else if (px <= 0 && py <= 0) {
VEC_COPY_2(temp_vertices[2],
marker->qlSet[i].ql[j].vertices[k]);
} else if (px <= 0 && py >= 0) {
VEC_COPY_2(temp_vertices[3],
marker->qlSet[i].ql[j].vertices[k]);
}
}
}
for (int l = 0; l < QL_NB_VERTICES; l++)
VEC_COPY_2(marker->qlSet[i].ql[j].vertices[l],
temp_vertices[l]);
}
}
return 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);
}
int orderQlSetArr2(quadrilateralSet *qlSet) {
quadrilateralSet qlSetTemp[MRKR_NB_QLSETS];
double vect[MRKR_NB_QLSETS][2];
double leng[MRKR_NB_QLSETS];
double dirs[2][2];
double angles[2] = { 0, 0 };
double ref_angle = -1;
double ref_index = -1;
double test_angle[2];
/*compute the 3 posible principal directions*/
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
VEC_DIFF_2(vect[i], qlSet[i].center,
qlSet[(i+1)%MRKR_NB_QLSETS].center);
VEC_LENGTH_2(leng[i], vect[i]);
}
/*find out y direction by minimum distance criteria*/
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
if ((leng[i] < leng[(i + 1) % MRKR_NB_QLSETS])
&& (leng[i] < leng[(i + 2) % MRKR_NB_QLSETS])) {
VEC_COPY_2(dirs[1], vect[i]);
VEC_ANGLE_2(ref_angle, dirs[1]);
ref_index = i;
break;
}
}
/*ahora vamos sacando dirección a dirección y clasificando con un threshold
* FIXME: se podría hacer en la función getQlSetDirections o cambiando nombre por getQlSetArrDirections*/
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
for (int j = 0; j < QLSET_NB_QLS; j++) {
for (int k = 0; k < QL_NB_VERTICES; k++) {
if (fabs(qlSet[i].ql[j].dirs_deg[k] - ref_angle) < ANGLE_TH
|| fabs(qlSet[i].ql[j].dirs_deg[k] - ref_angle)
> (180 - ANGLE_TH))
angles[1] += qlSet[i].ql[j].dirs_deg[k];
else
angles[0] += qlSet[i].ql[j].dirs_deg[k];
}
}
}
//mean angle acum
angles[0] = angles[0] / (MRKR_NB_QLSETS * QLSET_NB_QLS * QL_NB_VERTICES);
angles[1] = angles[1] / (MRKR_NB_QLSETS * QLSET_NB_QLS * QL_NB_VERTICES);
for (int i = 0; i < MRKR_NB_QLSETS; i++) {
VEC_DIFF_2(dirs[0], qlSet[i].center,
qlSet[(i+1)%MRKR_NB_QLSETS].center);
VEC_DIFF_2(dirs[1], qlSet[i].center,
qlSet[(i+2)%MRKR_NB_QLSETS].center);
VEC_ANGLE_2(test_angle[0], dirs[0]);
VEC_ANGLE_2(test_angle[1], dirs[1]);
if ((fabs(angles[0] - test_angle[0]) < ANGLE_TH
|| fabs(angles[0] - test_angle[0]) > (180 - ANGLE_TH))
&& (fabs(angles[1] - test_angle[1]) < ANGLE_TH
|| fabs(angles[1] - test_angle[1]) > (180 - ANGLE_TH))) {
qlSetTemp[0] = qlSet[i];
qlSetTemp[1] = qlSet[(i + 1) % MRKR_NB_QLSETS];
qlSetTemp[2] = qlSet[(i + 2) % MRKR_NB_QLSETS];
break;
} else if ((fabs(angles[0] - test_angle[1]) < ANGLE_TH
|| fabs(angles[0] - test_angle[1]) > (180 - ANGLE_TH))
&& (fabs(angles[1] - test_angle[0]) < ANGLE_TH
|| fabs(angles[1] - test_angle[0]) > (180 - ANGLE_TH))) {
qlSetTemp[0] = qlSet[i];
qlSetTemp[1] = qlSet[(i + 2) % MRKR_NB_QLSETS];
qlSetTemp[2] = qlSet[(i + 1) % MRKR_NB_QLSETS];
break;
}
}
/*
idx
for (int i=0;i<MRKR_NB_QLSETS;i++) {
qlSet[i] = qlSetTemp[i];
if (qlSet[i].id == -1)
}
*/
if (ref_index != -1)
return 0;
else
return ref_index;
}
