Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @ 2192

Last change on this file since 2192 was 2192, checked in by rgrieder, 16 years ago

Reverted all changes of attempt to update physics branch.

  • 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#include <assert.h>
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                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
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        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                clearUserCache(m_pointCache[insertIndex]);
179               
180        } else
181        {
182                m_cachedPoints++;
183
184               
185        }
186        btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
187        m_pointCache[insertIndex] = newPoint;
188        return insertIndex;
189}
190
191btScalar        btPersistentManifold::getContactBreakingThreshold() const
192{
193        return gContactBreakingThreshold;
194}
195
196
197
198void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
199{
200        int i;
201#ifdef DEBUG_PERSISTENCY
202        printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
203                trA.getOrigin().getX(),
204                trA.getOrigin().getY(),
205                trA.getOrigin().getZ(),
206                trB.getOrigin().getX(),
207                trB.getOrigin().getY(),
208                trB.getOrigin().getZ());
209#endif //DEBUG_PERSISTENCY
210        /// first refresh worldspace positions and distance
211        for (i=getNumContacts()-1;i>=0;i--)
212        {
213                btManifoldPoint &manifoldPoint = m_pointCache[i];
214                manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
215                manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
216                manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
217                manifoldPoint.m_lifeTime++;
218        }
219
220        /// then
221        btScalar distance2d;
222        btVector3 projectedDifference,projectedPoint;
223        for (i=getNumContacts()-1;i>=0;i--)
224        {
225               
226                btManifoldPoint &manifoldPoint = m_pointCache[i];
227                //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
228                if (!validContactDistance(manifoldPoint))
229                {
230                        removeContactPoint(i);
231                } else
232                {
233                        //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
234                        projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
235                        projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
236                        distance2d = projectedDifference.dot(projectedDifference);
237                        if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
238                        {
239                                removeContactPoint(i);
240                        } else
241                        {
242                                //contact point processed callback
243                                if (gContactProcessedCallback)
244                                        (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
245                        }
246                }
247        }
248#ifdef DEBUG_PERSISTENCY
249        DebugPersistency();
250#endif //
251}
252
253
254
255
256
Note: See TracBrowser for help on using the repository browser.