/*!
\param triangle
\param capsule
\param contacts Contains the closest points on the capsule, and the normal points to triangle
\post The contacts array is not set to 0. It adds aditional contacts
*/
int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
{
GUINT old_contact_size = contacts->m_size;
gim_closest_point_triangle_segment(triangle,capsule->m_point1,capsule->m_point2,contacts);
GIM_CONTACT * pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,(*contacts));
pcontact+= old_contact_size;
if(pcontact->m_depth > capsule->m_radius)
{
contacts->m_size = old_contact_size;
return 0;
}
vec3f vec;
while(old_contact_size<contacts->m_size)
{
//Scale the normal for pointing to triangle
VEC_SCALE(pcontact->m_normal,-1.0f,pcontact->m_normal);
//Fix the contact point
VEC_SCALE(vec,capsule->m_radius,pcontact->m_normal);
VEC_SUM(pcontact->m_point,vec,pcontact->m_point);
//Fix the depth
pcontact->m_depth = capsule->m_radius - pcontact->m_depth;
pcontact++;
old_contact_size++;
}
return 1;
}
void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
GDYNAMIC_ARRAY * dest_contacts)
{
dest_contacts->m_size = 0;
//Traverse the source contacts
GUINT32 source_count = source_contacts->m_size;
if(source_count==0) return;
GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
//add the unique contact
GIM_CONTACT * pcontact = 0;
GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
//set the first contact
GIM_COPY_CONTACTS(pcontact, psource_contacts);
if(source_count==1) return;
//scale the first contact
VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
psource_contacts++;
//Average the contacts
GUINT32 i;
for(i=1;i<source_count;i++)
{
VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
psource_contacts++;
}
GREAL divide_average = 1.0f/((GREAL)source_count);
VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
VEC_NORMALIZE(pcontact->m_normal);
/*GREAL normal_len;
VEC_INV_LENGTH(pcontact->m_normal,normal_len);
VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
//Deep = LEN(normal)/SQRT(source_count)
GIM_SQRT(divide_average,divide_average);
pcontact->m_depth = divide_average/normal_len;
*/
}
void draw_cut_style_cap_callback (int iloop,
double cap[][3],
float face_color[3],
gleDouble cut_vector[3],
gleDouble bisect_vector[3],
double norms[][3],
int frontwards)
{
int i;
if (face_color != NULL) C3F (face_color);
if (frontwards) {
/* if lighting is on, specify the endcap normal */
if (cut_vector != NULL) {
/* if normal pointing in wrong direction, flip it. */
if (cut_vector[2] < 0.0) {
VEC_SCALE (cut_vector, -1.0, cut_vector);
}
N3F_D (cut_vector);
}
BGNPOLYGON();
for (i=0; i<iloop; i++) {
V3F_D (cap[i], i, FRONT_CAP);
}
ENDPOLYGON();
} else {
/* if lighting is on, specify the endcap normal */
if (cut_vector != NULL) {
/* if normal pointing in wrong direction, flip it. */
if (cut_vector[2] > 0.0)
{
VEC_SCALE (cut_vector, -1.0, cut_vector);
}
N3F_D (cut_vector);
}
/* the sense of the loop is reversed for backfacing culling */
BGNPOLYGON();
for (i=iloop-1; i>-1; i--) {
V3F_D (cap[i], i, BACK_CAP);
}
ENDPOLYGON();
}
}
void draw_angle_style_front_cap (int ncp, /* number of contour points */
gleDouble bi[3], /* biscetor */
gleDouble point_array[][3]) /* polyline */
{
int j;
#ifdef OPENGL_10
GLUtriangulatorObj *tobj;
#endif /* OPENGL_10 */
if (bi[2] < 0.0) {
VEC_SCALE (bi, -1.0, bi);
}
#ifdef GL_32
/* old-style gl handles concave polygons no problem, so the code is
* simple. New-style gl is a lot more tricky. */
/* draw the end cap */
BGNPOLYGON ();
N3F (bi);
for (j=0; j<ncp; j++) {
V3F (point_array[j], j, FRONT_CAP);
}
ENDPOLYGON ();
#endif /* GL_32 */
#ifdef OPENGL_10
N3F(bi);
tobj = gluNewTess ();
gluTessCallback (tobj, GLU_BEGIN, glBegin);
gluTessCallback (tobj, GLU_VERTEX, glVertex3dv);
gluTessCallback (tobj, GLU_END, glEnd);
gluBeginPolygon (tobj);
for (j=0; j<ncp; j++) {
gluTessVertex (tobj, point_array[j], point_array[j]);
}
gluEndPolygon (tobj);
gluDeleteTess (tobj);
#endif /* OPENGL_10 */
}
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);
}
static void
draw_cut_style_cap_callback (int iloop,
double cap[][3],
float face_color[3],
gleDouble cut_vector[3],
gleDouble bisect_vector[3],
double norms[][3],
int frontwards)
{
#ifdef DELICATE_TESSELATOR
int i;
int is_colinear;
double *previous_vertex = 0x0;
double *first_vertex = 0x0;
#endif /* DELICATE_TESSELATOR */
#ifdef OPENGL_10
GLUtriangulatorObj *tobj;
tobj = gluNewTess ();
gluTessCallback (tobj, GLU_BEGIN, glBegin);
gluTessCallback (tobj, GLU_VERTEX, glVertex3dv);
gluTessCallback (tobj, GLU_END, glEnd);
#endif /* OPENGL_10 */
if (face_color != NULL) C3F (face_color);
if (frontwards) {
/* if lighting is on, specify the endcap normal */
if (cut_vector != NULL) {
/* if normal pointing in wrong direction, flip it. */
if (cut_vector[2] < 0.0) {
VEC_SCALE (cut_vector, -1.0, cut_vector);
}
N3F_D (cut_vector);
}
#ifdef GL_32
BGNPOLYGON();
for (i=0; i<iloop; i++) {
V3F_D (cap[i], i, FRONT_CAP);
}
ENDPOLYGON();
#endif /* GL_32 */
#ifdef OPENGL_10
/* If you have a tesselator that is happy with anything,
* including degenerate points, colinear segments, etc.
* then define this. Otherwise, pick one of the others.
*
* I beleive that the stock SGI tesselator is "lenient",
* despite explicit disclaimers in the documentation.
* (circa 1995).
*
* The Mesa tesselator is not at all forgiving of
* degenerate points.
* (circa 1997-1998)
*/
#ifdef LENIENT_TESSELATOR
gluBeginPolygon (tobj);
for (i=0; i<iloop; i++) {
gluTessVertex (tobj, cap[i], cap[i]);
}
gluEndPolygon (tobj);
#endif /* LENIENT_TESSELATOR */
#ifdef DELICATE_TESSELATOR
gluBeginPolygon (tobj);
first_vertex = 0x0;
previous_vertex = cap[iloop-1];
for (i=0; i<iloop-1; i++) {
COLINEAR (is_colinear, previous_vertex, cap[i], cap[i+1]);
if (!is_colinear) {
gluTessVertex (tobj, cap[i], cap[i]);
previous_vertex = cap[i];
if (!first_vertex) first_vertex = previous_vertex;
}
}
if (!first_vertex) first_vertex = cap[0];
COLINEAR (is_colinear, previous_vertex, cap[iloop-1], first_vertex);
if (!is_colinear) gluTessVertex (tobj, cap[iloop-1], cap[iloop-1]);
gluEndPolygon (tobj);
#endif /* DELICATE_TESSELATOR */
#endif /* OPENGL_10 */
} else {
/* if lighting is on, specify the endcap normal */
if (cut_vector != NULL) {
/* if normal pointing in wrong direction, flip it. */
if (cut_vector[2] > 0.0) {
VEC_SCALE (cut_vector, -1.0, cut_vector);
}
N3F_D (cut_vector);
}
/* the sense of the loop is reversed for backfacing culling */
#ifdef GL_32
BGNPOLYGON();
for (i=iloop-1; i>-1; i--) {
V3F_D (cap[i], i, BACK_CAP);
}
ENDPOLYGON();
#endif /* GL_32 */
#ifdef OPENGL_10
#ifdef LENIENT_TESSELATOR
gluBeginPolygon (tobj);
for (i=iloop-1; i>-1; i--) {
gluTessVertex (tobj, cap[i], cap[i]);
}
gluEndPolygon (tobj);
#endif /* LENIENT_TESSELATOR */
#ifdef DELICATE_TESSELATOR
gluBeginPolygon (tobj);
first_vertex = 0x0;
previous_vertex = cap[0];
for (i=iloop-1; i>0; i--) {
COLINEAR (is_colinear, previous_vertex, cap[i], cap[i-1]);
if (!is_colinear) {
gluTessVertex (tobj, cap[i], cap[i]);
previous_vertex = cap[i];
if (!first_vertex) first_vertex = previous_vertex;
}
}
if (!first_vertex) first_vertex = cap[iloop-1];
COLINEAR (is_colinear, previous_vertex, cap[0], first_vertex);
if (!is_colinear) gluTessVertex (tobj, cap[0], cap[0]);
gluEndPolygon (tobj);
#endif /* DELICATE_TESSELATOR */
#endif /* OPENGL_10 */
}
#ifdef OPENGL_10
gluDeleteTess (tobj);
#endif /* OPENGL_10 */
}
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);
}
/* ARGSUSED4 */
static void draw_cut_style_cap_callback (int iloop,
double cap[][3],
float face_color[3],
gleDouble cut_vector[3],
gleDouble bisect_vector[3],
double norms[][3],
int frontwards)
{
int i;
#ifdef OPENGL_10
GLUtriangulatorObj *tobj;
tobj = gluNewTess ();
gluTessCallback (tobj, GLU_BEGIN, (void (CALLBACK*)()) glBegin);
gluTessCallback (tobj, GLU_VERTEX, (void (CALLBACK*)()) glVertex3dv);
gluTessCallback (tobj, GLU_END, (void (CALLBACK*)()) glEnd);
#endif /* OPENGL_10 */
if (face_color != NULL) C3F (face_color);
if (frontwards) {
/* if lighting is on, specify the endcap normal */
if (cut_vector != NULL) {
/* if normal pointing in wrong direction, flip it. */
if (cut_vector[2] < 0.0) {
VEC_SCALE (cut_vector, -1.0, cut_vector);
}
N3F_D (cut_vector);
}
#ifdef GL_32
BGNPOLYGON();
for (i=0; i<iloop; i++) {
V3F_D (cap[i], i, FRONT_CAP);
}
ENDPOLYGON();
#endif /* GL_32 */
#ifdef OPENGL_10
gluBeginPolygon (tobj);
for (i=0; i<iloop; i++) {
gluTessVertex (tobj, cap[i], cap[i]);
}
gluEndPolygon (tobj);
#endif /* OPENGL_10 */
} else {
/* if lighting is on, specify the endcap normal */
if (cut_vector != NULL) {
/* if normal pointing in wrong direction, flip it. */
if (cut_vector[2] > 0.0) {
VEC_SCALE (cut_vector, -1.0, cut_vector);
}
N3F_D (cut_vector);
}
/* the sense of the loop is reversed for backfacing culling */
#ifdef GL_32
BGNPOLYGON();
for (i=iloop-1; i>-1; i--) {
V3F_D (cap[i], i, BACK_CAP);
}
ENDPOLYGON();
#endif /* GL_32 */
#ifdef OPENGL_10
gluBeginPolygon (tobj);
for (i=iloop-1; i>-1; i--) {
gluTessVertex (tobj, cap[i], cap[i]);
}
gluEndPolygon (tobj);
#endif /* OPENGL_10 */
}
#ifdef OPENGL_10
gluDeleteTess (tobj);
#endif /* OPENGL_10 */
}
int _gim_triangle_triangle_collision(
GIM_TRIANGLE_DATA *tri1,
GIM_TRIANGLE_DATA *tri2,
GIM_TRIANGLE_CONTACT_DATA * contact_data)
{
//Cache variables for triangle intersection
GUINT32 _max_candidates[MAX_TRI_CLIPPING];
vec3f _temp_clip[MAX_TRI_CLIPPING];
GUINT32 _temp_clip_count = 0;
vec3f _temp_clip2[MAX_TRI_CLIPPING];
GUINT32 _temp_clip_count2 = 0;
vec3f clipped_points2[MAX_TRI_CLIPPING];
vec3f deep_points2[MAX_TRI_CLIPPING];
vec3f clipped_points1[MAX_TRI_CLIPPING];
vec3f deep_points1[MAX_TRI_CLIPPING];
//State variabnles
GUINT32 mostdir=0;
GUINT32 clipped2_count=0;
//Clip tri2 by tri1 edges
CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri2->m_vertices,(&tri1->m_planes.m_planes[1]), clipped_points2, clipped2_count);
if(clipped2_count == 0 )
{
return 0;//Reject
}
//find most deep interval face1
GUINT32 deep2_count=0;
GREAL maxdeep;
MOST_DEEP_POINTS((tri1->m_planes.m_planes[0]), clipped_points2, clipped2_count, deep_points2, deep2_count, maxdeep);
if(deep2_count==0)
{
// *perror = 0.0f;
return 0;//Reject
}
//Normal pointing to triangle1
VEC_SCALE(contact_data->m_separating_normal,-1.0f,(tri1->m_planes.m_planes[0]));
//Clip tri1 by tri2 edges
GUINT32 clipped1_count=0;
CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri1->m_vertices,(&tri2->m_planes.m_planes[1]), clipped_points1, clipped1_count);
if(clipped2_count == 0 )
{
// *perror = 0.0f;
return 0;//Reject
}
//find interval face2
GUINT32 deep1_count=0;
GREAL dist;
MOST_DEEP_POINTS((tri2->m_planes.m_planes[0]), clipped_points1, clipped1_count, deep_points1, deep1_count, dist);
if(deep1_count==0)
{
// *perror = 0.0f;
return 0;
}
if(dist<maxdeep)
{
maxdeep = dist;
mostdir = 1;
VEC_COPY(contact_data->m_separating_normal,(tri2->m_planes.m_planes[0]));
}
//set deep
contact_data->m_penetration_depth = maxdeep;
////check most dir for contacts
if(mostdir==0)
{
contact_data->m_point_count = deep2_count;
for(mostdir=0; mostdir<deep2_count; mostdir++)
{
VEC_COPY(contact_data->m_points[mostdir] ,deep_points2[mostdir]);
}
}
else
{
contact_data->m_point_count = deep1_count;
for(mostdir=0; mostdir<deep1_count; mostdir++)
{
VEC_COPY(contact_data->m_points[mostdir] ,deep_points1[mostdir]);
}
}
return 1;
}
/*
* The uviewdirection 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.
*
* Note that this code is fairly tolerant of "weird" paramters.
* It normalizes when necessary, it does nothing when vectors are of
* zero length, or are co-linear. This code shouldn't croak, no matter
* what the user sends in as arguments.
*/
void uview_direction (gleDouble m[4][4], /* returned */
gleDouble v21[3], /* input */
gleDouble up[3]) /* input */
{
gleDouble amat[4][4];
gleDouble bmat[4][4];
gleDouble cmat[4][4];
gleDouble v_hat_21[3];
gleDouble v_xy[3];
gleDouble sine, cosine;
gleDouble len;
gleDouble up_proj[3];
gleDouble tmp[3];
/* find the unit vector that points in the v21 direction */
VEC_COPY (v_hat_21, v21);
VEC_LENGTH (len, v_hat_21);
if (len != 0.0) {
len = 1.0 / len;
VEC_SCALE (v_hat_21, len, v_hat_21);
/* rotate z in the xz-plane until same latitude */
sine = sqrt ( 1.0 - v_hat_21[2] * v_hat_21[2]);
ROTY_CS (amat, (-v_hat_21[2]), (-sine));
} else {
/* error condition: zero length vecotr passed in -- do nothing */
IDENTIFY_MATRIX_4X4 (amat);
}
/* project v21 onto the xy plane */
v_xy[0] = v21[0];
v_xy[1] = v21[1];
v_xy[2] = 0.0;
VEC_LENGTH (len, v_xy);
/* rotate in the x-y plane until v21 lies on z axis ---
* but of course, if its already there, do nothing */
if (len != 0.0) {
/* want xy projection to be unit vector, so that sines/cosines pop out */
len = 1.0 / len;
VEC_SCALE (v_xy, len, v_xy);
/* rotate the projection of v21 in the xy-plane over to the x axis */
ROTZ_CS (bmat, v_xy[0], v_xy[1]);
/* concatenate these together */
MATRIX_PRODUCT_4X4 (cmat, amat, bmat);
} else {
/* no-op -- vector is already in correct position */
COPY_MATRIX_4X4 (cmat, amat);
}
/* up vector really should be perpendicular to the x-form direction --
* Use up a couple of cycles, and make sure it is,
* just in case the user blew it.
*/
VEC_PERP (up_proj, up, v_hat_21);
VEC_LENGTH (len, up_proj);
if (len != 0.0) {
/* normalize the vector */
len = 1.0/len;
VEC_SCALE (up_proj, len, up_proj);
/* compare the up-vector to the y-axis to get the cosine of the angle */
tmp [0] = cmat [1][0];
tmp [1] = cmat [1][1];
tmp [2] = cmat [1][2];
VEC_DOT_PRODUCT (cosine, tmp, up_proj);
/* compare the up-vector to the x-axis to get the sine of the angle */
tmp [0] = cmat [0][0];
tmp [1] = cmat [0][1];
tmp [2] = cmat [0][2];
VEC_DOT_PRODUCT (sine, tmp, up_proj);
/* rotate to align the up vector with the y-axis */
ROTZ_CS (amat, cosine, -sine);
/* This xform, although computed last, acts first */
MATRIX_PRODUCT_4X4 (m, amat, cmat);
} else {
/* error condition: up vector is indeterminate (zero length)
* -- do nothing */
COPY_MATRIX_4X4 (m, cmat);
}
}
/*!
\param triangle
\param s1
\param s2
\param contacts Contains the closest points on the segment (1,2), and the normal points to segment, and m_depth contains the distance
\post The contacts array is not set to 0. It adds aditional contacts
*/
void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,vec3f s2, GDYNAMIC_ARRAY * contacts)
{
vec3f segment_points[4];
vec3f closest_points[2];
GUINT intersection_type, out_edge= 10;
GREAL dis, dis_temp,perpend;
vec4f sdiff;
dis = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s1);
dis_temp = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s2);
if(dis<=0.0f && dis_temp<=0.0f) return;
VEC_DIFF(sdiff,s2,s1);
perpend = VEC_DOT(sdiff,triangle->m_planes.m_planes[0]);
if(!IS_ZERO(perpend)) // Not perpendicular
{
if(dis<dis_temp)
{
VEC_COPY(closest_points[0],s1);
}
else
{
dis = dis_temp;
VEC_COPY(closest_points[0],s2);
}
//Testing segment vertices over triangle
if(dis>=0.0f && dis_temp>=0.0f)
{
POINT_IN_HULL(closest_points[0],(&triangle->m_planes.m_planes[1]),3,out_edge);
if(out_edge==0)//Point over face
{
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
return;
}
}
else
{
PLANE_CLIP_SEGMENT(s1,s2,triangle->m_planes.m_planes[0],closest_points[1]);
POINT_IN_HULL(closest_points[1],(&triangle->m_planes.m_planes[1]),3,out_edge);
if(out_edge==0)//Point over face
{
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
return;
}
}
}
else // Perpendicular Face
{
//out_edge=10
//Clip segment by triangle
// Edge1
PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,triangle->m_planes.m_planes[1],segment_points[0],segment_points[1],intersection_type);
if(intersection_type==0||intersection_type==1)
{
out_edge = 0;
VEC_COPY(closest_points[0],segment_points[0]);
}
else
{
//Edge2
PLANE_CLIP_SEGMENT_CLOSEST(segment_points[0],segment_points[1],triangle->m_planes.m_planes[2],segment_points[2],segment_points[3],intersection_type);
if(intersection_type==0||intersection_type==1)
{
out_edge = 1;
VEC_COPY(closest_points[0],segment_points[3]);
}
else
{
//Edge3
PLANE_CLIP_SEGMENT_CLOSEST(segment_points[2],segment_points[3],triangle->m_planes.m_planes[3],closest_points[0],closest_points[1],intersection_type);
if(intersection_type==0||intersection_type==1)
{
out_edge = 2;
}
}
}
//POST closest_points[0] and closest_points[1] are inside the triangle, if out_edge>2
if(out_edge>2) // Over triangle
{
dis = VEC_DOT(closest_points[0],triangle->m_planes.m_planes[0]);
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
GIM_PUSH_CONTACT((*contacts),closest_points[1] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
return;
}
}
//Find closest edges
out_edge = 10;
dis = G_REAL_INFINITY;
GUINT i;
for(i=0;i<3;i++)
{
SEGMENT_COLLISION(s1,s2,triangle->m_vertices[i],triangle->m_vertices[(i+1)%3],segment_points[0],segment_points[1]);
VEC_DIFF(sdiff,segment_points[0],segment_points[1]);
dis_temp = VEC_DOT(sdiff,sdiff);
if(dis_temp< dis)
{
dis = dis_temp;
out_edge = i;
VEC_COPY(closest_points[0],segment_points[0]);
VEC_COPY(closest_points[1],sdiff);//normal
}
}
if(out_edge>2) return ;// ???? ASSERT this please
if(IS_ZERO(dis))
{
//Set face plane
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,0.0f,0, 0, 0,0);
}
else
{
GIM_SQRT(dis,dis);
VEC_SCALE(closest_points[1],(1.0f/dis),closest_points[1]);//normal
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,closest_points[1],dis,0, 0, 0,0);
}
}
// capsule - trimesh By francisco leon
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dTriMeshClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)o1;
dxGeom* gCylinder = o2;
//Get capsule params
dMatrix3 mCapsuleRotation;
dVector3 vCapsulePosition;
dVector3 vCapsuleAxis;
dReal vCapsuleRadius;
dReal fCapsuleSize;
dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
memcpy(vCapsulePosition,pDst,sizeof(dVector3));
//Axis
vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
// Get size of CCylinder
dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
fCapsuleSize*=0.5f;
//Set Capsule params
GIM_CAPSULE_DATA capsule;
capsule.m_radius = vCapsuleRadius;
VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
//Collide trimeshe vs capsule
gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
if(trimeshcontacts.m_size == 0)
{
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return 0;
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
unsigned contactcount = trimeshcontacts.m_size;
unsigned contactmax = (unsigned)(flags & NUMC_MASK);
if (contactcount > contactmax)
{
contactcount = contactmax;
}
dContactGeom* pcontact;
unsigned i;
for (i=0;i<contactcount;i++)
{
pcontact = SAFECONTACT(flags, contact, i, skip);
pcontact->pos[0] = ptrimeshcontacts->m_point[0];
pcontact->pos[1] = ptrimeshcontacts->m_point[1];
pcontact->pos[2] = ptrimeshcontacts->m_point[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
pcontact->normal[3] = 0;
pcontact->depth = ptrimeshcontacts->m_depth;
pcontact->g1 = TriMesh;
pcontact->g2 = gCylinder;
pcontact->side1 = ptrimeshcontacts->m_feature1;
pcontact->side2 = -1;
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return (int)contactcount;
}
int gim_triangle_sphere_collision(
GIM_TRIANGLE_DATA *tri,
vec3f center, GREAL radius,
GIM_TRIANGLE_CONTACT_DATA * contact_data)
{
contact_data->m_point_count = 0;
//Find Face plane distance
GREAL dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[0],center);
if(dis>radius) return 0; //out
if(dis<-radius) return 0;//Out of triangle
contact_data->m_penetration_depth = dis;
//Find the most edge
GUINT32 most_edge = 4;//no edge
GREAL max_dis = 0.0f;
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[1],center);
if(dis>radius) return 0;//Out of triangle
if(dis>0.0f)
{
max_dis = dis;
most_edge = 0;
}
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[2],center);
if(dis>radius) return 0;//Out of triangle
if(dis>max_dis)// && dis>0.0f)
{
max_dis = dis;
most_edge = 1;
}
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[3],center);
if(dis>radius) return 0;//Out of triangle
if(dis>max_dis)// && dis>0.0f)
{
max_dis = dis;
most_edge = 2;
}
if(most_edge == 4) //Box is into triangle
{
//contact_data->m_penetration_depth = dis is set above
//Find Face plane point
VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[0]);
//Find point projection on plane
if(contact_data->m_penetration_depth>=0.0f)
{
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
}
else
{
VEC_SCALE(contact_data->m_points[0],radius,contact_data->m_separating_normal);
}
contact_data->m_penetration_depth = radius - contact_data->m_penetration_depth;
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
//Scale normal for pointing to triangle
VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
contact_data->m_point_count = 1;
return 1;
}
//find the edge
vec3f e1,e2;
VEC_COPY(e1,tri->m_vertices[most_edge]);
VEC_COPY(e2,tri->m_vertices[(most_edge+1)%3]);
CLOSEST_POINT_ON_SEGMENT(contact_data->m_points[0],center,e1,e2);
//find distance
VEC_DIFF(e1,center,contact_data->m_points[0]);
VEC_LENGTH(e1,dis);
if(dis>radius) return 0;
contact_data->m_penetration_depth = radius - dis;
if(IS_ZERO(dis))
{
VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[most_edge+1]);
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
}
else
{
VEC_SCALE(contact_data->m_separating_normal,1.0f/dis,e1);
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
}
//Scale normal for pointing to triangle
VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
contact_data->m_point_count = 1;
return 1;
}
void gleLathe (int ncp, /* number of contour points */
gleDouble contour[][2], /* 2D contour */
gleDouble cont_normal[][2], /* 2D contour normals */
gleDouble up[3], /* up vector for contour */
gleDouble startRadius,
gleDouble drdTheta, /* change in radius per revolution */
gleDouble startZ,
gleDouble dzdTheta, /* change in Z per revolution */
gleDouble startXform[2][3],
gleDouble dXformdTheta[2][3], /* tangent change xform per revln */
gleDouble startTheta, /* start angle, in degrees */
gleDouble sweepTheta) /* sweep angle, in degrees */
{
gleDouble localup[3];
gleDouble len;
gleDouble trans[2];
gleDouble start[2][3], delt[2][3];
/* Because the spiral always starts on the axis, and proceeds in the
* positive y direction, we can see that valid up-vectors must lie
* in the x-z plane. Therefore, we make sure we have a valid up
* vector by projecting it onto the x-z plane, and normalizing. */
if (up[1] != 0.0) {
localup[0] = up[0];
localup[1] = 0.0;
localup[2] = up[2];
VEC_LENGTH (len, localup);
if (len != 0.0) {
len = 1.0/len;
localup[0] *= len;
localup[2] *= len;
VEC_SCALE (localup, len, localup);
} else {
/* invalid up vector was passed in */
localup[0] = 0.0;
localup[2] = 1.0;
}
} else {
VEC_COPY (localup, up);
}
/* the dzdtheta derivative and the drdtheta derivative form a vector
* in the x-z plane. dzdtheta is the z component, and drdtheta is
* the x component. We need to convert this vector into the local
* coordinate system defined by the up vector. We do this by
* applying a 2D rotation matrix.
*/
trans[0] = localup[2] * drdTheta - localup[0] * dzdTheta;
trans[1] = localup[0] * drdTheta + localup[2] * dzdTheta;
/* now, add this translation vector into the affine xform */
if (startXform != NULL) {
if (dXformdTheta != NULL) {
COPY_MATRIX_2X3 (delt, dXformdTheta);
delt[0][2] += trans[0];
delt[1][2] += trans[1];
} else {
/*Hmm- the transforms don't exist */
delt[0][0] = 0.0;
delt[0][1] = 0.0;
delt[0][2] = trans[0];
delt[1][0] = 0.0;
delt[1][1] = 0.0;
delt[1][2] = trans[1];
}
gleSpiral (ncp, contour, cont_normal, up,
startRadius, 0.0, startZ, 0.0,
startXform, delt,
startTheta, sweepTheta);
} else {
/* Hmm- the transforms don't exist */
start[0][0] = 1.0;
start[0][1] = 0.0;
start[0][2] = 0.0;
start[1][0] = 0.0;
start[1][1] = 1.0;
start[1][2] = 0.0;
delt[0][0] = 0.0;
delt[0][1] = 0.0;
delt[0][2] = trans[0];
delt[1][0] = 0.0;
delt[1][1] = 0.0;
delt[1][2] = trans[1];
gleSpiral (ncp, contour, cont_normal, up,
startRadius, 0.0, startZ, 0.0,
start, delt,
startTheta, sweepTheta);
}
}
