== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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
74
75         for( i=1;i<keycontacts.size();i++)
76         {
77             key = keycontacts[i].m_key;
78                 const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
79
80                 if(last_key ==  key)//same points
81                 {
82                         //merge contact
83                         if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
84                         {
85                                 *pcontact = *scontact;
86                 coincident_count = 0;
87                         }
88                         else if(normal_contact_average)
89                         {
90                                 if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
91                 {
92                     if(coincident_count<MAX_COINCIDENT)
93                     {
94                         coincident_normals[coincident_count] = scontact->m_normal;
95                         coincident_count++;
96                     }
97                 }
98                         }
99                 }
100                 else
101                 {//add new contact
102
103                     if(normal_contact_average && coincident_count>0)
104                     {
105                         pcontact->interpolate_normals(coincident_normals,coincident_count);
106                         coincident_count = 0;
107                     }
108
109                     push_back(*scontact);
110                     pcontact = &back();
111         }
112                 last_key = key;
113         }
114 }
115
116 void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
117 {
118         clear();
119
120         if(contacts.size()==1)
121         {
122                 push_back(contacts.back());
123                 return;
124         }
125
126         GIM_CONTACT average_contact = contacts.back();
127
128         for (GUINT i=1;i<contacts.size() ;i++ )
129         {
130                 average_contact.m_point += contacts[i].m_point;
131                 average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
132         }
133
134         //divide
135         GREAL divide_average = 1.0f/((GREAL)contacts.size());
136
137         average_contact.m_point *= divide_average;
138
139         average_contact.m_normal *= divide_average;
140
141         average_contact.m_depth = average_contact.m_normal.length();
142
143         average_contact.m_normal /= average_contact.m_depth;
144
145 }
146