bullet3/Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "SpuContactResult.h"


//#define DEBUG_SPU_COLLISION_DETECTION 1


SpuContactResult::SpuContactResult()
{
	m_manifoldAddress = 0;
	m_spuManifold = NULL;
	m_RequiresWriteBack = false;
}

 SpuContactResult::~SpuContactResult()
{
	g_manifoldDmaExport.swapBuffers();
}

 void	SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, uint64_t	manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, bool isSwapped)
 {
	//spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
	m_rootWorldTransform0 = worldTrans0;
	m_rootWorldTransform1 = worldTrans1;
	m_manifoldAddress = manifoldAddress;    
	m_spuManifold = spuManifold;
	m_isSwapped = isSwapped;
 }

 void SpuContactResult::setShapeIdentifiers(int partId0,int index0,	int partId1,int index1)
 {
	
 }
	


 ///return true if it requires a dma transfer back
bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
								   const btVector3& pointInWorld,
								   float depth,
								   btPersistentManifold* manifoldPtr,
								   btTransform& transA,
								   btTransform& transB,
								   bool isSwapped)
{
	
	float contactTreshold = manifoldPtr->getContactBreakingThreshold();

	//spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);

#ifdef DEBUG_SPU_COLLISION_DETECTION
	spu_printf("SPU: contactTreshold %f\n",contactTreshold);
#endif //DEBUG_SPU_COLLISION_DETECTION
	if (depth > manifoldPtr->getContactBreakingThreshold())
		return false;

	//provide inverses or just calculate?
	btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform;
	btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform;

	btVector3 pointA;
	btVector3 localA;
	btVector3 localB;
	btVector3 normal;

	if (isSwapped)
	{
		normal = normalOnBInWorld * -1;
		pointA = pointInWorld + normal * depth;
		localA = transAInv(pointA );
		localB = transBInv(pointInWorld);
		/*localA = transBInv(pointA );
		localB = transAInv(pointInWorld);*/
	}
	else
	{
		normal = normalOnBInWorld;
		pointA = pointInWorld + normal * depth;
		localA = transAInv(pointA );
		localB = transBInv(pointInWorld);
	}

	btManifoldPoint newPt(localA,localB,normal,depth);

	int insertIndex = manifoldPtr->getCacheEntry(newPt);
	if (insertIndex >= 0)
	{
//		manifoldPtr->replaceContactPoint(newPt,insertIndex);
//		return true;

#ifdef DEBUG_SPU_COLLISION_DETECTION
		spu_printf("SPU: same contact detected, nothing done\n");
#endif //DEBUG_SPU_COLLISION_DETECTION
		// This is not needed, just use the old info! saves a DMA transfer as well
	} else
	{

		newPt.m_combinedFriction = 0.25f;//calculateCombinedFriction(m_body0,m_body1);
		newPt.m_combinedRestitution = 0.0f;//calculateCombinedRestitution(m_body0,m_body1);

		/*
		//potential TODO: SPU callbacks, either immediate (local on the SPU), or deferred
		//User can override friction and/or restitution
		if (gContactAddedCallback &&
			//and if either of the two bodies requires custom material
			 ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
			   (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
		{
			//experimental feature info, for per-triangle material etc.
			(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
		}
		*/
		manifoldPtr->AddManifoldPoint(newPt);
		return true;

	}
	return false;
	
}


void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold)
{
    memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));

    g_manifoldDmaExport.swapBuffers();
    uint64_t mmAddr = (uint32_t)mmManifold;
    g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9));
	// Should there be any kind of wait here?  What if somebody tries to use this tag again?  What if we call this function again really soon?
	//no, the swapBuffers does the wait
}

void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btPoint3& pointInWorld,float depth)
{
	//spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);

#ifdef DEBUG_SPU_COLLISION_DETECTION
 //   int sman = sizeof(rage::phManifold);
//	spu_printf("sizeof_manifold = %i\n",sman);
#endif //DEBUG_SPU_COLLISION_DETECTION

	btPersistentManifold* localManifold = m_spuManifold;

	btVector3	normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
	btVector3	pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());

	//process the contact point
	const bool retVal = ManifoldResultAddContactPoint(normalB,
		pointWrld,
		depth,
		localManifold,
		m_rootWorldTransform0,
		m_rootWorldTransform1,
		m_isSwapped);
	m_RequiresWriteBack = m_RequiresWriteBack || retVal;
}

void SpuContactResult::flush()
{

	if (m_spuManifold && m_spuManifold->getNumContacts())
	{
		m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
		m_RequiresWriteBack = true;
	}


	if (m_RequiresWriteBack)
	{
#ifdef DEBUG_SPU_COLLISION_DETECTION
		spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
		spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
		spu_printf("Manifold address: %llu\n", m_manifoldAddress);
#endif //DEBUG_SPU_COLLISION_DETECTION
	//	spu_printf("writeDoubleBufferedManifold\n");
		writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
#ifdef DEBUG_SPU_COLLISION_DETECTION
		spu_printf("SPU: Finished (Put) DMA\n");
#endif //DEBUG_SPU_COLLISION_DETECTION
	}
	m_spuManifold = NULL;
	m_RequiresWriteBack = false;
}