GIMPACT/src/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. 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
  21  This library is distributed in the hope that it will be useful,
  22  but WITHOUT ANY WARRANTY; without even the implied warranty of
  23  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
  24  GIMPACT-LICENSE-LGPL.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
  25
  26 -----------------------------------------------------------------------------
  27 */
  28
  29 #include "GIMPACT/gim_contact.h"
  30
  31 void gim_merge_contacts(GDYNAMIC_ARRAY * source_contacts,
  32                                         GDYNAMIC_ARRAY * dest_contacts)
  33 {
  34     dest_contacts->m_size = 0;
  35
  36         GUINT32 source_count = source_contacts->m_size;
  37         GIM_CONTACT * psource_contacts  = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
  38         //create keys
  39         GIM_RSORT_TOKEN * keycontacts = (GIM_RSORT_TOKEN * )gim_alloc(sizeof(GIM_RSORT_TOKEN)*source_count);
  40
  41     GUINT32 i;
  42         for(i=0;i<source_count;i++)
  43         {
  44                 keycontacts[i].m_value = i;
  45                 GIM_CALC_KEY_CONTACT(psource_contacts[i].m_point,keycontacts[i].m_key);
  46         }
  47
  48         //sort keys
  49         GIM_QUICK_SORT_ARRAY(GIM_RSORT_TOKEN , keycontacts, source_count, RSORT_TOKEN_COMPARATOR,GIM_DEF_EXCHANGE_MACRO);
  50
  51         // Merge contacts
  52         GIM_CONTACT * pcontact = 0;
  53         GIM_CONTACT * scontact = 0;
  54         GUINT32 key,last_key=0;
  55
  56         for(i=0;i<source_contacts->m_size;i++)
  57         {
  58             key = keycontacts[i].m_key;
  59                 scontact = &psource_contacts[keycontacts[i].m_value];
  60
  61                 if(i>0 && last_key ==  key)
  62                 {
  63                         //merge contact
  64                         if(pcontact->m_depth > scontact->m_depth + CONTACT_DIFF_EPSILON)
  65                         {
  66                             GIM_COPY_CONTACTS(pcontact, scontact);
  67                         }
  68                 }
  69                 else
  70                 {//add new contact
  71                     GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
  72             pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
  73                     GIM_COPY_CONTACTS(pcontact, scontact);
  74         }
  75                 last_key = key;
  76         }
  77         gim_free(keycontacts,0);
  78 }
  79
  80 void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
  81                                         GDYNAMIC_ARRAY * dest_contacts)
  82 {
  83     dest_contacts->m_size = 0;
  84     //Traverse the source contacts
  85         GUINT32 source_count = source_contacts->m_size;
  86         if(source_count==0) return;
  87
  88         GIM_CONTACT * psource_contacts  = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
  89
  90         //add the unique contact
  91         GIM_CONTACT * pcontact = 0;
  92     GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
  93     pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
  94     //set the first contact
  95     GIM_COPY_CONTACTS(pcontact, psource_contacts);
  96
  97     if(source_count==1) return;
  98     //scale the first contact
  99     VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
 100
 101     psource_contacts++;
 102
 103         //Average the contacts
 104     GUINT32 i;
 105         for(i=1;i<source_count;i++)
 106         {
 107             VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
 108             VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
 109             psource_contacts++;
 110         }
 111
 112         GREAL divide_average = 1.0f/((GREAL)source_count);
 113
 114         VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
 115
 116         pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
 117         GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
 118
 119         VEC_NORMALIZE(pcontact->m_normal);
 120
 121         /*GREAL normal_len;
 122     VEC_INV_LENGTH(pcontact->m_normal,normal_len);
 123         VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
 124
 125     //Deep = LEN(normal)/SQRT(source_count)
 126     GIM_SQRT(divide_average,divide_average);
 127         pcontact->m_depth = divide_average/normal_len;
 128         */
 129 }
 130
 131
 132