Fixed several bugs: python refcounting related and Bullet related (basic add/remove...
[blender.git] / extern / bullet / Bullet / NarrowPhaseCollision / ManifoldContactAddResult.cpp
1
2 #include "ManifoldContactAddResult.h"
3 #include "NarrowPhaseCollision/PersistentManifold.h"
4
5 ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr)
6                 :m_manifoldPtr(manifoldPtr)
7 {
8         m_transAInv = transA.inverse();
9         m_transBInv = transB.inverse();
10
11 }
12
13
14 void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
15 {
16         if (depth > m_manifoldPtr->GetManifoldMargin())
17                 return;
18
19
20         SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
21         SimdVector3 localA = m_transAInv(pointA );
22         SimdVector3 localB = m_transBInv(pointInWorld);
23         ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
24
25         int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
26         if (insertIndex >= 0)
27         {
28                 m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
29         } else
30         {
31                 m_manifoldPtr->AddManifoldPoint(newPt);
32         }
33 }