Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @ 8331

Last change on this file since 8331 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 7.0 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
20
21btScalar                                        gContactBreakingThreshold = btScalar(0.02);
22ContactDestroyedCallback        gContactDestroyedCallback = 0;
23ContactProcessedCallback        gContactProcessedCallback = 0;
24
25
26
27btPersistentManifold::btPersistentManifold()
28:m_body0(0),
29m_body1(0),
30m_cachedPoints (0),
31m_index1a(0)
32{
33}
34
35
36
37
38#ifdef DEBUG_PERSISTENCY
39#include <stdio.h>
40void    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
51void 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                btAssert(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
87int 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
145int 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
165int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
166{
167        btAssert(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                clearUserCache(m_pointCache[insertIndex]);
179               
180        } else
181        {
182                m_cachedPoints++;
183
184               
185        }
186        if (insertIndex<0)
187                insertIndex=0;
188
189        btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
190        m_pointCache[insertIndex] = newPoint;
191        return insertIndex;
192}
193
194btScalar        btPersistentManifold::getContactBreakingThreshold() const
195{
196        return m_contactBreakingThreshold;
197}
198
199
200
201void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
202{
203        int i;
204#ifdef DEBUG_PERSISTENCY
205        printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
206                trA.getOrigin().getX(),
207                trA.getOrigin().getY(),
208                trA.getOrigin().getZ(),
209                trB.getOrigin().getX(),
210                trB.getOrigin().getY(),
211                trB.getOrigin().getZ());
212#endif //DEBUG_PERSISTENCY
213        /// first refresh worldspace positions and distance
214        for (i=getNumContacts()-1;i>=0;i--)
215        {
216                btManifoldPoint &manifoldPoint = m_pointCache[i];
217                manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
218                manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
219                manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
220                manifoldPoint.m_lifeTime++;
221        }
222
223        /// then
224        btScalar distance2d;
225        btVector3 projectedDifference,projectedPoint;
226        for (i=getNumContacts()-1;i>=0;i--)
227        {
228               
229                btManifoldPoint &manifoldPoint = m_pointCache[i];
230                //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
231                if (!validContactDistance(manifoldPoint))
232                {
233                        removeContactPoint(i);
234                } else
235                {
236                        //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
237                        projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
238                        projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
239                        distance2d = projectedDifference.dot(projectedDifference);
240                        if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
241                        {
242                                removeContactPoint(i);
243                        } else
244                        {
245                                //contact point processed callback
246                                if (gContactProcessedCallback)
247                                        (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
248                        }
249                }
250        }
251#ifdef DEBUG_PERSISTENCY
252        DebugPersistency();
253#endif //
254}
255
256
257
258
259
Note: See TracBrowser for help on using the repository browser.