/*!
\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);
}
void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
GDYNAMIC_ARRAY * dest_contacts)
{
dest_contacts->m_size = 0;
//Traverse the source contacts
GUINT32 source_count = source_contacts->m_size;
if(source_count==0) return;
GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
//add the unique contact
GIM_CONTACT * pcontact = 0;
GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
//set the first contact
GIM_COPY_CONTACTS(pcontact, psource_contacts);
if(source_count==1) return;
//scale the first contact
VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
psource_contacts++;
//Average the contacts
GUINT32 i;
for(i=1;i<source_count;i++)
{
VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
psource_contacts++;
}
GREAL divide_average = 1.0f/((GREAL)source_count);
VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
VEC_NORMALIZE(pcontact->m_normal);
/*GREAL normal_len;
VEC_INV_LENGTH(pcontact->m_normal,normal_len);
VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
//Deep = LEN(normal)/SQRT(source_count)
GIM_SQRT(divide_average,divide_average);
pcontact->m_depth = divide_average/normal_len;
*/
}
void gim_merge_contacts(GDYNAMIC_ARRAY * source_contacts,
GDYNAMIC_ARRAY * dest_contacts)
{
dest_contacts->m_size = 0;
GUINT32 source_count = source_contacts->m_size;
GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
//create keys
GIM_RSORT_TOKEN * keycontacts = (GIM_RSORT_TOKEN * )gim_alloc(sizeof(GIM_RSORT_TOKEN)*source_count);
GUINT32 i;
for(i=0;i<source_count;i++)
{
keycontacts[i].m_value = i;
GIM_CALC_KEY_CONTACT(psource_contacts[i].m_point,keycontacts[i].m_key);
}
//sort keys
GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , keycontacts, source_count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
// Merge contacts
GIM_CONTACT * pcontact = 0;
GIM_CONTACT * scontact = 0;
GUINT32 key,last_key=0;
for(i=0;i<source_contacts->m_size;i++)
{
key = keycontacts[i].m_key;
scontact = &psource_contacts[keycontacts[i].m_value];
if(i>0 && last_key == key)
{
//merge contact
if(pcontact->m_depth > scontact->m_depth + CONTACT_DIFF_EPSILON)
{
GIM_COPY_CONTACTS(pcontact, scontact);
}
}
else
{//add new contact
GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
GIM_COPY_CONTACTS(pcontact, scontact);
}
last_key = key;
}
gim_free(keycontacts,0);
}