// distance between a 3D point P and a line (defined by its
// normalized direction vector V and passing through a 3D point S)
double dist_line_point3D(double *V,double *S,double *P)
{
double cross_prod[3];
double sm[3];
double norm;
sm[0] = P[0] - S[0];
sm[1] = P[1] - S[1];
sm[2] = P[2] - S[2];
VEC_CROSS_PRODUCT(cross_prod,V,sm);
VEC_LENGTH(norm,cross_prod);
return norm;
}
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;
}
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);
}
/*
* The uview_dire subroutine computes and returns a 4x4 rotation
* matrix that puts the negative z axis along the direction v21 and
* puts the y axis along the up vector.
*
* It computes exactly the same matrix as the code above
* (uview_direction), but with an entirely different (and slower)
* algorithm.
*
* Note that the code below is slightly less robust than that above --
* it may croak if the supplied vectors are of zero length, or are
* parallel to each other ...
*/
void uview_dire (float m[4][4], /* returned */
float v21[3], /* input */
float up[3]) /* input */
{
gleDouble theta;
float v_hat_21 [3];
float z_hat [3];
float v_cross_z [3];
float u[3];
float y_hat [3];
float u_cross_y [3];
gleDouble cosine;
float zmat [4][4];
float upmat[4][4];
float dot;
/* perform rotation to z-axis only if not already
* pointing down z */
if ((v21[0] != 0.0 ) || (v21[1] != 0.0)) {
/* find the unit vector that points in the v21 direction */
VEC_COPY (v_hat_21, v21);
VEC_NORMALIZE (v_hat_21);
/* cosine theta equals v_hat dot z_hat */
cosine = - v_hat_21 [2];
theta = - acos (cosine);
/* Take cros product with z -- we need this, because we will rotate
* about this axis */
z_hat[0] = 0.0;
z_hat[1] = 0.0;
z_hat[2] = -1.0;
VEC_CROSS_PRODUCT (v_cross_z, v_hat_21, z_hat);
VEC_NORMALIZE (v_cross_z);
/* compute rotation matrix that takes -z axis to the v21 axis */
urot_axis (zmat, (float) theta, v_cross_z);
} else {
IDENTIFY_MATRIX_4X4 (zmat);
if (v21[2] > 0.0) {
/* if its pointing down the positive z-axis, flip it, so that
* we point down negative z-axis. We flip x so that the partiy
* isn't destroyed (looks like a rotation)
*/
zmat[0][0] = -1.0;
zmat[2][2] = -1.0;
}
}
/* --------------------- */
/* OK, now compute the part that takes the y-axis to the up vector */
VEC_COPY (u, up);
/* the rotation blows up, if the up vector is not perpendicular to
* the v21 vector. Let us make sure that this is so. */
VEC_PERP (u, u, v_hat_21);
/* need to run the y axis through above x-form, to see where it went */
y_hat[0] = zmat [1][0];
y_hat[1] = zmat [1][1];
y_hat[2] = zmat [1][2];
/* perform rotation to up-axis only if not already
* pointing along y axis */
VEC_DOT_PRODUCT (dot, y_hat, u);
if ((-1.0 < dot) && (dot < 1.0)) {
/* make sure that up really is a unit vector */
VEC_NORMALIZE (u);
/* cosine phi equals y_hat dot up_vec */
VEC_DOT_PRODUCT (cosine, u, y_hat);
theta = - acos (cosine);
/* Take cross product with y */
VEC_CROSS_PRODUCT (u_cross_y, u, y_hat);
VEC_NORMALIZE (u_cross_y);
/* As a matter of fact, u_cross_y points either in the v21 direction,
* or in the minus v21 direction. In either case, we needed to compute
* it, because the the arccosine function returns values only for
* 0 to 180 degree, not 0 to 360, which is what we need. The
* cross-product helps us make up for this.
*/
/* rotate about the NEW z axis (i.e. v21) by the cosine */
urot_axis (upmat, (float) theta, u_cross_y);
} else {
IDENTIFY_MATRIX_4X4 (upmat);
if (dot == -1.0) {
/* if its pointing along the negative y-axis, flip it, so that
* we point along the positive y-axis. We flip x so that the partiy
* isn't destroyed (looks like a rotation)
*/
upmat[0][0] = -1.0;
upmat[1][1] = -1.0;
}
}
MATRIX_PRODUCT_4X4 (m, zmat, upmat);
}