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
gim_merge_contacts(GDYNAMIC_ARRAY * source_contacts,GDYNAMIC_ARRAY * dest_contacts)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
gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,GDYNAMIC_ARRAY * dest_contacts)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
133