← Back to branch summary

~ubuntu-branches/ubuntu/precise/supertuxkart/precise

« back to all changes in this revision

Viewing changes to src/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Christoph Egger
  • Date: 2011-02-24 22:36:25 UTC
  • mfrom: (1.1.9 upstream) (6.1.4 sid)
  • Revision ID: james.westby@ubuntu.com-20110224223625-ygrjfpg92obovuch
Tags: 0.7+dfsg1-1
New upstream release

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
1
 
/*
2
 
Bullet Continuous Collision Detection and Physics Library
3
 
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
 
 
5
 
This software is provided 'as-is', without any express or implied warranty.
6
 
In no event will the authors be held liable for any damages arising from the use of this software.
7
 
Permission is granted to anyone to use this software for any purpose, 
8
 
including commercial applications, and to alter it and redistribute it freely, 
9
 
subject to the following restrictions:
10
 
 
11
 
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12
 
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13
 
3. This notice may not be removed or altered from any source distribution.
14
 
*/
15
 
 
16
 
 
17
 
#include "btPersistentManifold.h"
18
 
#include "LinearMath/btTransform.h"
19
 
#include <assert.h>
20
 
 
21
 
btScalar                                        gContactBreakingThreshold = btScalar(0.02);
22
 
ContactDestroyedCallback        gContactDestroyedCallback = 0;
23
 
ContactProcessedCallback        gContactProcessedCallback = 0;
24
 
 
25
 
 
26
 
 
27
 
btPersistentManifold::btPersistentManifold()
28
 
:m_body0(0),
29
 
m_body1(0),
30
 
m_cachedPoints (0),
31
 
m_index1a(0)
32
 
{
33
 
}
34
 
 
35
 
 
36
 
 
37
 
 
38
 
#ifdef DEBUG_PERSISTENCY
39
 
#include <stdio.h>
40
 
void    btPersistentManifold::DebugPersistency()
41
 
{
42
 
        int i;
43
 
        printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
44
 
        for (i=0;i<m_cachedPoints;i++)
45
 
        {
46
 
                printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
47
 
        }
48
 
}
49
 
#endif //DEBUG_PERSISTENCY
50
 
 
51
 
void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
52
 
{
53
 
 
54
 
        void* oldPtr = pt.m_userPersistentData;
55
 
        if (oldPtr)
56
 
        {
57
 
#ifdef DEBUG_PERSISTENCY
58
 
                int i;
59
 
                int occurance = 0;
60
 
                for (i=0;i<m_cachedPoints;i++)
61
 
                {
62
 
                        if (m_pointCache[i].m_userPersistentData == oldPtr)
63
 
                        {
64
 
                                occurance++;
65
 
                                if (occurance>1)
66
 
                                        printf("error in clearUserCache\n");
67
 
                        }
68
 
                }
69
 
                assert(occurance<=0);
70
 
#endif //DEBUG_PERSISTENCY
71
 
 
72
 
                if (pt.m_userPersistentData && gContactDestroyedCallback)
73
 
                {
74
 
                        (*gContactDestroyedCallback)(pt.m_userPersistentData);
75
 
                        pt.m_userPersistentData = 0;
76
 
                }
77
 
                
78
 
#ifdef DEBUG_PERSISTENCY
79
 
                DebugPersistency();
80
 
#endif
81
 
        }
82
 
 
83
 
        
84
 
}
85
 
 
86
 
 
87
 
int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
88
 
{
89
 
 
90
 
                //calculate 4 possible cases areas, and take biggest area
91
 
                //also need to keep 'deepest'
92
 
                
93
 
                int maxPenetrationIndex = -1;
94
 
#define KEEP_DEEPEST_POINT 1
95
 
#ifdef KEEP_DEEPEST_POINT
96
 
                btScalar maxPenetration = pt.getDistance();
97
 
                for (int i=0;i<4;i++)
98
 
                {
99
 
                        if (m_pointCache[i].getDistance() < maxPenetration)
100
 
                        {
101
 
                                maxPenetrationIndex = i;
102
 
                                maxPenetration = m_pointCache[i].getDistance();
103
 
                        }
104
 
                }
105
 
#endif //KEEP_DEEPEST_POINT
106
 
                
107
 
                btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
108
 
                if (maxPenetrationIndex != 0)
109
 
                {
110
 
                        btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
111
 
                        btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
112
 
                        btVector3 cross = a0.cross(b0);
113
 
                        res0 = cross.length2();
114
 
                }
115
 
                if (maxPenetrationIndex != 1)
116
 
                {
117
 
                        btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
118
 
                        btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
119
 
                        btVector3 cross = a1.cross(b1);
120
 
                        res1 = cross.length2();
121
 
                }
122
 
 
123
 
                if (maxPenetrationIndex != 2)
124
 
                {
125
 
                        btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
126
 
                        btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
127
 
                        btVector3 cross = a2.cross(b2);
128
 
                        res2 = cross.length2();
129
 
                }
130
 
 
131
 
                if (maxPenetrationIndex != 3)
132
 
                {
133
 
                        btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
134
 
                        btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
135
 
                        btVector3 cross = a3.cross(b3);
136
 
                        res3 = cross.length2();
137
 
                }
138
 
 
139
 
                btVector4 maxvec(res0,res1,res2,res3);
140
 
                int biggestarea = maxvec.closestAxis4();
141
 
                return biggestarea;
142
 
}
143
 
 
144
 
 
145
 
int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
146
 
{
147
 
        btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
148
 
        int size = getNumContacts();
149
 
        int nearestPoint = -1;
150
 
        for( int i = 0; i < size; i++ )
151
 
        {
152
 
                const btManifoldPoint &mp = m_pointCache[i];
153
 
 
154
 
                btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
155
 
                const btScalar distToManiPoint = diffA.dot(diffA);
156
 
                if( distToManiPoint < shortestDist )
157
 
                {
158
 
                        shortestDist = distToManiPoint;
159
 
                        nearestPoint = i;
160
 
                }
161
 
        }
162
 
        return nearestPoint;
163
 
}
164
 
 
165
 
void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
166
 
{
167
 
        assert(validContactDistance(newPoint));
168
 
 
169
 
        int insertIndex = getNumContacts();
170
 
        if (insertIndex == MANIFOLD_CACHE_SIZE)
171
 
        {
172
 
#if MANIFOLD_CACHE_SIZE >= 4
173
 
                //sort cache so best points come first, based on area
174
 
                insertIndex = sortCachedPoints(newPoint);
175
 
#else
176
 
                insertIndex = 0;
177
 
#endif
178
 
 
179
 
                
180
 
        } else
181
 
        {
182
 
                m_cachedPoints++;
183
 
 
184
 
                
185
 
        }
186
 
        replaceContactPoint(newPoint,insertIndex);
187
 
}
188
 
 
189
 
btScalar        btPersistentManifold::getContactBreakingThreshold() const
190
 
{
191
 
        return gContactBreakingThreshold;
192
 
}
193
 
 
194
 
 
195
 
 
196
 
void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
197
 
{
198
 
        int i;
199
 
#ifdef DEBUG_PERSISTENCY
200
 
        printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
201
 
                trA.getOrigin().getX(),
202
 
                trA.getOrigin().getY(),
203
 
                trA.getOrigin().getZ(),
204
 
                trB.getOrigin().getX(),
205
 
                trB.getOrigin().getY(),
206
 
                trB.getOrigin().getZ());
207
 
#endif //DEBUG_PERSISTENCY
208
 
        /// first refresh worldspace positions and distance
209
 
        for (i=getNumContacts()-1;i>=0;i--)
210
 
        {
211
 
                btManifoldPoint &manifoldPoint = m_pointCache[i];
212
 
                manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
213
 
                manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
214
 
                manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
215
 
                manifoldPoint.m_lifeTime++;
216
 
        }
217
 
 
218
 
        /// then 
219
 
        btScalar distance2d;
220
 
        btVector3 projectedDifference,projectedPoint;
221
 
        for (i=getNumContacts()-1;i>=0;i--)
222
 
        {
223
 
                
224
 
                btManifoldPoint &manifoldPoint = m_pointCache[i];
225
 
                //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
226
 
                if (!validContactDistance(manifoldPoint))
227
 
                {
228
 
                        removeContactPoint(i);
229
 
                } else
230
 
                {
231
 
                        //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
232
 
                        projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
233
 
                        projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
234
 
                        distance2d = projectedDifference.dot(projectedDifference);
235
 
                        if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
236
 
                        {
237
 
                                removeContactPoint(i);
238
 
                        } else
239
 
                        {
240
 
                                //contact point processed callback
241
 
                                if (gContactProcessedCallback)
242
 
                                        (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
243
 
                        }
244
 
                }
245
 
        }
246
 
#ifdef DEBUG_PERSISTENCY
247
 
        DebugPersistency();
248
 
#endif //
249
 
}
250
 
 
251
 
 
252
 
 
253
 
 
254
 
 
 
1
/*
 
2
Bullet Continuous Collision Detection and Physics Library
 
3
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
4
 
 
5
This software is provided 'as-is', without any express or implied warranty.
 
6
In no event will the authors be held liable for any damages arising from the use of this software.
 
7
Permission is granted to anyone to use this software for any purpose, 
 
8
including commercial applications, and to alter it and redistribute it freely, 
 
9
subject to the following restrictions:
 
10
 
 
11
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 
12
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 
13
3. This notice may not be removed or altered from any source distribution.
 
14
*/
 
15
 
 
16
 
 
17
#include "btPersistentManifold.h"
 
18
#include "LinearMath/btTransform.h"
 
19
#include <assert.h>
 
20
 
 
21
btScalar                                        gContactBreakingThreshold = btScalar(0.02);
 
22
ContactDestroyedCallback        gContactDestroyedCallback = 0;
 
23
ContactProcessedCallback        gContactProcessedCallback = 0;
 
24
 
 
25
 
 
26
 
 
27
btPersistentManifold::btPersistentManifold()
 
28
:m_body0(0),
 
29
m_body1(0),
 
30
m_cachedPoints (0),
 
31
m_index1a(0)
 
32
{
 
33
}
 
34
 
 
35
 
 
36
 
 
37
 
 
38
#ifdef DEBUG_PERSISTENCY
 
39
#include <stdio.h>
 
40
void    btPersistentManifold::DebugPersistency()
 
41
{
 
42
        int i;
 
43
        printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
 
44
        for (i=0;i<m_cachedPoints;i++)
 
45
        {
 
46
                printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
 
47
        }
 
48
}
 
49
#endif //DEBUG_PERSISTENCY
 
50
 
 
51
void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
 
52
{
 
53
 
 
54
        void* oldPtr = pt.m_userPersistentData;
 
55
        if (oldPtr)
 
56
        {
 
57
#ifdef DEBUG_PERSISTENCY
 
58
                int i;
 
59
                int occurance = 0;
 
60
                for (i=0;i<m_cachedPoints;i++)
 
61
                {
 
62
                        if (m_pointCache[i].m_userPersistentData == oldPtr)
 
63
                        {
 
64
                                occurance++;
 
65
                                if (occurance>1)
 
66
                                        printf("error in clearUserCache\n");
 
67
                        }
 
68
                }
 
69
                assert(occurance<=0);
 
70
#endif //DEBUG_PERSISTENCY
 
71
 
 
72
                if (pt.m_userPersistentData && gContactDestroyedCallback)
 
73
                {
 
74
                        (*gContactDestroyedCallback)(pt.m_userPersistentData);
 
75
                        pt.m_userPersistentData = 0;
 
76
                }
 
77
                
 
78
#ifdef DEBUG_PERSISTENCY
 
79
                DebugPersistency();
 
80
#endif
 
81
        }
 
82
 
 
83
        
 
84
}
 
85
 
 
86
 
 
87
int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
 
88
{
 
89
 
 
90
                //calculate 4 possible cases areas, and take biggest area
 
91
                //also need to keep 'deepest'
 
92
                
 
93
                int maxPenetrationIndex = -1;
 
94
#define KEEP_DEEPEST_POINT 1
 
95
#ifdef KEEP_DEEPEST_POINT
 
96
                btScalar maxPenetration = pt.getDistance();
 
97
                for (int i=0;i<4;i++)
 
98
                {
 
99
                        if (m_pointCache[i].getDistance() < maxPenetration)
 
100
                        {
 
101
                                maxPenetrationIndex = i;
 
102
                                maxPenetration = m_pointCache[i].getDistance();
 
103
                        }
 
104
                }
 
105
#endif //KEEP_DEEPEST_POINT
 
106
                
 
107
                btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
 
108
                if (maxPenetrationIndex != 0)
 
109
                {
 
110
                        btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
 
111
                        btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
 
112
                        btVector3 cross = a0.cross(b0);
 
113
                        res0 = cross.length2();
 
114
                }
 
115
                if (maxPenetrationIndex != 1)
 
116
                {
 
117
                        btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
 
118
                        btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
 
119
                        btVector3 cross = a1.cross(b1);
 
120
                        res1 = cross.length2();
 
121
                }
 
122
 
 
123
                if (maxPenetrationIndex != 2)
 
124
                {
 
125
                        btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
 
126
                        btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
 
127
                        btVector3 cross = a2.cross(b2);
 
128
                        res2 = cross.length2();
 
129
                }
 
130
 
 
131
                if (maxPenetrationIndex != 3)
 
132
                {
 
133
                        btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
 
134
                        btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
 
135
                        btVector3 cross = a3.cross(b3);
 
136
                        res3 = cross.length2();
 
137
                }
 
138
 
 
139
                btVector4 maxvec(res0,res1,res2,res3);
 
140
                int biggestarea = maxvec.closestAxis4();
 
141
                return biggestarea;
 
142
}
 
143
 
 
144
 
 
145
int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
 
146
{
 
147
        btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
 
148
        int size = getNumContacts();
 
149
        int nearestPoint = -1;
 
150
        for( int i = 0; i < size; i++ )
 
151
        {
 
152
                const btManifoldPoint &mp = m_pointCache[i];
 
153
 
 
154
                btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
 
155
                const btScalar distToManiPoint = diffA.dot(diffA);
 
156
                if( distToManiPoint < shortestDist )
 
157
                {
 
158
                        shortestDist = distToManiPoint;
 
159
                        nearestPoint = i;
 
160
                }
 
161
        }
 
162
        return nearestPoint;
 
163
}
 
164
 
 
165
void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
 
166
{
 
167
        assert(validContactDistance(newPoint));
 
168
 
 
169
        int insertIndex = getNumContacts();
 
170
        if (insertIndex == MANIFOLD_CACHE_SIZE)
 
171
        {
 
172
#if MANIFOLD_CACHE_SIZE >= 4
 
173
                //sort cache so best points come first, based on area
 
174
                insertIndex = sortCachedPoints(newPoint);
 
175
#else
 
176
                insertIndex = 0;
 
177
#endif
 
178
 
 
179
                
 
180
        } else
 
181
        {
 
182
                m_cachedPoints++;
 
183
 
 
184
                
 
185
        }
 
186
        replaceContactPoint(newPoint,insertIndex);
 
187
}
 
188
 
 
189
btScalar        btPersistentManifold::getContactBreakingThreshold() const
 
190
{
 
191
        return gContactBreakingThreshold;
 
192
}
 
193
 
 
194
 
 
195
 
 
196
void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
 
197
{
 
198
        int i;
 
199
#ifdef DEBUG_PERSISTENCY
 
200
        printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
 
201
                trA.getOrigin().getX(),
 
202
                trA.getOrigin().getY(),
 
203
                trA.getOrigin().getZ(),
 
204
                trB.getOrigin().getX(),
 
205
                trB.getOrigin().getY(),
 
206
                trB.getOrigin().getZ());
 
207
#endif //DEBUG_PERSISTENCY
 
208
        /// first refresh worldspace positions and distance
 
209
        for (i=getNumContacts()-1;i>=0;i--)
 
210
        {
 
211
                btManifoldPoint &manifoldPoint = m_pointCache[i];
 
212
                manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
 
213
                manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
 
214
                manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
 
215
                manifoldPoint.m_lifeTime++;
 
216
        }
 
217
 
 
218
        /// then 
 
219
        btScalar distance2d;
 
220
        btVector3 projectedDifference,projectedPoint;
 
221
        for (i=getNumContacts()-1;i>=0;i--)
 
222
        {
 
223
                
 
224
                btManifoldPoint &manifoldPoint = m_pointCache[i];
 
225
                //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
 
226
                if (!validContactDistance(manifoldPoint))
 
227
                {
 
228
                        removeContactPoint(i);
 
229
                } else
 
230
                {
 
231
                        //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
 
232
                        projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
 
233
                        projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
 
234
                        distance2d = projectedDifference.dot(projectedDifference);
 
235
                        if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
 
236
                        {
 
237
                                removeContactPoint(i);
 
238
                        } else
 
239
                        {
 
240
                                //contact point processed callback
 
241
                                if (gContactProcessedCallback)
 
242
                                        (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
 
243
                        }
 
244
                }
 
245
        }
 
246
#ifdef DEBUG_PERSISTENCY
 
247
        DebugPersistency();
 
248
#endif //
 
249
}
 
250
 
 
251
 
 
252
 
 
253
 
 
254