Home | History | Annotate | Download | only in Gimpact 
      1 
      2 /*
      3 This source file is part of GIMPACT Library.
      4 
      5 For the latest info, see http://gimpact.sourceforge.net/
      6 
      7 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
      8 email: projectileman (at) yahoo.com
      9 
     10 
     11 This software is provided 'as-is', without any express or implied warranty.
     12 In no event will the authors be held liable for any damages arising from the use of this software.
     13 Permission is granted to anyone to use this software for any purpose,
     14 including commercial applications, and to alter it and redistribute it freely,
     15 subject to the following restrictions:
     16 
     17 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.
     18 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
     19 3. This notice may not be removed or altered from any source distribution.
     20 */
     21 #include "btContactProcessing.h"
     22 
     23 #define MAX_COINCIDENT 8
     24 
     25 struct CONTACT_KEY_TOKEN
     26 {
     27 	unsigned int m_key;
     28 	int m_value;
     29 	CONTACT_KEY_TOKEN()
     30     {
     31     }
     32 
     33     CONTACT_KEY_TOKEN(unsigned int key,int token)
     34     {
     35     	m_key = key;
     36     	m_value =  token;
     37     }
     38 
     39     CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
     40     {
     41     	m_key = rtoken.m_key;
     42     	m_value = rtoken.m_value;
     43     }
     44 
     45     inline bool operator <(const CONTACT_KEY_TOKEN& other) const
     46 	{
     47 		return (m_key < other.m_key);
     48 	}
     49 
     50 	inline bool operator >(const CONTACT_KEY_TOKEN& other) const
     51 	{
     52 		return (m_key > other.m_key);
     53 	}
     54 
     55 };
     56 
     57 class CONTACT_KEY_TOKEN_COMP
     58 {
     59 	public:
     60 
     61 		bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const
     62 		{
     63 			return ( a < b );
     64 		}
     65 };
     66 
     67 
     68 void btContactArray::merge_contacts(
     69 	const btContactArray & contacts, bool normal_contact_average)
     70 {
     71 	clear();
     72 
     73 	int i;
     74 	if(contacts.size()==0) return;
     75 
     76 
     77 	if(contacts.size()==1)
     78 	{
     79 		push_back(contacts[0]);
     80 		return;
     81 	}
     82 
     83 	btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;
     84 
     85 	keycontacts.reserve(contacts.size());
     86 
     87 	//fill key contacts
     88 
     89 	for ( i = 0;i<contacts.size() ;i++ )
     90 	{
     91 		keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
     92 	}
     93 
     94 	//sort keys
     95 	keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
     96 
     97 	// Merge contacts
     98 	int coincident_count=0;
     99 	btVector3 coincident_normals[MAX_COINCIDENT];
    100 
    101 	unsigned int last_key = keycontacts[0].m_key;
    102 	unsigned int key = 0;
    103 
    104 	push_back(contacts[keycontacts[0].m_value]);
    105 
    106 	GIM_CONTACT * pcontact = &(*this)[0];
    107 
    108 	for( i=1;i<keycontacts.size();i++)
    109 	{
    110 	    key = keycontacts[i].m_key;
    111 		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
    112 
    113 		if(last_key ==  key)//same points
    114 		{
    115 			//merge contact
    116 			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
    117 			{
    118 				*pcontact = *scontact;
    119                 coincident_count = 0;
    120 			}
    121 			else if(normal_contact_average)
    122 			{
    123 				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
    124                 {
    125                     if(coincident_count<MAX_COINCIDENT)
    126                     {
    127                     	coincident_normals[coincident_count] = scontact->m_normal;
    128                         coincident_count++;
    129                     }
    130                 }
    131 			}
    132 		}
    133 		else
    134 		{//add new contact
    135 
    136 		    if(normal_contact_average && coincident_count>0)
    137 		    {
    138 		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
    139 		        coincident_count = 0;
    140 		    }
    141 
    142 		    push_back(*scontact);
    143 		    pcontact = &(*this)[this->size()-1];
    144         }
    145 		last_key = key;
    146 	}
    147 }
    148 
    149 void btContactArray::merge_contacts_unique(const btContactArray & contacts)
    150 {
    151 	clear();
    152 
    153 	if(contacts.size()==0) return;
    154 
    155 	if(contacts.size()==1)
    156 	{
    157 		push_back(contacts[0]);
    158 		return;
    159 	}
    160 
    161 	GIM_CONTACT average_contact = contacts[0];
    162 
    163 	for (int i=1;i<contacts.size() ;i++ )
    164 	{
    165 		average_contact.m_point += contacts[i].m_point;
    166 		average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
    167 	}
    168 
    169 	//divide
    170 	btScalar divide_average = 1.0f/((btScalar)contacts.size());
    171 
    172 	average_contact.m_point *= divide_average;
    173 
    174 	average_contact.m_normal *= divide_average;
    175 
    176 	average_contact.m_depth = average_contact.m_normal.length();
    177 
    178 	average_contact.m_normal /= average_contact.m_depth;
    179 
    180 }
    181 
    182