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