void gim_contact_array::merge_contacts( const gim_contact_array & contacts, bool normal_contact_average) { clear(); if(contacts.size()==1) { push_back(contacts.back()); return; } gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size()); keycontacts.resize(contacts.size(),false); //fill key contacts GUINT i; for (i = 0;i<contacts.size() ;i++ ) { keycontacts[i].m_key = contacts[i].calc_key_contact(); keycontacts[i].m_value = i; } //sort keys gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR()); // Merge contacts GUINT coincident_count=0; btVector3 coincident_normals[MAX_COINCIDENT]; GUINT last_key = keycontacts[0].m_key; GUINT key = 0; push_back(contacts[keycontacts[0].m_value]); GIM_CONTACT * pcontact = &back(); for( i=1;i<keycontacts.size();i++) { key = keycontacts[i].m_key; const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; if(last_key == key)//same points { //merge contact if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) { *pcontact = *scontact; coincident_count = 0; } else if(normal_contact_average) { if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON) { if(coincident_count<MAX_COINCIDENT) { coincident_normals[coincident_count] = scontact->m_normal; coincident_count++; } } } } else {//add new contact if(normal_contact_average && coincident_count>0) { pcontact->interpolate_normals(coincident_normals,coincident_count); coincident_count = 0; } push_back(*scontact); pcontact = &back(); } last_key = key; } }
void btContactArray::merge_contacts( const btContactArray & contacts, bool normal_contact_average) { clear(); int i; if(contacts.size()==0) return; if(contacts.size()==1) { push_back(contacts[0]); return; } btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts; keycontacts.reserve(contacts.size()); //fill key contacts for ( i = 0;i<contacts.size() ;i++ ) { keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i)); } //sort keys keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP()); // Merge contacts int coincident_count=0; btVector3 coincident_normals[MAX_COINCIDENT]; unsigned int last_key = keycontacts[0].m_key; unsigned int key = 0; push_back(contacts[keycontacts[0].m_value]); GIM_CONTACT * pcontact = &(*this)[0]; for( i=1;i<keycontacts.size();i++) { key = keycontacts[i].m_key; const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; if(last_key == key)//same points { //merge contact if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) { *pcontact = *scontact; coincident_count = 0; } else if(normal_contact_average) { if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON) { if(coincident_count<MAX_COINCIDENT) { coincident_normals[coincident_count] = scontact->m_normal; coincident_count++; } } } } else {//add new contact if(normal_contact_average && coincident_count>0) { pcontact->interpolate_normals(coincident_normals,coincident_count); coincident_count = 0; } push_back(*scontact); pcontact = &(*this)[this->size()-1]; } last_key = key; } }