[216] | 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 | GUINT 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 | GUINT 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 | GUINT 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 | GUINT 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 | GUINT 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 | |
---|