dali-physics/third-party/bullet3/src/BulletCollision/Gimpact/btContactProcessing.cpp

   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@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 class CONTACT_KEY_TOKEN_COMP
  57 {
  58 public:
  59         bool operator()(const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b) const
  60         {
  61                 return (a < b);
  62         }
  63 };
  64
  65 void btContactArray::merge_contacts(
  66         const btContactArray& contacts, bool normal_contact_average)
  67 {
  68         clear();
  69
  70         int i;
  71         if (contacts.size() == 0) return;
  72
  73         if (contacts.size() == 1)
  74         {
  75                 push_back(contacts[0]);
  76                 return;
  77         }
  78
  79         btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;
  80
  81         keycontacts.reserve(contacts.size());
  82
  83         //fill key contacts
  84
  85         for (i = 0; i < contacts.size(); i++)
  86         {
  87                 keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(), i));
  88         }
  89
  90         //sort keys
  91         keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
  92
  93         // Merge contacts
  94         int coincident_count = 0;
  95         btVector3 coincident_normals[MAX_COINCIDENT];
  96
  97         unsigned int last_key = keycontacts[0].m_key;
  98         unsigned int key = 0;
  99
 100         push_back(contacts[keycontacts[0].m_value]);
 101
 102         GIM_CONTACT* pcontact = &(*this)[0];
 103
 104         for (i = 1; i < keycontacts.size(); i++)
 105         {
 106                 key = keycontacts[i].m_key;
 107                 const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value];
 108
 109                 if (last_key == key)  //same points
 110                 {
 111                         //merge contact
 112                         if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)  //)
 113                         {
 114                                 *pcontact = *scontact;
 115                                 coincident_count = 0;
 116                         }
 117                         else if (normal_contact_average)
 118                         {
 119                                 if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON)
 120                                 {
 121                                         if (coincident_count < MAX_COINCIDENT)
 122                                         {
 123                                                 coincident_normals[coincident_count] = scontact->m_normal;
 124                                                 coincident_count++;
 125                                         }
 126                                 }
 127                         }
 128                 }
 129                 else
 130                 {  //add new contact
 131
 132                         if (normal_contact_average && coincident_count > 0)
 133                         {
 134                                 pcontact->interpolate_normals(coincident_normals, coincident_count);
 135                                 coincident_count = 0;
 136                         }
 137
 138                         push_back(*scontact);
 139                         pcontact = &(*this)[this->size() - 1];
 140                 }
 141                 last_key = key;
 142         }
 143 }
 144
 145 void btContactArray::merge_contacts_unique(const btContactArray& contacts)
 146 {
 147         clear();
 148
 149         if (contacts.size() == 0) return;
 150
 151         if (contacts.size() == 1)
 152         {
 153                 push_back(contacts[0]);
 154                 return;
 155         }
 156
 157         GIM_CONTACT average_contact = contacts[0];
 158
 159         for (int i = 1; i < contacts.size(); i++)
 160         {
 161                 average_contact.m_point += contacts[i].m_point;
 162                 average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
 163         }
 164
 165         //divide
 166         btScalar divide_average = 1.0f / ((btScalar)contacts.size());
 167
 168         average_contact.m_point *= divide_average;
 169
 170         average_contact.m_normal *= divide_average;
 171
 172         average_contact.m_depth = average_contact.m_normal.length();
 173
 174         average_contact.m_normal /= average_contact.m_depth;
 175 }