| /* | |
| This source file is part of GIMPACT Library. | |
| For the latest info, see http://gimpact.sourceforge.net/ | |
| Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. | |
| email: projectileman@yahoo.com | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #include "BulletCollision/Gimpact/btContactProcessing.h" | |
| #define MAX_COINCIDENT 8 | |
| struct CONTACT_KEY_TOKEN | |
| { | |
| unsigned int m_key; | |
| int m_value; | |
| CONTACT_KEY_TOKEN() | |
| { | |
| } | |
| CONTACT_KEY_TOKEN(unsigned int key,int token) | |
| { | |
| m_key = key; | |
| m_value = token; | |
| } | |
| CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken) | |
| { | |
| m_key = rtoken.m_key; | |
| m_value = rtoken.m_value; | |
| } | |
| inline bool operator <(const CONTACT_KEY_TOKEN& other) const | |
| { | |
| return (m_key < other.m_key); | |
| } | |
| inline bool operator >(const CONTACT_KEY_TOKEN& other) const | |
| { | |
| return (m_key > other.m_key); | |
| } | |
| }; | |
| class CONTACT_KEY_TOKEN_COMP | |
| { | |
| public: | |
| bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) | |
| { | |
| return ( a < b ); | |
| } | |
| }; | |
| 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; | |
| } | |
| } | |
| void btContactArray::merge_contacts_unique(const btContactArray & contacts) | |
| { | |
| clear(); | |
| if(contacts.size()==0) return; | |
| if(contacts.size()==1) | |
| { | |
| push_back(contacts[0]); | |
| return; | |
| } | |
| GIM_CONTACT average_contact = contacts[0]; | |
| for (int i=1;i<contacts.size() ;i++ ) | |
| { | |
| average_contact.m_point += contacts[i].m_point; | |
| average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth; | |
| } | |
| //divide | |
| btScalar divide_average = 1.0f/((btScalar)contacts.size()); | |
| average_contact.m_point *= divide_average; | |
| average_contact.m_normal *= divide_average; | |
| average_contact.m_depth = average_contact.m_normal.length(); | |
| average_contact.m_normal /= average_contact.m_depth; | |
| } | |