mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-07-11 14:44:36 +00:00
Bullet Library v2.81
This commit is contained in:
LuisAntonRebollo
parent
64fef8b2ad
commit
1eb94f4828
462 changed files with 59613 additions and 8036 deletions
|
|
@ -24,11 +24,11 @@ subject to the following restrictions:
|
|||
|
||||
#include <math.h>
|
||||
|
||||
//#include "BulletMultiThreaded/vectormath/scalar/cpp/vectormath_aos.h"
|
||||
#include <vectormath_aos.h>
|
||||
|
||||
#include "../PlatformDefinitions.h"
|
||||
|
||||
|
||||
|
||||
using namespace Vectormath::Aos;
|
||||
|
||||
enum FeatureType { F, E, V };
|
||||
|
||||
|
|
@ -39,21 +39,21 @@ enum FeatureType { F, E, V };
|
|||
class Box
|
||||
{
|
||||
public:
|
||||
Vector3 half;
|
||||
vmVector3 mHalf;
|
||||
|
||||
inline Box()
|
||||
{}
|
||||
inline Box(PE_REF(Vector3) half_);
|
||||
inline Box(PE_REF(vmVector3) half_);
|
||||
inline Box(float hx, float hy, float hz);
|
||||
|
||||
inline void Set(PE_REF(Vector3) half_);
|
||||
inline void Set(PE_REF(vmVector3) half_);
|
||||
inline void Set(float hx, float hy, float hz);
|
||||
|
||||
inline Vector3 GetAABB(const Matrix3& rotation) const;
|
||||
inline vmVector3 GetAABB(const vmMatrix3& rotation) const;
|
||||
};
|
||||
|
||||
inline
|
||||
Box::Box(PE_REF(Vector3) half_)
|
||||
Box::Box(PE_REF(vmVector3) half_)
|
||||
{
|
||||
Set(half_);
|
||||
}
|
||||
|
|
@ -66,23 +66,23 @@ Box::Box(float hx, float hy, float hz)
|
|||
|
||||
inline
|
||||
void
|
||||
Box::Set(PE_REF(Vector3) half_)
|
||||
Box::Set(PE_REF(vmVector3) half_)
|
||||
{
|
||||
half = half_;
|
||||
mHalf = half_;
|
||||
}
|
||||
|
||||
inline
|
||||
void
|
||||
Box::Set(float hx, float hy, float hz)
|
||||
{
|
||||
half = Vector3(hx, hy, hz);
|
||||
mHalf = vmVector3(hx, hy, hz);
|
||||
}
|
||||
|
||||
inline
|
||||
Vector3
|
||||
Box::GetAABB(const Matrix3& rotation) const
|
||||
vmVector3
|
||||
Box::GetAABB(const vmMatrix3& rotation) const
|
||||
{
|
||||
return absPerElem(rotation) * half;
|
||||
return absPerElem(rotation) * mHalf;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------------
|
||||
|
|
@ -95,7 +95,7 @@ class BoxPoint
|
|||
public:
|
||||
BoxPoint() : localPoint(0.0f) {}
|
||||
|
||||
Point3 localPoint;
|
||||
vmPoint3 localPoint;
|
||||
FeatureType featureType;
|
||||
int featureIdx;
|
||||
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape*
|
|||
const btTransform& t = xform;
|
||||
btMatrix3x3 abs_b = t.getBasis().absolute();
|
||||
btVector3 center = t.getOrigin();
|
||||
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
|
||||
btVector3 extent = halfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
|
||||
|
||||
aabbMin = center - extent;
|
||||
aabbMax = center + extent;
|
||||
|
|
@ -67,7 +67,7 @@ void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape*
|
|||
const btTransform& t = xform;
|
||||
btMatrix3x3 abs_b = t.getBasis().absolute();
|
||||
btVector3 center = t.getOrigin();
|
||||
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
|
||||
btVector3 extent = halfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
|
||||
|
||||
aabbMin = center - extent;
|
||||
aabbMax = center + extent;
|
||||
|
|
@ -198,6 +198,12 @@ int getShapeTypeSize(int shapeType)
|
|||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
case STATIC_PLANE_PROXYTYPE:
|
||||
{
|
||||
int shapeSize = sizeof(btStaticPlaneShape);
|
||||
btAssert(shapeSize < MAX_SHAPE_SIZE);
|
||||
return shapeSize;
|
||||
}
|
||||
|
||||
default:
|
||||
btAssert(0);
|
||||
|
|
@ -225,6 +231,7 @@ void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionSha
|
|||
{
|
||||
register int dmaSize = getShapeTypeSize(shapeType);
|
||||
cellDmaGet(collisionShapeLocation, collisionShapePtr , dmaSize, DMA_TAG(dmaTag), 0, 0);
|
||||
//cellDmaGetReadOnly(collisionShapeLocation, collisionShapePtr , dmaSize, DMA_TAG(dmaTag), 0, 0);
|
||||
//cellDmaWaitTagStatusAll(DMA_MASK(dmaTag));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ subject to the following restrictions:
|
|||
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
|
||||
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
|
||||
|
|
@ -32,7 +33,9 @@ subject to the following restrictions:
|
|||
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
|
||||
#define MAX_NUM_SPU_CONVEX_POINTS 128
|
||||
#define MAX_NUM_SPU_CONVEX_POINTS 128 //@fallback to PPU if a btConvexHullShape has more than MAX_NUM_SPU_CONVEX_POINTS points
|
||||
#define MAX_SPU_COMPOUND_SUBSHAPES 16 //@fallback on PPU if compound has more than MAX_SPU_COMPOUND_SUBSHAPES child shapes
|
||||
#define MAX_SHAPE_SIZE 256 //@todo: assert on this
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) SpuConvexPolyhedronVertexData
|
||||
{
|
||||
|
|
@ -43,7 +46,7 @@ ATTRIBUTE_ALIGNED16(struct) SpuConvexPolyhedronVertexData
|
|||
ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]);
|
||||
};
|
||||
|
||||
#define MAX_SHAPE_SIZE 256
|
||||
|
||||
|
||||
ATTRIBUTE_ALIGNED16(struct) CollisionShape_LocalStoreMemory
|
||||
{
|
||||
|
|
@ -53,7 +56,7 @@ ATTRIBUTE_ALIGNED16(struct) CollisionShape_LocalStoreMemory
|
|||
ATTRIBUTE_ALIGNED16(struct) CompoundShape_LocalStoreMemory
|
||||
{
|
||||
// Compound data
|
||||
#define MAX_SPU_COMPOUND_SUBSHAPES 16
|
||||
|
||||
ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]);
|
||||
ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -17,6 +17,12 @@ subject to the following restrictions:
|
|||
|
||||
//#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()
|
||||
{
|
||||
|
|
@ -99,50 +105,50 @@ bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
|
|||
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;
|
||||
//if (depth > manifoldPtr->getContactProcessingThreshold())
|
||||
// return false;
|
||||
|
||||
|
||||
|
||||
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);*/
|
||||
localA = transA.invXform(pointA );
|
||||
localB = transB.invXform(pointInWorld);
|
||||
}
|
||||
else
|
||||
{
|
||||
normal = normalOnBInWorld;
|
||||
pointA = pointInWorld + normal * depth;
|
||||
localA = transAInv(pointA );
|
||||
localB = transBInv(pointInWorld);
|
||||
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)
|
||||
{
|
||||
// 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
|
||||
// we need to replace the current contact point, otherwise small errors will accumulate (spheres start rolling etc)
|
||||
manifoldPtr->replaceContactPoint(newPt,insertIndex);
|
||||
return true;
|
||||
|
||||
} else
|
||||
{
|
||||
|
||||
newPt.m_combinedFriction = combinedFriction;
|
||||
newPt.m_combinedRestitution = combinedRestitution;
|
||||
|
||||
/*
|
||||
///@todo: SPU callbacks, either immediate (local on the SPU), or deferred
|
||||
//User can override friction and/or restitution
|
||||
|
|
@ -155,6 +161,7 @@ bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
|
|||
(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
|
||||
}
|
||||
*/
|
||||
|
||||
manifoldPtr->addManifoldPoint(newPt);
|
||||
return true;
|
||||
|
||||
|
|
@ -181,7 +188,12 @@ void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManif
|
|||
|
||||
void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
|
||||
{
|
||||
//spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",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);
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ subject to the following restrictions:
|
|||
#define SPU_CONTACT_RESULT2_H
|
||||
|
||||
|
||||
#ifndef WIN32
|
||||
#ifndef _WIN32
|
||||
#include <stdint.h>
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ class btStackAlloc;
|
|||
class btIDebugDraw;
|
||||
#include "BulletCollision/NarrowphaseCollision/btConvexPenetrationDepthSolver.h"
|
||||
|
||||
#include <LinearMath/btTransform.h>
|
||||
#include "LinearMath/btTransform.h"
|
||||
|
||||
|
||||
///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ subject to the following restrictions:
|
|||
#ifdef __SPU__
|
||||
///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases
|
||||
#ifndef USE_LIBSPE2
|
||||
#define USE_SOFTWARE_CACHE 1
|
||||
//#define USE_SOFTWARE_CACHE 1
|
||||
#endif
|
||||
#endif //__SPU__
|
||||
|
||||
|
|
@ -190,10 +190,27 @@ void* createCollisionLocalStoreMemory()
|
|||
{
|
||||
return &gLocalStoreMemory;
|
||||
}
|
||||
void deleteCollisionLocalStoreMemory()
|
||||
{
|
||||
}
|
||||
#else
|
||||
|
||||
btAlignedObjectArray<CollisionTask_LocalStoreMemory*> sLocalStorePointers;
|
||||
|
||||
void* createCollisionLocalStoreMemory()
|
||||
{
|
||||
return new CollisionTask_LocalStoreMemory;
|
||||
CollisionTask_LocalStoreMemory* localStore = new CollisionTask_LocalStoreMemory;
|
||||
sLocalStorePointers.push_back(localStore);
|
||||
return localStore;
|
||||
}
|
||||
|
||||
void deleteCollisionLocalStoreMemory()
|
||||
{
|
||||
for (int i=0;i<sLocalStorePointers.size();i++)
|
||||
{
|
||||
delete sLocalStorePointers[i];
|
||||
}
|
||||
sLocalStorePointers.clear();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -372,6 +389,151 @@ public:
|
|||
};
|
||||
|
||||
|
||||
|
||||
void btConvexPlaneCollideSingleContact (SpuCollisionPairInput* wuInput,CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts)
|
||||
{
|
||||
|
||||
btConvexShape* convexShape = (btConvexShape*) wuInput->m_spuCollisionShapes[0];
|
||||
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) wuInput->m_spuCollisionShapes[1];
|
||||
|
||||
bool hasCollision = false;
|
||||
const btVector3& planeNormal = planeShape->getPlaneNormal();
|
||||
const btScalar& planeConstant = planeShape->getPlaneConstant();
|
||||
|
||||
|
||||
btTransform convexWorldTransform = wuInput->m_worldTransform0;
|
||||
btTransform convexInPlaneTrans;
|
||||
convexInPlaneTrans= wuInput->m_worldTransform1.inverse() * convexWorldTransform;
|
||||
btTransform planeInConvex;
|
||||
planeInConvex= convexWorldTransform.inverse() * wuInput->m_worldTransform1;
|
||||
|
||||
//btVector3 vtx = convexShape->localGetSupportVertexWithoutMarginNonVirtual(planeInConvex.getBasis()*-planeNormal);
|
||||
btVector3 vtx = convexShape->localGetSupportVertexNonVirtual(planeInConvex.getBasis()*-planeNormal);
|
||||
|
||||
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
|
||||
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
|
||||
|
||||
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
|
||||
btVector3 vtxInPlaneWorld = wuInput->m_worldTransform1 * vtxInPlaneProjected;
|
||||
|
||||
hasCollision = distance < lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold();
|
||||
//resultOut->setPersistentManifold(m_manifoldPtr);
|
||||
if (hasCollision)
|
||||
{
|
||||
/// report a contact. internally this will be kept persistent, and contact reduction is done
|
||||
btVector3 normalOnSurfaceB =wuInput->m_worldTransform1.getBasis() * planeNormal;
|
||||
btVector3 pOnB = vtxInPlaneWorld;
|
||||
spuContacts.addContactPoint(normalOnSurfaceB,pOnB,distance);
|
||||
}
|
||||
}
|
||||
|
||||
void ProcessConvexPlaneSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)
|
||||
{
|
||||
|
||||
register int dmaSize = 0;
|
||||
register ppu_address_t dmaPpuAddress2;
|
||||
btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;
|
||||
|
||||
///DMA in the vertices for convex shapes
|
||||
ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);
|
||||
ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);
|
||||
|
||||
if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )
|
||||
{
|
||||
// spu_printf("SPU: DMA btConvexHullShape\n");
|
||||
|
||||
dmaSize = sizeof(btConvexHullShape);
|
||||
dmaPpuAddress2 = wuInput->m_collisionShapes[0];
|
||||
|
||||
cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
|
||||
//cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
}
|
||||
|
||||
if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
|
||||
{
|
||||
// spu_printf("SPU: DMA btConvexHullShape\n");
|
||||
dmaSize = sizeof(btConvexHullShape);
|
||||
dmaPpuAddress2 = wuInput->m_collisionShapes[1];
|
||||
cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);
|
||||
//cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
}
|
||||
|
||||
if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
|
||||
{
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);
|
||||
lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];
|
||||
}
|
||||
|
||||
|
||||
if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
|
||||
{
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);
|
||||
lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];
|
||||
}
|
||||
|
||||
|
||||
btConvexPointCloudShape cpc0,cpc1;
|
||||
|
||||
if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
|
||||
{
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(2));
|
||||
lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];
|
||||
btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0];
|
||||
const btVector3& localScaling = ch->getLocalScalingNV();
|
||||
cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling);
|
||||
wuInput->m_spuCollisionShapes[0] = &cpc0;
|
||||
}
|
||||
|
||||
if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )
|
||||
{
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(2));
|
||||
lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];
|
||||
btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1];
|
||||
const btVector3& localScaling = ch->getLocalScalingNV();
|
||||
cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling);
|
||||
wuInput->m_spuCollisionShapes[1] = &cpc1;
|
||||
|
||||
}
|
||||
|
||||
|
||||
// const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0];
|
||||
// const btConvexShape* shape1Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[1];
|
||||
// int shapeType0 = wuInput->m_shapeType0;
|
||||
// int shapeType1 = wuInput->m_shapeType1;
|
||||
float marginA = wuInput->m_collisionMargin0;
|
||||
float marginB = wuInput->m_collisionMargin1;
|
||||
|
||||
SpuClosestPointInput cpInput;
|
||||
cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];
|
||||
cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];
|
||||
cpInput.m_transformA = wuInput->m_worldTransform0;
|
||||
cpInput.m_transformB = wuInput->m_worldTransform1;
|
||||
float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold());
|
||||
cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;
|
||||
|
||||
ppu_address_t manifoldAddress = (ppu_address_t)manifold;
|
||||
|
||||
btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr();
|
||||
//spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);
|
||||
spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),
|
||||
lsMemPtr->getColObj1()->getWorldTransform(),
|
||||
lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),
|
||||
lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),
|
||||
wuInput->m_isSwapped);
|
||||
|
||||
|
||||
btConvexPlaneCollideSingleContact(wuInput,lsMemPtr,spuContacts);
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
////////////////////////
|
||||
/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc)
|
||||
///////////////////
|
||||
|
|
@ -476,8 +638,9 @@ void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionT
|
|||
}
|
||||
|
||||
|
||||
int stats[11]={0,0,0,0,0,0,0,0,0,0,0};
|
||||
int degenerateStats[11]={0,0,0,0,0,0,0,0,0,0,0};
|
||||
#define MAX_DEGENERATE_STATS 15
|
||||
int stats[MAX_DEGENERATE_STATS]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
||||
int degenerateStats[MAX_DEGENERATE_STATS]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
||||
|
||||
|
||||
////////////////////////
|
||||
|
|
@ -613,8 +776,10 @@ void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTa
|
|||
{
|
||||
btGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&simplexSolver,penetrationSolver);//&vsSolver,penetrationSolver);
|
||||
gjk.getClosestPoints(cpInput,spuContacts,0);//,debugDraw);
|
||||
|
||||
|
||||
btAssert(gjk.m_lastUsedMethod <MAX_DEGENERATE_STATS);
|
||||
stats[gjk.m_lastUsedMethod]++;
|
||||
btAssert(gjk.m_degenerateSimplex <MAX_DEGENERATE_STATS);
|
||||
degenerateStats[gjk.m_degenerateSimplex]++;
|
||||
|
||||
#ifdef USE_SEPDISTANCE_UTIL
|
||||
|
|
@ -719,16 +884,21 @@ void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTas
|
|||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
|
||||
int childShapeCount0 = spuCompoundShape0->getNumChildShapes();
|
||||
btAssert(childShapeCount0< MAX_SPU_COMPOUND_SUBSHAPES);
|
||||
int childShapeCount1 = spuCompoundShape1->getNumChildShapes();
|
||||
btAssert(childShapeCount1< MAX_SPU_COMPOUND_SUBSHAPES);
|
||||
|
||||
// Start the N^2
|
||||
for (int i = 0; i < childShapeCount0; ++i)
|
||||
{
|
||||
btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i];
|
||||
btAssert(!btBroadphaseProxy::isCompound(childShape0.m_childShapeType));
|
||||
|
||||
for (int j = 0; j < childShapeCount1; ++j)
|
||||
{
|
||||
btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j];
|
||||
btAssert(!btBroadphaseProxy::isCompound(childShape1.m_childShapeType));
|
||||
|
||||
|
||||
/* Create a new collision pair input struct using the two child shapes */
|
||||
SpuCollisionPairInput cinput (collisionPairInput);
|
||||
|
|
@ -741,9 +911,10 @@ void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTas
|
|||
cinput.m_shapeType1 = childShape1.m_childShapeType;
|
||||
cinput.m_collisionMargin1 = childShape1.m_childMargin;
|
||||
/* Recursively call handleCollisionPair () with new collision pair input */
|
||||
|
||||
handleCollisionPair(cinput, lsMem, spuContacts,
|
||||
(ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i],
|
||||
(ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false); // bug fix: changed index to j.
|
||||
(ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -761,11 +932,12 @@ void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTas
|
|||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
|
||||
int childShapeCount = spuCompoundShape->getNumChildShapes();
|
||||
btAssert(childShapeCount< MAX_SPU_COMPOUND_SUBSHAPES);
|
||||
|
||||
for (int i = 0; i < childShapeCount; ++i)
|
||||
{
|
||||
btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];
|
||||
|
||||
btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType));
|
||||
// Dma the child shape
|
||||
dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
|
|
@ -793,10 +965,13 @@ void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTas
|
|||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
|
||||
int childShapeCount = spuCompoundShape->getNumChildShapes();
|
||||
btAssert(childShapeCount< MAX_SPU_COMPOUND_SUBSHAPES);
|
||||
|
||||
|
||||
for (int i = 0; i < childShapeCount; ++i)
|
||||
{
|
||||
btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];
|
||||
btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType));
|
||||
// Dma the child shape
|
||||
dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);
|
||||
cellDmaWaitTagStatusAll(DMA_MASK(1));
|
||||
|
|
@ -845,17 +1020,33 @@ void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTas
|
|||
cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));
|
||||
}
|
||||
|
||||
btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
|
||||
btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc;
|
||||
if (collisionPairInput.m_shapeType1 == STATIC_PLANE_PROXYTYPE)
|
||||
{
|
||||
btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
|
||||
btStaticPlaneShape* planeShape= (btStaticPlaneShape*)collisionShape1Loc;
|
||||
|
||||
btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
|
||||
collisionPairInput.m_primitiveDimensions0 = dim0;
|
||||
collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
|
||||
collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
|
||||
collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
|
||||
collisionPairInput.m_spuCollisionShapes[1] = trimeshShape;
|
||||
btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
|
||||
collisionPairInput.m_primitiveDimensions0 = dim0;
|
||||
collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
|
||||
collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
|
||||
collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
|
||||
collisionPairInput.m_spuCollisionShapes[1] = planeShape;
|
||||
|
||||
ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts);
|
||||
ProcessConvexPlaneSpuCollision(&collisionPairInput,&lsMem,spuContacts);
|
||||
} else
|
||||
{
|
||||
btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;
|
||||
btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc;
|
||||
|
||||
btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();
|
||||
collisionPairInput.m_primitiveDimensions0 = dim0;
|
||||
collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;
|
||||
collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;
|
||||
collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;
|
||||
collisionPairInput.m_spuCollisionShapes[1] = trimeshShape;
|
||||
|
||||
ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1033,7 +1224,7 @@ void processCollisionTask(void* userPtr, void* lsMemPtr)
|
|||
collisionPairInput.m_isSwapped);
|
||||
|
||||
|
||||
float distance=0.f;
|
||||
//float distance=0.f;
|
||||
btVector3 normalInB;
|
||||
|
||||
|
||||
|
|
@ -1054,38 +1245,64 @@ void processCollisionTask(void* userPtr, void* lsMemPtr)
|
|||
btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();
|
||||
btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0);
|
||||
btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1);
|
||||
/*
|
||||
//Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());
|
||||
vmVector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin());
|
||||
vmVector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin());
|
||||
vmMatrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis());
|
||||
vmMatrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis());
|
||||
|
||||
Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());
|
||||
Vector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin());
|
||||
Vector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin());
|
||||
Matrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis());
|
||||
Matrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis());
|
||||
|
||||
Transform3 transformA(vmMatrix0,vmPos0);
|
||||
vmTransform3 transformA(vmMatrix0,vmPos0);
|
||||
Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());
|
||||
Transform3 transformB(vmMatrix1,vmPos1);
|
||||
vmTransform3 transformB(vmMatrix1,vmPos1);
|
||||
BoxPoint resultClosestBoxPointA;
|
||||
BoxPoint resultClosestBoxPointB;
|
||||
Vector3 resultNormal;
|
||||
vmVector3 resultNormal;
|
||||
*/
|
||||
|
||||
#ifdef USE_SEPDISTANCE_UTIL
|
||||
float distanceThreshold = FLT_MAX
|
||||
#else
|
||||
float distanceThreshold = 0.f;
|
||||
//float distanceThreshold = 0.f;
|
||||
#endif
|
||||
|
||||
|
||||
distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB, boxA, transformA, boxB,transformB,distanceThreshold);
|
||||
vmVector3 n;
|
||||
Box boxA;
|
||||
vmVector3 hA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());
|
||||
vmVector3 hB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());
|
||||
boxA.mHalf= hA;
|
||||
vmTransform3 trA;
|
||||
trA.setTranslation(getVmVector3(collisionPairInput.m_worldTransform0.getOrigin()));
|
||||
trA.setUpper3x3(getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis()));
|
||||
Box boxB;
|
||||
boxB.mHalf = hB;
|
||||
vmTransform3 trB;
|
||||
trB.setTranslation(getVmVector3(collisionPairInput.m_worldTransform1.getOrigin()));
|
||||
trB.setUpper3x3(getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis()));
|
||||
|
||||
normalInB = -getBtVector3(resultNormal);
|
||||
float distanceThreshold = spuManifold->getContactBreakingThreshold();//0.001f;
|
||||
|
||||
if(distance < spuManifold->getContactBreakingThreshold())
|
||||
|
||||
BoxPoint ptA,ptB;
|
||||
float dist = boxBoxDistance(n, ptA, ptB,
|
||||
boxA, trA, boxB, trB,
|
||||
distanceThreshold );
|
||||
|
||||
|
||||
// float distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB, boxA, transformA, boxB,transformB,distanceThreshold);
|
||||
|
||||
normalInB = -getBtVector3(n);//resultNormal);
|
||||
|
||||
//if(dist < distanceThreshold)//spuManifold->getContactBreakingThreshold())
|
||||
if(dist < spuManifold->getContactBreakingThreshold())
|
||||
{
|
||||
btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(resultClosestBoxPointB.localPoint));
|
||||
btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(ptB.localPoint));
|
||||
|
||||
spuContacts.addContactPoint(
|
||||
normalInB,
|
||||
pointOnB,
|
||||
distance);
|
||||
dist);
|
||||
}
|
||||
}
|
||||
#else
|
||||
|
|
@ -1163,7 +1380,9 @@ void processCollisionTask(void* userPtr, void* lsMemPtr)
|
|||
#endif //USE_SEPDISTANCE_UTIL
|
||||
)
|
||||
{
|
||||
handleCollisionPair(collisionPairInput, lsMem, spuContacts, (ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape, (ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);
|
||||
handleCollisionPair(collisionPairInput, lsMem, spuContacts,
|
||||
(ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape,
|
||||
(ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);
|
||||
} else
|
||||
{
|
||||
//spu_printf("boxbox dist = %f\n",distance);
|
||||
|
|
@ -1209,3 +1428,5 @@ void processCollisionTask(void* userPtr, void* lsMemPtr)
|
|||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ __attribute__ ((aligned (128)))
|
|||
void processCollisionTask(void* userPtr, void* lsMemory);
|
||||
|
||||
void* createCollisionLocalStoreMemory();
|
||||
|
||||
void deleteCollisionLocalStoreMemory();
|
||||
|
||||
#if defined(USE_LIBSPE2) && defined(__SPU__)
|
||||
#include "../SpuLibspe2Support.h"
|
||||
|
|
|
|||
|
|
@ -15,7 +15,11 @@ subject to the following restrictions:
|
|||
*/
|
||||
|
||||
|
||||
#include "Box.h"
|
||||
//#include "PfxContactBoxBox.h"
|
||||
|
||||
#include <math.h>
|
||||
#include "../PlatformDefinitions.h"
|
||||
#include "boxBoxDistance.h"
|
||||
|
||||
static inline float sqr( float a )
|
||||
{
|
||||
|
|
@ -114,18 +118,18 @@ VertexBFaceATest(
|
|||
bool & inVoronoi,
|
||||
float & t0,
|
||||
float & t1,
|
||||
const Vector3 & hA,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsB,
|
||||
PE_REF(Vector3) scalesB )
|
||||
const vmVector3 & hA,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsB,
|
||||
PE_REF(vmVector3) scalesB )
|
||||
{
|
||||
// compute a corner of box B in A's coordinate system
|
||||
|
||||
Vector3 corner =
|
||||
Vector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );
|
||||
vmVector3 corner =
|
||||
vmVector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );
|
||||
|
||||
// compute the parameters of the point on A, closest to this corner
|
||||
|
||||
|
|
@ -144,8 +148,8 @@ VertexBFaceATest(
|
|||
// do the Voronoi test: already know the point on B is in the Voronoi region of the
|
||||
// point on A, check the reverse.
|
||||
|
||||
Vector3 facePointB =
|
||||
Vector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) );
|
||||
vmVector3 facePointB =
|
||||
vmVector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) );
|
||||
|
||||
inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) &&
|
||||
( facePointB[1] >= voronoiTol * facePointB[0] ) &&
|
||||
|
|
@ -169,17 +173,17 @@ void
|
|||
VertexBFaceATests(
|
||||
bool & done,
|
||||
float & minDistSqr,
|
||||
Point3 & localPointA,
|
||||
Point3 & localPointB,
|
||||
vmPoint3 & localPointA,
|
||||
vmPoint3 & localPointB,
|
||||
FeatureType & featureA,
|
||||
FeatureType & featureB,
|
||||
const Vector3 & hA,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsB,
|
||||
PE_REF(Vector3) scalesB,
|
||||
const vmVector3 & hA,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsB,
|
||||
PE_REF(vmVector3) scalesB,
|
||||
bool first )
|
||||
{
|
||||
|
||||
|
|
@ -247,16 +251,16 @@ VertexAFaceBTest(
|
|||
bool & inVoronoi,
|
||||
float & t0,
|
||||
float & t1,
|
||||
const Vector3 & hB,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsA,
|
||||
PE_REF(Vector3) scalesA )
|
||||
const vmVector3 & hB,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsA,
|
||||
PE_REF(vmVector3) scalesA )
|
||||
{
|
||||
Vector3 corner =
|
||||
Vector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() );
|
||||
vmVector3 corner =
|
||||
vmVector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() );
|
||||
|
||||
t0 = corner[0];
|
||||
t1 = corner[1];
|
||||
|
|
@ -270,8 +274,8 @@ VertexAFaceBTest(
|
|||
else if ( t1 < -hB[1] )
|
||||
t1 = -hB[1];
|
||||
|
||||
Vector3 facePointA =
|
||||
Vector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) );
|
||||
vmVector3 facePointA =
|
||||
vmVector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) );
|
||||
|
||||
inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) &&
|
||||
( facePointA[1] >= voronoiTol * facePointA[0] ) &&
|
||||
|
|
@ -295,17 +299,17 @@ void
|
|||
VertexAFaceBTests(
|
||||
bool & done,
|
||||
float & minDistSqr,
|
||||
Point3 & localPointA,
|
||||
Point3 & localPointB,
|
||||
vmPoint3 & localPointA,
|
||||
vmPoint3 & localPointB,
|
||||
FeatureType & featureA,
|
||||
FeatureType & featureB,
|
||||
const Vector3 & hB,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsA,
|
||||
PE_REF(Vector3) scalesA,
|
||||
const vmVector3 & hB,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsA,
|
||||
PE_REF(vmVector3) scalesA,
|
||||
bool first )
|
||||
{
|
||||
float t0, t1;
|
||||
|
|
@ -363,7 +367,7 @@ VertexAFaceBTests(
|
|||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------------
|
||||
// EdgeEdgeTest:
|
||||
// CustomEdgeEdgeTest:
|
||||
//
|
||||
// tests whether a pair of edges are the closest features
|
||||
//
|
||||
|
|
@ -374,10 +378,10 @@ VertexAFaceBTests(
|
|||
// the dimension of the face normal is 2
|
||||
//-------------------------------------------------------------------------------------------------
|
||||
|
||||
#define EdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \
|
||||
#define CustomEdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \
|
||||
{ \
|
||||
Vector3 edgeOffsetAB; \
|
||||
Vector3 edgeOffsetBA; \
|
||||
vmVector3 edgeOffsetAB; \
|
||||
vmVector3 edgeOffsetBA; \
|
||||
\
|
||||
edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \
|
||||
edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \
|
||||
|
|
@ -421,8 +425,8 @@ VertexAFaceBTests(
|
|||
else if ( tA > hA[ad] ) tA = hA[ad]; \
|
||||
} \
|
||||
\
|
||||
Vector3 edgeOffAB = Vector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\
|
||||
Vector3 edgeOffBA = Vector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\
|
||||
vmVector3 edgeOffAB = vmVector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\
|
||||
vmVector3 edgeOffBA = vmVector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\
|
||||
\
|
||||
inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \
|
||||
( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \
|
||||
|
|
@ -436,79 +440,79 @@ VertexAFaceBTests(
|
|||
}
|
||||
|
||||
float
|
||||
EdgeEdgeTest_0101(
|
||||
CustomEdgeEdgeTest_0101(
|
||||
bool & inVoronoi,
|
||||
float & tA,
|
||||
float & tB,
|
||||
const Vector3 & hA,
|
||||
const Vector3 & hB,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsA,
|
||||
PE_REF(Vector3) signsB,
|
||||
PE_REF(Vector3) scalesA,
|
||||
PE_REF(Vector3) scalesB )
|
||||
const vmVector3 & hA,
|
||||
const vmVector3 & hB,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsA,
|
||||
PE_REF(vmVector3) signsB,
|
||||
PE_REF(vmVector3) scalesA,
|
||||
PE_REF(vmVector3) scalesB )
|
||||
{
|
||||
EdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );
|
||||
CustomEdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );
|
||||
}
|
||||
|
||||
float
|
||||
EdgeEdgeTest_0110(
|
||||
CustomEdgeEdgeTest_0110(
|
||||
bool & inVoronoi,
|
||||
float & tA,
|
||||
float & tB,
|
||||
const Vector3 & hA,
|
||||
const Vector3 & hB,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsA,
|
||||
PE_REF(Vector3) signsB,
|
||||
PE_REF(Vector3) scalesA,
|
||||
PE_REF(Vector3) scalesB )
|
||||
const vmVector3 & hA,
|
||||
const vmVector3 & hB,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsA,
|
||||
PE_REF(vmVector3) signsB,
|
||||
PE_REF(vmVector3) scalesA,
|
||||
PE_REF(vmVector3) scalesB )
|
||||
{
|
||||
EdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );
|
||||
CustomEdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );
|
||||
}
|
||||
|
||||
float
|
||||
EdgeEdgeTest_1001(
|
||||
CustomEdgeEdgeTest_1001(
|
||||
bool & inVoronoi,
|
||||
float & tA,
|
||||
float & tB,
|
||||
const Vector3 & hA,
|
||||
const Vector3 & hB,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsA,
|
||||
PE_REF(Vector3) signsB,
|
||||
PE_REF(Vector3) scalesA,
|
||||
PE_REF(Vector3) scalesB )
|
||||
const vmVector3 & hA,
|
||||
const vmVector3 & hB,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsA,
|
||||
PE_REF(vmVector3) signsB,
|
||||
PE_REF(vmVector3) scalesA,
|
||||
PE_REF(vmVector3) scalesB )
|
||||
{
|
||||
EdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );
|
||||
CustomEdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );
|
||||
}
|
||||
|
||||
float
|
||||
EdgeEdgeTest_1010(
|
||||
CustomEdgeEdgeTest_1010(
|
||||
bool & inVoronoi,
|
||||
float & tA,
|
||||
float & tB,
|
||||
const Vector3 & hA,
|
||||
const Vector3 & hB,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsA,
|
||||
PE_REF(Vector3) signsB,
|
||||
PE_REF(Vector3) scalesA,
|
||||
PE_REF(Vector3) scalesB )
|
||||
const vmVector3 & hA,
|
||||
const vmVector3 & hB,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsA,
|
||||
PE_REF(vmVector3) signsB,
|
||||
PE_REF(vmVector3) scalesA,
|
||||
PE_REF(vmVector3) scalesB )
|
||||
{
|
||||
EdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X );
|
||||
CustomEdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X );
|
||||
}
|
||||
|
||||
#define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \
|
||||
|
|
@ -528,22 +532,22 @@ void
|
|||
EdgeEdgeTests(
|
||||
bool & done,
|
||||
float & minDistSqr,
|
||||
Point3 & localPointA,
|
||||
Point3 & localPointB,
|
||||
vmPoint3 & localPointA,
|
||||
vmPoint3 & localPointB,
|
||||
int & otherFaceDimA,
|
||||
int & otherFaceDimB,
|
||||
FeatureType & featureA,
|
||||
FeatureType & featureB,
|
||||
const Vector3 & hA,
|
||||
const Vector3 & hB,
|
||||
PE_REF(Vector3) faceOffsetAB,
|
||||
PE_REF(Vector3) faceOffsetBA,
|
||||
const Matrix3 & matrixAB,
|
||||
const Matrix3 & matrixBA,
|
||||
PE_REF(Vector3) signsA,
|
||||
PE_REF(Vector3) signsB,
|
||||
PE_REF(Vector3) scalesA,
|
||||
PE_REF(Vector3) scalesB,
|
||||
const vmVector3 & hA,
|
||||
const vmVector3 & hB,
|
||||
PE_REF(vmVector3) faceOffsetAB,
|
||||
PE_REF(vmVector3) faceOffsetBA,
|
||||
const vmMatrix3 & matrixAB,
|
||||
const vmMatrix3 & matrixBA,
|
||||
PE_REF(vmVector3) signsA,
|
||||
PE_REF(vmVector3) signsB,
|
||||
PE_REF(vmVector3) scalesA,
|
||||
PE_REF(vmVector3) scalesB,
|
||||
bool first )
|
||||
{
|
||||
|
||||
|
|
@ -555,7 +559,7 @@ EdgeEdgeTests(
|
|||
testOtherFaceDimA = 0;
|
||||
testOtherFaceDimB = 0;
|
||||
|
||||
distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( first ) {
|
||||
|
|
@ -572,7 +576,7 @@ EdgeEdgeTests(
|
|||
signsA.setX( -signsA.getX() );
|
||||
scalesA.setX( -scalesA.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -585,7 +589,7 @@ EdgeEdgeTests(
|
|||
signsB.setX( -signsB.getX() );
|
||||
scalesB.setX( -scalesB.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -598,7 +602,7 @@ EdgeEdgeTests(
|
|||
signsA.setX( -signsA.getX() );
|
||||
scalesA.setX( -scalesA.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -613,7 +617,7 @@ EdgeEdgeTests(
|
|||
signsB.setX( -signsB.getX() );
|
||||
scalesB.setX( -scalesB.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -626,7 +630,7 @@ EdgeEdgeTests(
|
|||
signsA.setY( -signsA.getY() );
|
||||
scalesA.setY( -scalesA.getY() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -639,7 +643,7 @@ EdgeEdgeTests(
|
|||
signsB.setX( -signsB.getX() );
|
||||
scalesB.setX( -scalesB.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -652,7 +656,7 @@ EdgeEdgeTests(
|
|||
signsA.setY( -signsA.getY() );
|
||||
scalesA.setY( -scalesA.getY() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -667,7 +671,7 @@ EdgeEdgeTests(
|
|||
signsB.setX( -signsB.getX() );
|
||||
scalesB.setX( -scalesB.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -680,7 +684,7 @@ EdgeEdgeTests(
|
|||
signsA.setX( -signsA.getX() );
|
||||
scalesA.setX( -scalesA.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -693,7 +697,7 @@ EdgeEdgeTests(
|
|||
signsB.setY( -signsB.getY() );
|
||||
scalesB.setY( -scalesB.getY() );
|
||||
|
||||
distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -706,7 +710,7 @@ EdgeEdgeTests(
|
|||
signsA.setX( -signsA.getX() );
|
||||
scalesA.setX( -scalesA.getX() );
|
||||
|
||||
distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -721,7 +725,7 @@ EdgeEdgeTests(
|
|||
signsB.setY( -signsB.getY() );
|
||||
scalesB.setY( -scalesB.getY() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -734,7 +738,7 @@ EdgeEdgeTests(
|
|||
signsA.setY( -signsA.getY() );
|
||||
scalesA.setY( -scalesA.getY() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -747,7 +751,7 @@ EdgeEdgeTests(
|
|||
signsB.setY( -signsB.getY() );
|
||||
scalesB.setY( -scalesB.getY() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -760,7 +764,7 @@ EdgeEdgeTests(
|
|||
signsA.setY( -signsA.getY() );
|
||||
scalesA.setY( -scalesA.getY() );
|
||||
|
||||
distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,
|
||||
matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );
|
||||
|
||||
if ( distSqr < minDistSqr ) {
|
||||
|
|
@ -768,27 +772,25 @@ EdgeEdgeTests(
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
float
|
||||
boxBoxDistance(
|
||||
Vector3& normal,
|
||||
BoxPoint& boxPointA,
|
||||
BoxPoint& boxPointB,
|
||||
PE_REF(Box) boxA, const Transform3& transformA,
|
||||
PE_REF(Box) boxB, const Transform3& transformB,
|
||||
float distanceThreshold )
|
||||
boxBoxDistance(vmVector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
|
||||
PE_REF(Box) boxA, const vmTransform3 & transformA, PE_REF(Box) boxB,
|
||||
const vmTransform3 & transformB,
|
||||
float distanceThreshold)
|
||||
{
|
||||
Matrix3 identity;
|
||||
identity = Matrix3::identity();
|
||||
Vector3 ident[3];
|
||||
vmMatrix3 identity;
|
||||
identity = vmMatrix3::identity();
|
||||
vmVector3 ident[3];
|
||||
ident[0] = identity.getCol0();
|
||||
ident[1] = identity.getCol1();
|
||||
ident[2] = identity.getCol2();
|
||||
|
||||
// get relative transformations
|
||||
|
||||
Transform3 transformAB, transformBA;
|
||||
Matrix3 matrixAB, matrixBA;
|
||||
Vector3 offsetAB, offsetBA;
|
||||
vmTransform3 transformAB, transformBA;
|
||||
vmMatrix3 matrixAB, matrixBA;
|
||||
vmVector3 offsetAB, offsetBA;
|
||||
|
||||
transformAB = orthoInverse(transformA) * transformB;
|
||||
transformBA = orthoInverse(transformAB);
|
||||
|
|
@ -798,25 +800,25 @@ boxBoxDistance(
|
|||
matrixBA = transformBA.getUpper3x3();
|
||||
offsetBA = transformBA.getTranslation();
|
||||
|
||||
Matrix3 absMatrixAB = absPerElem(matrixAB);
|
||||
Matrix3 absMatrixBA = absPerElem(matrixBA);
|
||||
vmMatrix3 absMatrixAB = absPerElem(matrixAB);
|
||||
vmMatrix3 absMatrixBA = absPerElem(matrixBA);
|
||||
|
||||
// find separating axis with largest gap between projections
|
||||
|
||||
BoxSepAxisType axisType;
|
||||
Vector3 axisA(0.0f), axisB(0.0f);
|
||||
vmVector3 axisA(0.0f), axisB(0.0f);
|
||||
float gap, maxGap;
|
||||
int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0;
|
||||
|
||||
// face axes
|
||||
|
||||
Vector3 gapsA = absPerElem(offsetAB) - boxA.half - absMatrixAB * boxB.half;
|
||||
vmVector3 gapsA = absPerElem(offsetAB) - boxA.mHalf - absMatrixAB * boxB.mHalf;
|
||||
|
||||
AaxisTest(0,X,true);
|
||||
AaxisTest(1,Y,false);
|
||||
AaxisTest(2,Z,false);
|
||||
|
||||
Vector3 gapsB = absPerElem(offsetBA) - boxB.half - absMatrixBA * boxA.half;
|
||||
vmVector3 gapsB = absPerElem(offsetBA) - boxB.mHalf - absMatrixBA * boxA.mHalf;
|
||||
|
||||
BaxisTest(0,X);
|
||||
BaxisTest(1,Y);
|
||||
|
|
@ -825,10 +827,10 @@ boxBoxDistance(
|
|||
// cross product axes
|
||||
|
||||
// ŠO<C5A0>Ï‚ª‚O‚Ì‚Æ‚«‚Ì‘Î<E28098>ô
|
||||
absMatrixAB += Matrix3(1.0e-5f);
|
||||
absMatrixBA += Matrix3(1.0e-5f);
|
||||
absMatrixAB += vmMatrix3(1.0e-5f);
|
||||
absMatrixBA += vmMatrix3(1.0e-5f);
|
||||
|
||||
Matrix3 lsqrs, projOffset, projAhalf, projBhalf;
|
||||
vmMatrix3 lsqrs, projOffset, projAhalf, projBhalf;
|
||||
|
||||
lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +
|
||||
mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) );
|
||||
|
|
@ -841,15 +843,15 @@ boxBoxDistance(
|
|||
projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ());
|
||||
projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX());
|
||||
|
||||
projAhalf.setCol0(absMatrixBA.getCol1() * boxA.half.getZ() + absMatrixBA.getCol2() * boxA.half.getY());
|
||||
projAhalf.setCol1(absMatrixBA.getCol2() * boxA.half.getX() + absMatrixBA.getCol0() * boxA.half.getZ());
|
||||
projAhalf.setCol2(absMatrixBA.getCol0() * boxA.half.getY() + absMatrixBA.getCol1() * boxA.half.getX());
|
||||
projAhalf.setCol0(absMatrixBA.getCol1() * boxA.mHalf.getZ() + absMatrixBA.getCol2() * boxA.mHalf.getY());
|
||||
projAhalf.setCol1(absMatrixBA.getCol2() * boxA.mHalf.getX() + absMatrixBA.getCol0() * boxA.mHalf.getZ());
|
||||
projAhalf.setCol2(absMatrixBA.getCol0() * boxA.mHalf.getY() + absMatrixBA.getCol1() * boxA.mHalf.getX());
|
||||
|
||||
projBhalf.setCol0(absMatrixAB.getCol1() * boxB.half.getZ() + absMatrixAB.getCol2() * boxB.half.getY());
|
||||
projBhalf.setCol1(absMatrixAB.getCol2() * boxB.half.getX() + absMatrixAB.getCol0() * boxB.half.getZ());
|
||||
projBhalf.setCol2(absMatrixAB.getCol0() * boxB.half.getY() + absMatrixAB.getCol1() * boxB.half.getX());
|
||||
projBhalf.setCol0(absMatrixAB.getCol1() * boxB.mHalf.getZ() + absMatrixAB.getCol2() * boxB.mHalf.getY());
|
||||
projBhalf.setCol1(absMatrixAB.getCol2() * boxB.mHalf.getX() + absMatrixAB.getCol0() * boxB.mHalf.getZ());
|
||||
projBhalf.setCol2(absMatrixAB.getCol0() * boxB.mHalf.getY() + absMatrixAB.getCol1() * boxB.mHalf.getX());
|
||||
|
||||
Matrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf);
|
||||
vmMatrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf);
|
||||
|
||||
CrossAxisTest(0,0,X);
|
||||
CrossAxisTest(0,1,Y);
|
||||
|
|
@ -872,7 +874,7 @@ boxBoxDistance(
|
|||
axisA = -axisA;
|
||||
axisB = matrixBA * -axisA;
|
||||
|
||||
Vector3 absAxisB = Vector3(absPerElem(axisB));
|
||||
vmVector3 absAxisB = vmVector3(absPerElem(axisB));
|
||||
|
||||
if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) )
|
||||
faceDimB = 0;
|
||||
|
|
@ -885,7 +887,7 @@ boxBoxDistance(
|
|||
axisB = -axisB;
|
||||
axisA = matrixAB * -axisB;
|
||||
|
||||
Vector3 absAxisA = Vector3(absPerElem(axisA));
|
||||
vmVector3 absAxisA = vmVector3(absPerElem(axisA));
|
||||
|
||||
if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) )
|
||||
faceDimA = 0;
|
||||
|
|
@ -900,8 +902,8 @@ boxBoxDistance(
|
|||
axisA = -axisA;
|
||||
axisB = matrixBA * -axisA;
|
||||
|
||||
Vector3 absAxisA = Vector3(absPerElem(axisA));
|
||||
Vector3 absAxisB = Vector3(absPerElem(axisB));
|
||||
vmVector3 absAxisA = vmVector3(absPerElem(axisA));
|
||||
vmVector3 absAxisB = vmVector3(absPerElem(axisB));
|
||||
|
||||
dimA[1] = edgeDimA;
|
||||
dimB[1] = edgeDimB;
|
||||
|
|
@ -966,7 +968,7 @@ boxBoxDistance(
|
|||
dimB[1] = (faceDimB+2)%3;
|
||||
}
|
||||
|
||||
Matrix3 aperm_col, bperm_col;
|
||||
vmMatrix3 aperm_col, bperm_col;
|
||||
|
||||
aperm_col.setCol0(ident[dimA[0]]);
|
||||
aperm_col.setCol1(ident[dimA[1]]);
|
||||
|
|
@ -976,32 +978,32 @@ boxBoxDistance(
|
|||
bperm_col.setCol1(ident[dimB[1]]);
|
||||
bperm_col.setCol2(ident[dimB[2]]);
|
||||
|
||||
Matrix3 aperm_row, bperm_row;
|
||||
vmMatrix3 aperm_row, bperm_row;
|
||||
|
||||
aperm_row = transpose(aperm_col);
|
||||
bperm_row = transpose(bperm_col);
|
||||
|
||||
// permute all box parameters to be in the face coordinate systems
|
||||
|
||||
Matrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;
|
||||
Matrix3 matrixBA_perm = transpose(matrixAB_perm);
|
||||
vmMatrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;
|
||||
vmMatrix3 matrixBA_perm = transpose(matrixAB_perm);
|
||||
|
||||
Vector3 offsetAB_perm, offsetBA_perm;
|
||||
vmVector3 offsetAB_perm, offsetBA_perm;
|
||||
|
||||
offsetAB_perm = aperm_row * offsetAB;
|
||||
offsetBA_perm = bperm_row * offsetBA;
|
||||
|
||||
Vector3 halfA_perm, halfB_perm;
|
||||
vmVector3 halfA_perm, halfB_perm;
|
||||
|
||||
halfA_perm = aperm_row * boxA.half;
|
||||
halfB_perm = bperm_row * boxB.half;
|
||||
halfA_perm = aperm_row * boxA.mHalf;
|
||||
halfB_perm = bperm_row * boxB.mHalf;
|
||||
|
||||
// compute the vector between the centers of each face, in each face's coordinate frame
|
||||
|
||||
Vector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;
|
||||
vmVector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;
|
||||
|
||||
signsA_perm = copySignPerElem(Vector3(1.0f),aperm_row * axisA);
|
||||
signsB_perm = copySignPerElem(Vector3(1.0f),bperm_row * axisB);
|
||||
signsA_perm = copySignPerElem(vmVector3(1.0f),aperm_row * axisA);
|
||||
signsB_perm = copySignPerElem(vmVector3(1.0f),bperm_row * axisB);
|
||||
scalesA_perm = mulPerElem( signsA_perm, halfA_perm );
|
||||
scalesB_perm = mulPerElem( signsB_perm, halfB_perm );
|
||||
|
||||
|
|
@ -1031,11 +1033,11 @@ boxBoxDistance(
|
|||
// if for some reason no case passes the Voronoi test, the features with the minimum
|
||||
// distance are returned.
|
||||
|
||||
Point3 localPointA_perm, localPointB_perm;
|
||||
vmPoint3 localPointA_perm, localPointB_perm;
|
||||
float minDistSqr;
|
||||
bool done;
|
||||
|
||||
Vector3 hA_perm( halfA_perm ), hB_perm( halfB_perm );
|
||||
vmVector3 hA_perm( halfA_perm ), hB_perm( halfB_perm );
|
||||
|
||||
localPointA_perm.setZ( scalesA_perm.getZ() );
|
||||
localPointB_perm.setZ( scalesB_perm.getZ() );
|
||||
|
|
@ -1109,9 +1111,11 @@ boxBoxDistance(
|
|||
|
||||
// convert local points from face-local to box-local coordinate system
|
||||
|
||||
boxPointA.localPoint = Point3( aperm_col * Vector3( localPointA_perm ) );
|
||||
boxPointB.localPoint = Point3( bperm_col * Vector3( localPointB_perm ) );
|
||||
|
||||
boxPointA.localPoint = vmPoint3( aperm_col * vmVector3( localPointA_perm )) ;
|
||||
boxPointB.localPoint = vmPoint3( bperm_col * vmVector3( localPointB_perm )) ;
|
||||
|
||||
#if 0
|
||||
// find which features of the boxes are involved.
|
||||
// the only feature pairs which occur in this function are VF, FV, and EE, even though the
|
||||
// closest points might actually lie on sub-features, as in a VF contact might be used for
|
||||
|
|
@ -1144,6 +1148,7 @@ boxBoxDistance(
|
|||
} else {
|
||||
boxPointB.setVertexFeature( sB[0], sB[1], sB[2] );
|
||||
}
|
||||
#endif
|
||||
|
||||
normal = transformA * axisA;
|
||||
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@ subject to the following restrictions:
|
|||
|
||||
#include "Box.h"
|
||||
|
||||
using namespace Vectormath::Aos;
|
||||
|
||||
//---------------------------------------------------------------------------
|
||||
// boxBoxDistance:
|
||||
|
|
@ -38,7 +37,7 @@ using namespace Vectormath::Aos;
|
|||
// positive or negative distance between two boxes.
|
||||
//
|
||||
// args:
|
||||
// Vector3& normal: set to a unit contact normal pointing from box A to box B.
|
||||
// vmVector3& normal: set to a unit contact normal pointing from box A to box B.
|
||||
//
|
||||
// BoxPoint& boxPointA, BoxPoint& boxPointB:
|
||||
// set to a closest point or point of penetration on each box.
|
||||
|
|
@ -46,7 +45,7 @@ using namespace Vectormath::Aos;
|
|||
// Box boxA, Box boxB:
|
||||
// boxes, represented as 3 half-widths
|
||||
//
|
||||
// const Transform3& transformA, const Transform3& transformB:
|
||||
// const vmTransform3& transformA, const vmTransform3& transformB:
|
||||
// box transformations, in world coordinates
|
||||
//
|
||||
// float distanceThreshold:
|
||||
|
|
@ -58,9 +57,9 @@ using namespace Vectormath::Aos;
|
|||
//---------------------------------------------------------------------------
|
||||
|
||||
float
|
||||
boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
|
||||
PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB,
|
||||
const Transform3 & transformB,
|
||||
boxBoxDistance(vmVector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
|
||||
PE_REF(Box) boxA, const vmTransform3 & transformA, PE_REF(Box) boxB,
|
||||
const vmTransform3 & transformB,
|
||||
float distanceThreshold = FLT_MAX );
|
||||
|
||||
#endif /* __BOXBOXDISTANCE_H__ */
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue