[1963] | 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 | |
---|