added some get methods and stuff
[blender.git] / extern / bullet / Bullet / NarrowPhaseCollision / PersistentManifold.cpp
1 /*
2 * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
3 *
4 * Permission to use, copy, modify, distribute and sell this software
5 * and its documentation for any purpose is hereby granted without fee,
6 * provided that the above copyright notice appear in all copies.
7 * Erwin Coumans makes no representations about the suitability 
8 * of this software for any purpose.  
9 * It is provided "as is" without express or implied warranty.
10 */
11
12
13 #include "PersistentManifold.h"
14 #include "SimdTransform.h"
15 #include <assert.h>
16
17 float gContactBreakingTreshold = 0.02f;
18
19 PersistentManifold::PersistentManifold()
20 :m_body0(0),
21 m_body1(0),
22 m_cachedPoints (0),
23 m_index1(0)
24 {
25 }
26
27
28 void    PersistentManifold::ClearManifold()
29 {
30         m_cachedPoints = 0;
31 }
32
33
34
35
36
37 int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt) 
38 {
39
40                 //calculate 4 possible cases areas, and take biggest area
41
42                 SimdScalar res0,res1,res2,res3;
43
44                 {
45                         SimdVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
46                         SimdVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
47                         SimdVector3 cross = a0.cross(b0);
48                         res0 = cross.length2();
49                 }
50                 {
51                         SimdVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
52                         SimdVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
53                         SimdVector3 cross = a1.cross(b1);
54                         res1 = cross.length2();
55                 }
56                 {
57                         SimdVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
58                         SimdVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
59                         SimdVector3 cross = a2.cross(b2);
60                         res2 = cross.length2();
61                 }
62                 {
63                         SimdVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
64                         SimdVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
65                         SimdVector3 cross = a3.cross(b3);
66                         res3 = cross.length2();
67                 }
68
69                 SimdVector4 maxvec(res0,res1,res2,res3);
70                 int biggestarea = maxvec.closestAxis4();
71
72                 return biggestarea;
73
74
75 }
76
77
78
79 int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const
80 {
81         SimdScalar shortestDist =  GetManifoldMargin() * GetManifoldMargin();
82         int size = GetNumContacts();
83         int nearestPoint = -1;
84         for( int i = 0; i < size; i++ )
85         {
86                 const ManifoldPoint &mp = m_pointCache[i];
87
88                 SimdVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
89                 const SimdScalar distToManiPoint = diffA.dot(diffA);
90                 if( distToManiPoint < shortestDist )
91                 {
92                         shortestDist = distToManiPoint;
93                         nearestPoint = i;
94                 }
95         }
96         return nearestPoint;
97 }
98
99 void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
100 {
101         assert(ValidContactDistance(newPoint));
102
103         int insertIndex = GetNumContacts();
104         if (insertIndex == MANIFOLD_CACHE_SIZE)
105         {
106 #if MANIFOLD_CACHE_SIZE >= 4
107                 //sort cache so best points come first, based on area
108                 insertIndex = SortCachedPoints(newPoint);
109 #else
110                 insertIndex = 0;
111 #endif
112
113         } else
114         {
115                 m_cachedPoints++;
116         }
117         ReplaceContactPoint(newPoint,insertIndex);
118 }
119
120 float   PersistentManifold::GetManifoldMargin() const
121 {
122         return gContactBreakingTreshold;
123 }
124
125 void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB)
126 {
127         int i;
128
129         /// first refresh worldspace positions and distance
130         for (i=GetNumContacts()-1;i>=0;i--)
131         {
132                 ManifoldPoint &manifoldPoint = m_pointCache[i];
133                 manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
134                 manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
135                 manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
136                 manifoldPoint.m_lifeTime++;
137         }
138
139         /// then 
140         SimdScalar distance2d;
141         SimdVector3 projectedDifference,projectedPoint;
142         for (i=GetNumContacts()-1;i>=0;i--)
143         {
144                 
145                 ManifoldPoint &manifoldPoint = m_pointCache[i];
146                 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
147                 if (!ValidContactDistance(manifoldPoint))
148                 {
149                         RemoveContactPoint(i);
150                 } else
151                 {
152                         //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
153                         projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
154                         projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
155                         distance2d = projectedDifference.dot(projectedDifference);
156                         if (distance2d  > GetManifoldMargin()*GetManifoldMargin() )
157                         {
158                                 RemoveContactPoint(i);
159                         }
160                 }
161         }
162 }
163
164
165 //todo: remove this treshold
166 float gPenetrationDistanceCheck = -0.05f;
167
168 float   PersistentManifold::GetCollisionImpulse() const
169 {
170         float averageImpulse = 0.f;
171         if (GetNumContacts() > 0)
172         {
173                 float totalImpulse = 0.f;
174
175                 //return the sum of the applied impulses on the box
176                 for (int i=0;i<GetNumContacts();i++)
177                 {
178                         const ManifoldPoint& cp = GetContactPoint(i);
179                         //avoid conflic noice
180                         if ( cp.GetDistance() <gPenetrationDistanceCheck)
181                                 return 0.f;
182
183                         totalImpulse += cp.m_appliedImpulse;
184
185                 }
186                 averageImpulse = totalImpulse / ((float)GetNumContacts());
187
188         }
189         return averageImpulse;
190
191 }
192
193