/*!
\pre tv_plane and tu_plane must be set
\post
distances[2] is set with the distance
closest_point_u, closest_point_v, edge_edge_dir are set too
\return
- 0: faces are paralele
- 1: face U casts face V
- 2: face V casts face U
- 3: nearest edges
*/
SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
{
// Compute direction of intersection line
edge_edge_dir = tu_plane.cross(tv_plane);
GREAL Dlen;
VEC_LENGTH(edge_edge_dir,Dlen);
if(Dlen<0.0001)
{
return 0; //faces near paralele
}
edge_edge_dir*= 1/Dlen;//normalize
// Compute interval for triangle 1
GUINT tu_e0,tu_e1;//edge indices
GREAL tu_scale_e0,tu_scale_e1;//edge scale
if(!compute_intervals(du[0],du[1],du[2],
du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
// Compute interval for triangle 2
GUINT tv_e0,tv_e1;//edge indices
GREAL tv_scale_e0,tv_scale_e1;//edge scale
if(!compute_intervals(dv[0],dv[1],dv[2],
dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
//proyected vertices
btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
//proyected intervals
GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
if(midpoint_u<midpoint_v)
{
if(isect_u[1]>=isect_v[1]) // face U casts face V
{
return 1;
}
else if(isect_v[0]<=isect_u[0]) // face V casts face U
{
return 2;
}
// closest points
closest_point_u = up_e1;
closest_point_v = vp_e0;
// calc edges and separation
if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
{
SEGMENT_COLLISION(
tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
closest_point_u,
closest_point_v);
edge_edge_dir = closest_point_u-closest_point_v;
VEC_LENGTH(edge_edge_dir,distances[2]);
edge_edge_dir *= 1.0f/distances[2];// normalize
}
else
{
distances[2] = isect_v[0]-isect_u[1];//distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
}
}
else
{
if(isect_v[1]>=isect_u[1]) // face V casts face U
{
return 2;
}
else if(isect_u[0]<=isect_v[0]) // face U casts face V
{
return 1;
}
// closest points
closest_point_u = up_e0;
closest_point_v = vp_e1;
// calc edges and separation
if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
{
SEGMENT_COLLISION(
tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
closest_point_u,
closest_point_v);
edge_edge_dir = closest_point_u-closest_point_v;
VEC_LENGTH(edge_edge_dir,distances[2]);
edge_edge_dir *= 1.0f/distances[2];// normalize
}
else
{
distances[2] = isect_u[0]-isect_v[1];//distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
}
}
return 3;
}
/*!
\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);
}
}
