Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/GIMPACT/src/gim_contact.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 17 years ago

[Physik] add ode-0.9

File size: 4.2 KB
Line 
1
2/*
3-----------------------------------------------------------------------------
4This source file is part of GIMPACT Library.
5
6For the latest info, see http://gimpact.sourceforge.net/
7
8Copyright (c) 2006 Francisco Leon. C.C. 80087371.
9email: 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
31void 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
80void 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
Note: See TracBrowser for help on using the repository browser.