/*! \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); } }