/*! \param trimesh \param plane vec4f plane \param contacts A vec4f array. Must be initialized (~100). Each element have the coordinate point in the first 3 elements, and vec4f[3] has the penetration depth. */ void gim_trimesh_plane_collision(GIM_TRIMESH * trimesh,vec4f plane, GDYNAMIC_ARRAY * contacts) { contacts->m_size = 0; char classify; PLANE_CLASSIFY_BOX(plane,trimesh->m_aabbset.m_global_bound,classify); if(classify>1) return; // in front of plane //Locks mesh gim_trimesh_locks_work_data(trimesh); //Get vertices GUINT32 i, vertcount = trimesh->m_transformed_vertex_buffer.m_element_count; vec3f * vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0); GREAL dist; vec4f * result_contact; for (i=0; i<vertcount; i++) { dist = DISTANCE_PLANE_POINT(plane,vertices[i]); if(dist<=0.0f) { GIM_DYNARRAY_PUSH_EMPTY(vec4f,(*contacts)); result_contact = GIM_DYNARRAY_POINTER_LAST(vec4f,(*contacts)); VEC_COPY((*result_contact),vertices[i]); (*result_contact)[3] = -dist; } } gim_trimesh_unlocks_work_data(trimesh); }
//! collides by two sides SIMD_FORCE_INLINE bool triangle_collision( const btVector3 & u0, const btVector3 & u1, const btVector3 & u2, GREAL margin_u, const btVector3 & v0, const btVector3 & v1, const btVector3 & v2, GREAL margin_v, GIM_TRIANGLE_CONTACT_DATA & contacts) { margin = margin_u + margin_v; tu_vertices[0] = u0; tu_vertices[1] = u1; tu_vertices[2] = u2; tv_vertices[0] = v0; tv_vertices[1] = v1; tv_vertices[2] = v2; //create planes // plane v vs U points TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane); du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]); du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]); du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]); du0du1 = du[0] * du[1]; du0du2 = du[0] * du[2]; if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? { if(du[0]<0) //we need test behind the triangle plane { distances[0] = GIM_MAX3(du[0],du[1],du[2]); distances[0] = -distances[0]; if(distances[0]>margin) return false; //never intersect //reorder triangle v VEC_SWAP(tv_vertices[0],tv_vertices[1]); VEC_SCALE_4(tv_plane,-1.0f,tv_plane); } else { distances[0] = GIM_MIN3(du[0],du[1],du[2]); if(distances[0]>margin) return false; //never intersect } } else { //Look if we need to invert the triangle distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid if(distances[0]<0.0f) { //reorder triangle v VEC_SWAP(tv_vertices[0],tv_vertices[1]); VEC_SCALE_4(tv_plane,-1.0f,tv_plane); distances[0] = GIM_MAX3(du[0],du[1],du[2]); distances[0] = -distances[0]; } else { distances[0] = GIM_MIN3(du[0],du[1],du[2]); } } // plane U vs V points TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane); dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]); dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]); dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]); dv0dv1 = dv[0] * dv[1]; dv0dv2 = dv[0] * dv[2]; if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? { if(dv[0]<0) //we need test behind the triangle plane { distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); distances[1] = -distances[1]; if(distances[1]>margin) return false; //never intersect //reorder triangle u VEC_SWAP(tu_vertices[0],tu_vertices[1]); VEC_SCALE_4(tu_plane,-1.0f,tu_plane); } else { distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); if(distances[1]>margin) return false; //never intersect } } else { //Look if we need to invert the triangle distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid if(distances[1]<0.0f) { //reorder triangle v VEC_SWAP(tu_vertices[0],tu_vertices[1]); VEC_SCALE_4(tu_plane,-1.0f,tu_plane); distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); distances[1] = -distances[1]; } else { distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); } } GUINT bl; /* bl = cross_line_intersection_test(); if(bl==3) { //take edge direction too bl = distances.maxAxis(); } else {*/ bl = 0; if(distances[0]<distances[1]) bl = 1; //} if(bl==2) //edge edge separation { if(distances[2]>margin) return false; contacts.m_penetration_depth = -distances[2] + margin; contacts.m_points[0] = closest_point_v; contacts.m_point_count = 1; VEC_COPY(contacts.m_separating_normal,edge_edge_dir); return true; } //clip face against other GUINT point_count; //TODO if(bl == 0) //clip U points against V { point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points); if(point_count == 0) return false; contacts.merge_points(tv_plane,margin,contact_points,point_count); } else //clip V points against U { point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points); if(point_count == 0) return false; contacts.merge_points(tu_plane,margin,contact_points,point_count); contacts.m_separating_normal *= -1.f; } if(contacts.m_point_count == 0) return false; return true; }
/*! \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); } }
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; }