SpuContactResult.cpp

Go to the documentation of this file.

/*
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

#ifdef DEBUG_SPU_COLLISION_DETECTION
#ifndef __SPU__
#include <stdio.h>
#define spu_printf printf
#endif
#endif //DEBUG_SPU_COLLISION_DETECTION

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

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

inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1)
{
        btScalar friction = friction0*friction1;

        const btScalar MAX_FRICTION  = btScalar(10.);

        if (friction < -MAX_FRICTION)
                friction = -MAX_FRICTION;
        if (friction > MAX_FRICTION)
                friction = MAX_FRICTION;
        return friction;

}

inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1)
{
        return restitution0*restitution1;
}



 void   SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t       manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped)
 {
        //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
        m_rootWorldTransform0 = worldTrans0;
        m_rootWorldTransform1 = worldTrans1;
        m_manifoldAddress = manifoldAddress;    
        m_spuManifold = spuManifold;

        m_combinedFriction = calculateCombinedFriction(friction0,friction1);
        m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
        m_isSwapped = isSwapped;
 }

 void SpuContactResult::setShapeIdentifiersA(int partId0,int index0)
 {
        
 }

 void SpuContactResult::setShapeIdentifiersB(int partId1,int index1)
 {
        
 }



bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
                                                                   const btVector3& pointInWorld,
                                                                   float depth,
                                                                   btPersistentManifold* manifoldPtr,
                                                                   btTransform& transA,
                                                                   btTransform& transB,
                                                                        btScalar        combinedFriction,
                                                                        btScalar        combinedRestitution,
                                                                   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;

        //if (depth > manifoldPtr->getContactProcessingThreshold())
        //      return false;



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


        if (isSwapped)
        {
                normal = normalOnBInWorld * -1;
                pointA = pointInWorld + normal * depth;
                localA = transA.invXform(pointA );
                localB = transB.invXform(pointInWorld);
        }
        else
        {
                normal = normalOnBInWorld;
                pointA = pointInWorld + normal * depth;
                localA = transA.invXform(pointA );
                localB = transB.invXform(pointInWorld);
        }

        btManifoldPoint newPt(localA,localB,normal,depth);
        newPt.m_positionWorldOnA = pointA;
        newPt.m_positionWorldOnB = pointInWorld;

        newPt.m_combinedFriction = combinedFriction;
        newPt.m_combinedRestitution = combinedRestitution;


        int insertIndex = manifoldPtr->getCacheEntry(newPt);
        if (insertIndex >= 0)
        {
                // we need to replace the current contact point, otherwise small errors will accumulate (spheres start rolling etc)
                manifoldPtr->replaceContactPoint(newPt,insertIndex);
                return true;
                
        } else
        {

                /*
                //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)
{
#if defined (__SPU__) || defined (USE_LIBSPE2)
    memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));

    g_manifoldDmaExport.swapBuffers();
    ppu_address_t mmAddr = (ppu_address_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
#endif
}

void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
#ifdef DEBUG_SPU_COLLISION_DETECTION
        spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
        spu_printf("*** normal = %f,%f,%f\n",normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
        spu_printf("*** position = %f,%f,%f\n",pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
#endif //DEBUG_SPU_COLLISION_DETECTION
        

#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_combinedFriction,
                m_combinedRestitution,
                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;
}