gim_contact.cpp

Go to the documentation of this file.
00001 
00002 /*
00003 -----------------------------------------------------------------------------
00004 This source file is part of GIMPACT Library.
00005 
00006 For the latest info, see http://gimpact.sourceforge.net/
00007 
00008 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
00009 email: projectileman@yahoo.com
00010 
00011  This library is free software; you can redistribute it and/or
00012  modify it under the terms of EITHER:
00013    (1) The GNU Lesser General Public License as published by the Free
00014        Software Foundation; either version 2.1 of the License, or (at
00015        your option) any later version. The text of the GNU Lesser
00016        General Public License is included with this library in the
00017        file GIMPACT-LICENSE-LGPL.TXT.
00018    (2) The BSD-style license that is included with this library in
00019        the file GIMPACT-LICENSE-BSD.TXT.
00020    (3) The zlib/libpng license that is included with this library in
00021        the file GIMPACT-LICENSE-ZLIB.TXT.
00022 
00023  This library is distributed in the hope that it will be useful,
00024  but WITHOUT ANY WARRANTY; without even the implied warranty of
00025  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
00026  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
00027 
00028 -----------------------------------------------------------------------------
00029 */
00030 
00031 #include "gim_contact.h"
00032 
00033 #define MAX_COINCIDENT 8
00034 
00035 void gim_contact_array::merge_contacts(
00036         const gim_contact_array & contacts, bool normal_contact_average)
00037 {
00038         clear();
00039 
00040         if(contacts.size()==1)
00041         {
00042                 push_back(contacts.back());
00043                 return;
00044         }
00045 
00046         gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
00047         keycontacts.resize(contacts.size(),false);
00048 
00049         //fill key contacts
00050 
00051         GUINT i;
00052 
00053         for (i = 0;i<contacts.size() ;i++ )
00054         {
00055                 keycontacts[i].m_key = contacts[i].calc_key_contact();
00056                 keycontacts[i].m_value = i;
00057         }
00058 
00059         //sort keys
00060         gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
00061 
00062         // Merge contacts
00063 
00064         GUINT coincident_count=0;
00065         btVector3 coincident_normals[MAX_COINCIDENT];
00066 
00067         GUINT last_key = keycontacts[0].m_key;
00068         GUINT key = 0;
00069 
00070         push_back(contacts[keycontacts[0].m_value]);
00071         GIM_CONTACT * pcontact = &back();
00072 
00073 
00074 
00075         for( i=1;i<keycontacts.size();i++)
00076         {
00077             key = keycontacts[i].m_key;
00078                 const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
00079 
00080                 if(last_key ==  key)//same points
00081                 {
00082                         //merge contact
00083                         if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
00084                         {
00085                                 *pcontact = *scontact;
00086                 coincident_count = 0;
00087                         }
00088                         else if(normal_contact_average)
00089                         {
00090                                 if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
00091                 {
00092                     if(coincident_count<MAX_COINCIDENT)
00093                     {
00094                         coincident_normals[coincident_count] = scontact->m_normal;
00095                         coincident_count++;
00096                     }
00097                 }
00098                         }
00099                 }
00100                 else
00101                 {//add new contact
00102 
00103                     if(normal_contact_average && coincident_count>0)
00104                     {
00105                         pcontact->interpolate_normals(coincident_normals,coincident_count);
00106                         coincident_count = 0;
00107                     }
00108 
00109                     push_back(*scontact);
00110                     pcontact = &back();
00111         }
00112                 last_key = key;
00113         }
00114 }
00115 
00116 void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
00117 {
00118         clear();
00119 
00120         if(contacts.size()==1)
00121         {
00122                 push_back(contacts.back());
00123                 return;
00124         }
00125 
00126         GIM_CONTACT average_contact = contacts.back();
00127 
00128         for (GUINT i=1;i<contacts.size() ;i++ )
00129         {
00130                 average_contact.m_point += contacts[i].m_point;
00131                 average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
00132         }
00133 
00134         //divide
00135         GREAL divide_average = 1.0f/((GREAL)contacts.size());
00136 
00137         average_contact.m_point *= divide_average;
00138 
00139         average_contact.m_normal *= divide_average;
00140 
00141         average_contact.m_depth = average_contact.m_normal.length();
00142 
00143         average_contact.m_normal /= average_contact.m_depth;
00144 
00145 }
00146