btContactProcessing.cpp

Go to the documentation of this file.
00001 
00002 /*
00003 This source file is part of GIMPACT Library.
00004 
00005 For the latest info, see http://gimpact.sourceforge.net/
00006 
00007 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
00008 email: projectileman@yahoo.com
00009 
00010 
00011 This software is provided 'as-is', without any express or implied warranty.
00012 In no event will the authors be held liable for any damages arising from the use of this software.
00013 Permission is granted to anyone to use this software for any purpose,
00014 including commercial applications, and to alter it and redistribute it freely,
00015 subject to the following restrictions:
00016 
00017 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.
00018 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00019 3. This notice may not be removed or altered from any source distribution.
00020 */
00021 #include "btContactProcessing.h"
00022 
00023 #define MAX_COINCIDENT 8
00024 
00025 struct CONTACT_KEY_TOKEN
00026 {
00027         unsigned int m_key;
00028         int m_value;
00029         CONTACT_KEY_TOKEN()
00030     {
00031     }
00032 
00033     CONTACT_KEY_TOKEN(unsigned int key,int token)
00034     {
00035         m_key = key;
00036         m_value =  token;
00037     }
00038 
00039     CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
00040     {
00041         m_key = rtoken.m_key;
00042         m_value = rtoken.m_value;
00043     }
00044 
00045     inline bool operator <(const CONTACT_KEY_TOKEN& other) const
00046         {
00047                 return (m_key < other.m_key);
00048         }
00049 
00050         inline bool operator >(const CONTACT_KEY_TOKEN& other) const
00051         {
00052                 return (m_key > other.m_key);
00053         }
00054 
00055 };
00056 
00057 class CONTACT_KEY_TOKEN_COMP
00058 {
00059         public:
00060 
00061                 bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const
00062                 {
00063                         return ( a < b );
00064                 }
00065 };
00066 
00067 
00068 void btContactArray::merge_contacts(
00069         const btContactArray & contacts, bool normal_contact_average)
00070 {
00071         clear();
00072 
00073         int i;
00074         if(contacts.size()==0) return;
00075 
00076 
00077         if(contacts.size()==1)
00078         {
00079                 push_back(contacts[0]);
00080                 return;
00081         }
00082 
00083         btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;
00084 
00085         keycontacts.reserve(contacts.size());
00086 
00087         //fill key contacts
00088 
00089         for ( i = 0;i<contacts.size() ;i++ )
00090         {
00091                 keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
00092         }
00093 
00094         //sort keys
00095         keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
00096 
00097         // Merge contacts
00098         int coincident_count=0;
00099         btVector3 coincident_normals[MAX_COINCIDENT];
00100 
00101         unsigned int last_key = keycontacts[0].m_key;
00102         unsigned int key = 0;
00103 
00104         push_back(contacts[keycontacts[0].m_value]);
00105 
00106         GIM_CONTACT * pcontact = &(*this)[0];
00107 
00108         for( i=1;i<keycontacts.size();i++)
00109         {
00110             key = keycontacts[i].m_key;
00111                 const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
00112 
00113                 if(last_key ==  key)//same points
00114                 {
00115                         //merge contact
00116                         if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
00117                         {
00118                                 *pcontact = *scontact;
00119                 coincident_count = 0;
00120                         }
00121                         else if(normal_contact_average)
00122                         {
00123                                 if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
00124                 {
00125                     if(coincident_count<MAX_COINCIDENT)
00126                     {
00127                         coincident_normals[coincident_count] = scontact->m_normal;
00128                         coincident_count++;
00129                     }
00130                 }
00131                         }
00132                 }
00133                 else
00134                 {//add new contact
00135 
00136                     if(normal_contact_average && coincident_count>0)
00137                     {
00138                         pcontact->interpolate_normals(coincident_normals,coincident_count);
00139                         coincident_count = 0;
00140                     }
00141 
00142                     push_back(*scontact);
00143                     pcontact = &(*this)[this->size()-1];
00144         }
00145                 last_key = key;
00146         }
00147 }
00148 
00149 void btContactArray::merge_contacts_unique(const btContactArray & contacts)
00150 {
00151         clear();
00152 
00153         if(contacts.size()==0) return;
00154 
00155         if(contacts.size()==1)
00156         {
00157                 push_back(contacts[0]);
00158                 return;
00159         }
00160 
00161         GIM_CONTACT average_contact = contacts[0];
00162 
00163         for (int i=1;i<contacts.size() ;i++ )
00164         {
00165                 average_contact.m_point += contacts[i].m_point;
00166                 average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
00167         }
00168 
00169         //divide
00170         btScalar divide_average = 1.0f/((btScalar)contacts.size());
00171 
00172         average_contact.m_point *= divide_average;
00173 
00174         average_contact.m_normal *= divide_average;
00175 
00176         average_contact.m_depth = average_contact.m_normal.length();
00177 
00178         average_contact.m_normal /= average_contact.m_depth;
00179 
00180 }
00181