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 #include "btManifoldResult.h"
17 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
20
21 ///This is to allow MaterialCombiner/Custom Friction/Restitution values
22 ContactAddedCallback gContactAddedCallback = 0;
23
24 CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution;
25 CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction;
26 CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction;
27 CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction;
28 CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping;
29 CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness;
30
calculateCombinedRollingFriction(const btCollisionObject * body0,const btCollisionObject * body1)31 btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1)
32 {
33 btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
34
35 const btScalar MAX_FRICTION = btScalar(10.);
36 if (friction < -MAX_FRICTION)
37 friction = -MAX_FRICTION;
38 if (friction > MAX_FRICTION)
39 friction = MAX_FRICTION;
40 return friction;
41 }
42
calculateCombinedSpinningFriction(const btCollisionObject * body0,const btCollisionObject * body1)43 btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1)
44 {
45 btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
46
47 const btScalar MAX_FRICTION = btScalar(10.);
48 if (friction < -MAX_FRICTION)
49 friction = -MAX_FRICTION;
50 if (friction > MAX_FRICTION)
51 friction = MAX_FRICTION;
52 return friction;
53 }
54
55 ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
calculateCombinedFriction(const btCollisionObject * body0,const btCollisionObject * body1)56 btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1)
57 {
58 btScalar friction = body0->getFriction() * body1->getFriction();
59
60 const btScalar MAX_FRICTION = btScalar(10.);
61 if (friction < -MAX_FRICTION)
62 friction = -MAX_FRICTION;
63 if (friction > MAX_FRICTION)
64 friction = MAX_FRICTION;
65 return friction;
66 }
67
calculateCombinedRestitution(const btCollisionObject * body0,const btCollisionObject * body1)68 btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1)
69 {
70 return body0->getRestitution() * body1->getRestitution();
71 }
72
calculateCombinedContactDamping(const btCollisionObject * body0,const btCollisionObject * body1)73 btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1)
74 {
75 return body0->getContactDamping() + body1->getContactDamping();
76 }
77
calculateCombinedContactStiffness(const btCollisionObject * body0,const btCollisionObject * body1)78 btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1)
79 {
80 btScalar s0 = body0->getContactStiffness();
81 btScalar s1 = body1->getContactStiffness();
82
83 btScalar tmp0 = btScalar(1) / s0;
84 btScalar tmp1 = btScalar(1) / s1;
85 btScalar combinedStiffness = btScalar(1) / (tmp0 + tmp1);
86 return combinedStiffness;
87 }
88
btManifoldResult(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap)89 btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
90 : m_manifoldPtr(0),
91 m_body0Wrap(body0Wrap),
92 m_body1Wrap(body1Wrap)
93 #ifdef DEBUG_PART_INDEX
94 ,
95 m_partId0(-1),
96 m_partId1(-1),
97 m_index0(-1),
98 m_index1(-1)
99 #endif //DEBUG_PART_INDEX
100 ,
101 m_closestPointDistanceThreshold(0)
102 {
103 }
104
addContactPoint(const btVector3 & normalOnBInWorld,const btVector3 & pointInWorld,btScalar depth)105 void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
106 {
107 btAssert(m_manifoldPtr);
108 //order in manifold needs to match
109
110 if (depth > m_manifoldPtr->getContactBreakingThreshold())
111 // if (depth > m_manifoldPtr->getContactProcessingThreshold())
112 return;
113
114 bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
115 bool isNewCollision = m_manifoldPtr->getNumContacts() == 0;
116
117 btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
118
119 btVector3 localA;
120 btVector3 localB;
121
122 if (isSwapped)
123 {
124 localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
125 localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
126 }
127 else
128 {
129 localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
130 localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
131 }
132
133 btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
134 newPt.m_positionWorldOnA = pointA;
135 newPt.m_positionWorldOnB = pointInWorld;
136
137 int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
138
139 newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
140 newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
141 newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
142 newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
143
144 if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
145 (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
146 {
147 newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
148 newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
149 newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
150 }
151
152 if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
153 (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR))
154 {
155 newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
156 }
157
158 btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2);
159
160 //BP mod, store contact triangles.
161 if (isSwapped)
162 {
163 newPt.m_partId0 = m_partId1;
164 newPt.m_partId1 = m_partId0;
165 newPt.m_index0 = m_index1;
166 newPt.m_index1 = m_index0;
167 }
168 else
169 {
170 newPt.m_partId0 = m_partId0;
171 newPt.m_partId1 = m_partId1;
172 newPt.m_index0 = m_index0;
173 newPt.m_index1 = m_index1;
174 }
175 //printf("depth=%f\n",depth);
176 ///@todo, check this for any side effects
177 if (insertIndex >= 0)
178 {
179 //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
180 m_manifoldPtr->replaceContactPoint(newPt, insertIndex);
181 }
182 else
183 {
184 insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
185 }
186
187 //User can override friction and/or restitution
188 if (gContactAddedCallback &&
189 //and if either of the two bodies requires custom material
190 ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
191 (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
192 {
193 //experimental feature info, for per-triangle material etc.
194 const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
195 const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
196 (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
197 }
198
199 if (gContactStartedCallback && isNewCollision)
200 {
201 gContactStartedCallback(m_manifoldPtr);
202 }
203 }
204