forked from bartvdbraak/blender
33 lines
926 B
C++
33 lines
926 B
C++
|
|
#include "ManifoldContactAddResult.h"
|
|
#include "NarrowPhaseCollision/PersistentManifold.h"
|
|
|
|
ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr)
|
|
:m_manifoldPtr(manifoldPtr)
|
|
{
|
|
m_transAInv = transA.inverse();
|
|
m_transBInv = transB.inverse();
|
|
|
|
}
|
|
|
|
|
|
void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
|
|
{
|
|
if (depth > m_manifoldPtr->GetManifoldMargin())
|
|
return;
|
|
|
|
|
|
SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
|
|
SimdVector3 localA = m_transAInv(pointA );
|
|
SimdVector3 localB = m_transBInv(pointInWorld);
|
|
ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
|
|
|
|
int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
|
|
if (insertIndex >= 0)
|
|
{
|
|
m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
|
|
} else
|
|
{
|
|
m_manifoldPtr->AddManifoldPoint(newPt);
|
|
}
|
|
} |