void gim_trimesh_get_triangle_data(GIM_TRIMESH * trimesh, GUINT32 triangle_index, GIM_TRIANGLE_DATA * tri_data)
{
vec3f * transformed_vertices = GIM_BUFFER_ARRAY_POINTER(vec3f,trimesh->m_transformed_vertex_buffer,0);
GUINT32 * triangle_indices = GIM_BUFFER_ARRAY_POINTER(GUINT32,trimesh->m_tri_index_buffer,triangle_index*3);
//Copy the vertices
VEC_COPY(tri_data->m_vertices[0],transformed_vertices[triangle_indices[0]]);
VEC_COPY(tri_data->m_vertices[1],transformed_vertices[triangle_indices[1]]);
VEC_COPY(tri_data->m_vertices[2],transformed_vertices[triangle_indices[2]]);
//Get the planes
GIM_TRIPLANES_CACHE * planes = GIM_DYNARRAY_POINTER(GIM_TRIPLANES_CACHE,trimesh->m_planes_cache_buffer);
planes += triangle_index;
//verify planes cache
GUINT32 bit_eval;
GIM_BITSET_GET(trimesh->m_planes_cache_bitset,triangle_index,bit_eval);
if(bit_eval == 0)// Needs to calc the planes
{
//Calc the face plane
TRIANGLE_PLANE(tri_data->m_vertices[0],tri_data->m_vertices[1],tri_data->m_vertices[2],planes->m_planes[0]);
//Calc the edge 1
EDGE_PLANE(tri_data->m_vertices[0],tri_data->m_vertices[1],(planes->m_planes[0]),(planes->m_planes[1]));
//Calc the edge 2
EDGE_PLANE(tri_data->m_vertices[1],tri_data->m_vertices[2],(planes->m_planes[0]),(planes->m_planes[2]));
//Calc the edge 3
EDGE_PLANE(tri_data->m_vertices[2],tri_data->m_vertices[0],(planes->m_planes[0]), (planes->m_planes[3]));
//mark
GIM_BITSET_SET(trimesh->m_planes_cache_bitset,triangle_index);
}
VEC_COPY_4((tri_data->m_planes.m_planes[0]),(planes->m_planes[0]));//face plane
VEC_COPY_4((tri_data->m_planes.m_planes[1]),(planes->m_planes[1]));//edge1
VEC_COPY_4((tri_data->m_planes.m_planes[2]),(planes->m_planes[2]));//edge2
VEC_COPY_4((tri_data->m_planes.m_planes[3]),(planes->m_planes[3]));//edge3
}
//! 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;
}
