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

   1
   2 /*
   3 -----------------------------------------------------------------------------
   4 This source file is part of GIMPACT Library.
   5
   6 For the latest info, see http://gimpact.sourceforge.net/
   7
   8 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
   9 email: projectileman@yahoo.com
  10
  11  This library is free software; you can redistribute it and/or
  12  modify it under the terms of EITHER:
  13    (1) The GNU Lesser General Public License as published by the Free
  14        Software Foundation; either version 2.1 of the License, or (at
  15        your option) any later version. The text of the GNU Lesser
  16        General Public License is included with this library in the
  17        file GIMPACT-LICENSE-LGPL.TXT.
  18    (2) The BSD-style license that is included with this library in
  19        the file GIMPACT-LICENSE-BSD.TXT.
  20    (3) The zlib/libpng license that is included with this library in
  21        the file GIMPACT-LICENSE-ZLIB.TXT.
  22
  23  This library is distributed in the hope that it will be useful,
  24  but WITHOUT ANY WARRANTY; without even the implied warranty of
  25  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
  26  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
  27
  28 -----------------------------------------------------------------------------
  29 */
  30
  31 #include "gim_contact.h"
  32
  33 #define MAX_COINCIDENT 8
  34
  35 void gim_contact_array::merge_contacts(
  36         const gim_contact_array& contacts, bool normal_contact_average)
  37 {
  38         clear();
  39
  40         if (contacts.size() == 1)
  41         {
  42                 push_back(contacts.back());
  43                 return;
  44         }
  45
  46         gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
  47         keycontacts.resize(contacts.size(), false);
  48
  49         //fill key contacts
  50
  51         GUINT i;
  52
  53         for (i = 0; i < contacts.size(); i++)
  54         {
  55                 keycontacts[i].m_key = contacts[i].calc_key_contact();
  56                 keycontacts[i].m_value = i;
  57         }
  58
  59         //sort keys
  60         gim_heap_sort(keycontacts.pointer(), keycontacts.size(), GIM_RSORT_TOKEN_COMPARATOR());
  61
  62         // Merge contacts
  63
  64         GUINT coincident_count = 0;
  65         btVector3 coincident_normals[MAX_COINCIDENT];
  66
  67         GUINT last_key = keycontacts[0].m_key;
  68         GUINT key = 0;
  69
  70         push_back(contacts[keycontacts[0].m_value]);
  71         GIM_CONTACT* pcontact = &back();
  72
  73         for (i = 1; i < keycontacts.size(); i++)
  74         {
  75                 key = keycontacts[i].m_key;
  76                 const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value];
  77
  78                 if (last_key == key)  //same points
  79                 {
  80                         //merge contact
  81                         if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)  //)
  82                         {
  83                                 *pcontact = *scontact;
  84                                 coincident_count = 0;
  85                         }
  86                         else if (normal_contact_average)
  87                         {
  88                                 if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON)
  89                                 {
  90                                         if (coincident_count < MAX_COINCIDENT)
  91                                         {
  92                                                 coincident_normals[coincident_count] = scontact->m_normal;
  93                                                 coincident_count++;
  94                                         }
  95                                 }
  96                         }
  97                 }
  98                 else
  99                 {  //add new contact
 100
 101                         if (normal_contact_average && coincident_count > 0)
 102                         {
 103                                 pcontact->interpolate_normals(coincident_normals, coincident_count);
 104                                 coincident_count = 0;
 105                         }
 106
 107                         push_back(*scontact);
 108                         pcontact = &back();
 109                 }
 110                 last_key = key;
 111         }
 112 }
 113
 114 void gim_contact_array::merge_contacts_unique(const gim_contact_array& contacts)
 115 {
 116         clear();
 117
 118         if (contacts.size() == 1)
 119         {
 120                 push_back(contacts.back());
 121                 return;
 122         }
 123
 124         GIM_CONTACT average_contact = contacts.back();
 125
 126         for (GUINT i = 1; i < contacts.size(); i++)
 127         {
 128                 average_contact.m_point += contacts[i].m_point;
 129                 average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
 130         }
 131
 132         //divide
 133         GREAL divide_average = 1.0f / ((GREAL)contacts.size());
 134
 135         average_contact.m_point *= divide_average;
 136
 137         average_contact.m_normal *= divide_average;
 138
 139         average_contact.m_depth = average_contact.m_normal.length();
 140
 141         average_contact.m_normal /= average_contact.m_depth;
 142 }