void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
{
BT_PROFILE("convexSweepTest");
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
btTransform convexFromTrans,convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
btVector3 linVel, angVel;
btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
btVector3 zeroLinVel;
zeroLinVel.setValue(0,0,0);
btTransform R;
R.setIdentity ();
R.setRotation (convexFromTrans.getRotation());
castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
}
#ifndef USE_BRUTEFORCE_RAYBROADPHASE
btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration);
m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax);
#else
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
{
objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback,
allowedCcdPenetration);
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
bool Raytracer::lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{
btScalar closestHitResults = 1.f;
bool hasHit = false;
btConvexCast::CastResult rayResult;
btSphereShape pointShape(0.0f);
btTransform rayFromTrans;
btTransform rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayTo);
for (int s=0;s<numObjects;s++)
{
//do some culling, ray versus aabb
btVector3 aabbMin,aabbMax;
shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
btScalar hitLambda = 1.f;
btVector3 hitNormal;
btCollisionObject tmpObj;
tmpObj.setWorldTransform(transforms[s]);
if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
{
//reset previous result
//choose the continuous collision detection method
btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
//btGjkConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
//btContinuousConvexCollision convexCaster(&pointShape,shapePtr[s],&simplexSolver,0);
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
{
if (rayResult.m_fraction < closestHitResults)
{
closestHitResults = rayResult.m_fraction;
worldNormal = transforms[s].getBasis() *rayResult.m_normal;
worldNormal.normalize();
hasHit = true;
}
}
}
}
return hasHit;
}
bool Raytracer::singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{
btScalar closestHitResults = 1.f;
ClosestRayResultCallback resultCallback(rayFrom,rayTo);
bool hasHit = false;
btConvexCast::CastResult rayResult;
btSphereShape pointShape(0.0f);
btTransform rayFromTrans;
btTransform rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayTo);
for (int s=0;s<numObjects;s++)
{
//comment-out next line to get all hits, instead of just the closest hit
//resultCallback.m_closestHitFraction = 1.f;
//do some culling, ray versus aabb
btVector3 aabbMin,aabbMax;
shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
btScalar hitLambda = 1.f;
btVector3 hitNormal;
btCollisionObject tmpObj;
tmpObj.setWorldTransform(transforms[s]);
if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
{
//reset previous result
btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, &tmpObj, shapePtr[s], transforms[s], resultCallback);
if (resultCallback.hasHit())
{
//float fog = 1.f - 0.1f * rayResult.m_fraction;
resultCallback.m_hitNormalWorld.normalize();//.m_normal.normalize();
worldNormal = resultCallback.m_hitNormalWorld;
//worldNormal = transforms[s].getBasis() *rayResult.m_normal;
worldNormal.normalize();
hasHit = true;
}
}
}
return hasHit;
}
void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
{
btTransform convexFromTrans,convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
btVector3 linVel, angVel;
btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
btTransform R;
R.setIdentity ();
R.setRotation (convexFromTrans.getRotation());
castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
}
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_overlappingObjects.size();i++)
{
btCollisionObject* collisionObject= m_overlappingObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
{
btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback,
allowedCcdPenetration);
}
}
}
}
void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask)
{
btTransform rayFromTrans,rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFromWorld);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayToWorld);
/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
{
rayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback);
}
}
}
}
void processRaycastTask(void* userPtr, void* lsMemory)
{
RaycastTask_LocalStoreMemory* localMemory = (RaycastTask_LocalStoreMemory*)lsMemory;
SpuRaycastTaskDesc* taskDescPtr = (SpuRaycastTaskDesc*)userPtr;
SpuRaycastTaskDesc& taskDesc = *taskDescPtr;
SpuCollisionObjectWrapper* cows = (SpuCollisionObjectWrapper*)taskDesc.spuCollisionObjectsWrappers;
//spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers);
/* for each object */
RaycastGatheredObjectData gatheredObjectData;
for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++)
{
//spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers);
/* load initial collision shape */
GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]);
if (btBroadphaseProxy::isConcave (gatheredObjectData.m_shapeType))
{
SpuRaycastTaskWorkUnitOut tWorkUnitsOut[SPU_RAYCAST_WORK_UNITS_PER_TASK];
for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
tWorkUnitsOut[rayId].hitFraction = 1.0;
}
performRaycastAgainstConcave (&gatheredObjectData, &taskDesc.workUnits[0], &tWorkUnitsOut[0], taskDesc.numWorkUnits, localMemory);
for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
if (tWorkUnitsOut[rayId].hitFraction == 1.0)
continue;
ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
/* XXX Only support taking the closest hit for now */
if (tWorkUnitsOut[rayId].hitFraction < workUnitOut.hitFraction)
{
workUnitOut.hitFraction = tWorkUnitsOut[rayId].hitFraction;
workUnitOut.hitNormal = tWorkUnitsOut[rayId].hitNormal;
}
/* write ray cast data back */
dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
} else if (btBroadphaseProxy::isConvex (gatheredObjectData.m_shapeType)) {
btVector3 objectBoxMin, objectBoxMax;
computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform);
for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
btScalar ignored_param = 1.0;
btVector3 ignored_normal;
if (btRayAabb(workUnit.rayFrom, workUnit.rayTo, objectBoxMin, objectBoxMax, ignored_param, ignored_normal))
{
ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
SpuRaycastTaskWorkUnitOut tWorkUnitOut;
tWorkUnitOut.hitFraction = 1.0;
performRaycastAgainstConvex (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
if (tWorkUnitOut.hitFraction == 1.0)
continue;
dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
/* XXX Only support taking the closest hit for now */
if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
{
workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
/* write ray cast data back */
dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
} else if (btBroadphaseProxy::isCompound (gatheredObjectData.m_shapeType)) {
for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
SpuRaycastTaskWorkUnitOut tWorkUnitOut;
tWorkUnitOut.hitFraction = 1.0;
performRaycastAgainstCompound (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
if (tWorkUnitOut.hitFraction == 1.0)
continue;
dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
/* XXX Only support taking the closest hit for now */
if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
{
workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
}
/* write ray cast data back */
dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
}
void spuWalkStacklessQuantizedTreeAgainstRays(RaycastTask_LocalStoreMemory* lsMemPtr,
btNodeOverlapCallback* nodeCallback,
const btVector3* rayFrom,
const btVector3* rayTo,
int numWorkUnits,
unsigned short int* quantizedQueryAabbMin,
unsigned short int* quantizedQueryAabbMax,
const btQuantizedBvhNode* rootNode,
int startNodeIndex,int endNodeIndex)
{
int curIndex = startNodeIndex;
int walkIterations = 0;
int subTreeSize = endNodeIndex - startNodeIndex;
int escapeIndex;
unsigned int boxBoxOverlap, rayBoxOverlap, anyRayBoxOverlap;
unsigned int isLeafNode;
#define RAYAABB2
#ifdef RAYAABB2
unsigned int sign[SPU_RAYCAST_WORK_UNITS_PER_TASK][3];
btVector3 rayInvDirection[SPU_RAYCAST_WORK_UNITS_PER_TASK];
btScalar lambda_max[SPU_RAYCAST_WORK_UNITS_PER_TASK];
for (int i = 0; i < numWorkUnits; i++)
{
btVector3 rayDirection = (rayTo[i]-rayFrom[i]);
rayDirection.normalize ();
lambda_max[i] = rayDirection.dot(rayTo[i]-rayFrom[i]);
rayInvDirection[i][0] = btScalar(1.0) / rayDirection[0];
rayInvDirection[i][1] = btScalar(1.0) / rayDirection[1];
rayInvDirection[i][2] = btScalar(1.0) / rayDirection[2];
sign[i][0] = rayDirection[0] < 0.0;
sign[i][1] = rayDirection[1] < 0.0;
sign[i][2] = rayDirection[2] < 0.0;
}
#endif
while (curIndex < endNodeIndex)
{
//catch bugs in tree data
assert (walkIterations < subTreeSize);
walkIterations++;
isLeafNode = rootNode->isLeafNode();
anyRayBoxOverlap = 0;
for (int i = 0; i < numWorkUnits; i++)
{
unsigned short int* quamin = (quantizedQueryAabbMin + 3 * i);
unsigned short int* quamax = (quantizedQueryAabbMax + 3 * i);
boxBoxOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quamin,quamax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
if (!boxBoxOverlap)
continue;
rayBoxOverlap = 0;
btScalar param = 1.0;
btVector3 normal;
btVector3 bounds[2];
bounds[0] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMin);
bounds[1] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMax);
#ifdef RAYAABB2
rayBoxOverlap = btRayAabb2 (rayFrom[i], rayInvDirection[i], sign[i], bounds, param, 0.0, lambda_max[i]);
#else
rayBoxOverlap = btRayAabb(rayFrom[i], rayTo[i], bounds[0], bounds[1], param, normal);
#endif
#ifndef CALLBACK_ALL
anyRayBoxOverlap = rayBoxOverlap || anyRayBoxOverlap;
/* If we have any ray vs. box overlap and this isn't a leaf node
we know that we need to dig deeper
*/
if (!isLeafNode && anyRayBoxOverlap)
break;
if (isLeafNode && rayBoxOverlap)
{
spuRaycastNodeCallback1* callback = (spuRaycastNodeCallback1*)nodeCallback;
callback->setWorkUnit (i);
nodeCallback->processNode (0, rootNode->getTriangleIndex());
}
#else
/* If we have any ray vs. box overlap and this isn't a leaf node
we know that we need to dig deeper
*/
if (rayBoxOverlap)
{
anyRayBoxOverlap = 1;
break;
}
#endif
}
#ifdef CALLBACK_ALL
if (isLeafNode && anyRayBoxOverlap)
{
nodeCallback->processNode (0, rootNode->getTriangleIndex());
}
#endif
if (anyRayBoxOverlap || isLeafNode)
{
rootNode++;
curIndex++;
} else
{
escapeIndex = rootNode->getEscapeIndex();
rootNode += escapeIndex;
curIndex += escapeIndex;
}
}
}
