void render_debug_box(
const PfxVector3 ¢er,
const PfxVector3 &extent,
const PfxVector3 &color)
{
const PfxVector3 points[8] = {
center + mulPerElem(PfxVector3(-1,-1,-1),extent),
center + mulPerElem(PfxVector3(-1,-1, 1),extent),
center + mulPerElem(PfxVector3( 1,-1, 1),extent),
center + mulPerElem(PfxVector3( 1,-1,-1),extent),
center + mulPerElem(PfxVector3(-1, 1,-1),extent),
center + mulPerElem(PfxVector3(-1, 1, 1),extent),
center + mulPerElem(PfxVector3( 1, 1, 1),extent),
center + mulPerElem(PfxVector3( 1, 1,-1),extent),
};
const unsigned short indices[] = {
0,1,1,2,2,3,3,0,4,5,5,6,6,7,7,4,0,4,1,5,2,6,3,7,
};
glColor4fv((float*)&color);
glEnableClientState(GL_VERTEX_ARRAY);
glVertexPointer(3,GL_FLOAT,16,(float*)points);
glDrawElements(GL_LINES,24,GL_UNSIGNED_SHORT,indices);
glDisableClientState(GL_VERTEX_ARRAY);
}
Magic3D::Vector3 Magic3D::Object::getScaleFromParent()
{
Vector3 scale = getScale();
if (getParent() && parentScale)
{
Vector3 v = getParent()->getScaleFromParent();
if (getParentBone())
{
v = mulPerElem(v, getParentBone()->getScaleFromParent());
}
scale = mulPerElem(v, scale);
}
return scale;
}
void akPoseBlender::blendJoint(BlendMode bmode, RotMode rmode, akScalar weight, const akTransformState& a, const akTransformState& b, akTransformState& out)
{
switch(bmode)
{
case PB_BM_LERP:
out.loc = lerp(weight, a.loc, b.loc);
if(rmode==PB_RM_SLERP)
out.rot = slerp(weight, a.rot, b.rot);
else
out.rot = lerp(weight, a.rot, b.rot);
out.scale = lerp(weight, a.scale, b.scale);
break;
case PB_BM_ADD:
out.loc = a.loc + b.loc * weight;
if(rmode==PB_RM_SLERP)
out.rot = slerp(weight, a.rot, a.rot*b.rot);
else
out.rot = lerp(weight, a.rot, a.rot*b.rot);
out.scale = lerp(weight, a.scale, mulPerElem(a.scale, b.scale) );
// {
// akTransformState sum;
// akMathUtils::extractTransform(a.toMatrix()*b.toMatrix(), sum.loc, sum.rot, sum.scale);
// out.loc = lerp(weight, a.loc, sum.loc);
// if(rmode==PB_RM_SLERP)
// out.rot = slerp(weight, a.rot, sum.rot);
// else
// out.rot = lerp(weight, a.rot, sum.rot);
// out.scale = lerp(weight, a.scale, sum.scale);
// }
break;
//TODO test this.
case PB_BM_SUB:
out.loc = a.loc - b.loc * weight;
akQuat invrot = conj(b.rot);
if(rmode==PB_RM_SLERP)
out.rot = slerp(weight, a.rot, a.rot*invrot);
else
out.rot = lerp(weight, a.rot, a.rot*invrot);
// Needs a check for dision by 0 (not likely to happen).
out.scale = lerp(weight, a.scale, divPerElem(a.scale, b.scale) );
break;
}
}
hBool hViewFrustum::testAABB( const Heart::hAABB& AABB ) {
if ( Heart::hAABB::intersect( AABB, viewFrustumAABB_ ) )
{
//do culling test against planes
hVec3 vex[ 8 ];
//bottom left near
vex[0] = AABB.c_ - AABB.r_;
//bottom right near
vex[1] = AABB.c_ + mulPerElem( AABB.r_, hVec3( 1.f, -1.f, -1.f ) );
//bottom right far
vex[2] = AABB.c_ + mulPerElem( AABB.r_, hVec3( 1.f, -1.f, 1.f ) );
//bottom left far
vex[3] = AABB.c_ + mulPerElem( AABB.r_, hVec3( -1.f, -1.f, +1.f ) );
//top left near
vex[4] = AABB.c_ + mulPerElem( AABB.r_, hVec3( -1.f, 1.f, -1.f ) );
//top right near
vex[5] = AABB.c_ + mulPerElem( AABB.r_, hVec3( 1.f, 1.f, -1.f ) );
//top right far
vex[6] = AABB.c_ + AABB.r_;
//top left far
vex[7] = AABB.c_ + mulPerElem( AABB.r_, hVec3( -1.f, 1.f, 1.f ) );
hPlane* planes[ 5 ] = {&top_, &bottom_, &left_, &right_, &far_ };
for ( hUint32 p = 0; p < 5; ++p )
{
hPlane* pPlane = planes[ p ];
hBool fullyBehindPlane = hTrue;
for ( hUint32 i = 0; i < 8; ++i )
{
if ( !pointBehindPlane( vex[ i ], *pPlane ) )
{
fullyBehindPlane = hFalse;
break;
}
}
if ( fullyBehindPlane )
{
return hFalse;
}
}
//is visible
return hTrue;
}
return hFalse;
}
void broadphase()
{
//J 剛体が最も分散している軸を見つける
//E Find the axis along which all rigid bodies are most widely positioned
int axis = 0;
{
PfxVector3 s(0.0f),s2(0.0f);
for(int i=0;i<numRigidBodies;i++) {
PfxVector3 c = states[i].getPosition();
s += c;
s2 += mulPerElem(c,c);
}
PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;
if(v[1] > v[0]) axis = 1;
if(v[2] > v[axis]) axis = 2;
}
//J ブロードフェーズプロキシの更新
//E Create broadpahse proxies
{
for(int i=0;i<numRigidBodies;i++) {
pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis);
}
int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies;
void *workBuff = pool.allocate(workBytes);
pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes);
pool.deallocate(workBuff);
}
//J 交差ペア探索
//E Find overlapped pairs
{
PfxFindPairsParam param;
param.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS);
param.workBuff = pool.allocate(param.workBytes);
param.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS);
param.pairBuff = pool.allocate(param.pairBytes);
param.proxies = proxies;
param.numProxies = numRigidBodies;
param.maxPairs = NUM_CONTACTS;
param.axis = axis;
PfxFindPairsResult result;
int ret = pfxFindPairs(param,result);
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);
memcpy(pairs,result.pairs,sizeof(PfxBroadphasePair)*result.numPairs);
numPairs = result.numPairs;
pool.deallocate(param.pairBuff);
pool.deallocate(param.workBuff);
}
//J 新規ペアのコンタクトを初期化
//E Add new contacts and initialize
for(PfxUInt32 i=0;i<numPairs;i++) {
pfxSetContactId(pairs[i],i);
PfxContactManifold &contact = contacts[i];
contact.reset(pfxGetObjectIdA(pairs[i]),pfxGetObjectIdB(pairs[i]));
}
numContacts = numPairs;
}
void broadphase()
{
pairSwap = 1-pairSwap;
unsigned int &numPreviousPairs = numPairs[1-pairSwap];
unsigned int &numCurrentPairs = numPairs[pairSwap];
PfxBroadphasePair *previousPairs = pairsBuff[1-pairSwap];
PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
//J 剛体が最も分散している軸を見つける
//E Find the axis along which all rigid bodies are most widely positioned
int axis = 0;
{
PfxVector3 s(0.0f),s2(0.0f);
for(int i=0;i<numRigidBodies;i++) {
PfxVector3 c = states[i].getPosition();
s += c;
s2 += mulPerElem(c,c);
}
PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;
if(v[1] > v[0]) axis = 1;
if(v[2] > v[axis]) axis = 2;
}
//J ブロードフェーズプロキシの更新
//E Create broadpahse proxies
{
//J レイキャストと共用するため、全ての軸に対するプロキシ配列を作成する
//E To share with ray casting, create proxy arrays for all axis
PfxUpdateBroadphaseProxiesParam param;
param.workBytes = pfxGetWorkBytesOfUpdateBroadphaseProxies(numRigidBodies);
param.workBuff = pool.allocate(param.workBytes,128);
param.numRigidBodies = numRigidBodies;
param.offsetRigidStates = states;
param.offsetCollidables = collidables;
param.proxiesX = proxies[0];
param.proxiesY = proxies[1];
param.proxiesZ = proxies[2];
param.proxiesXb = proxies[3];
param.proxiesYb = proxies[4];
param.proxiesZb = proxies[5];
param.worldCenter = worldCenter;
param.worldExtent = worldExtent;
PfxUpdateBroadphaseProxiesResult result;
int ret = pfxUpdateBroadphaseProxies(param,result);
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxUpdateBroadphaseProxies failed %d\n",ret);
pool.deallocate(param.workBuff);
}
//J 交差ペア探索
//E Find overlapped pairs
{
PfxFindPairsParam findPairsParam;
findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS);
findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes);
findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS);
findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes);
findPairsParam.proxies = proxies[axis];
findPairsParam.numProxies = numRigidBodies;
findPairsParam.maxPairs = NUM_CONTACTS;
findPairsParam.axis = axis;
PfxFindPairsResult findPairsResult;
int ret = pfxFindPairs(findPairsParam,findPairsResult);
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);
pool.deallocate(findPairsParam.workBuff);
//J 交差ペア合成
//E Decompose overlapped pairs into 3 arrays
PfxDecomposePairsParam decomposePairsParam;
decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes);
decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes);
decomposePairsParam.previousPairs = previousPairs;
decomposePairsParam.numPreviousPairs = numPreviousPairs;
decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs()
decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs()
PfxDecomposePairsResult decomposePairsResult;
ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);
pool.deallocate(decomposePairsParam.workBuff);
PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs;
PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs;
PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs;
PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs;
PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs;
PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs;
//J 廃棄ペアのコンタクトをプールに戻す
//E Put removed contacts into the contact pool
for(PfxUInt32 i=0;i<numOutRemovePairs;i++) {
contactIdPool[numContactIdPool++] = pfxGetContactId(outRemovePairs[i]);
//J 寝てる剛体を起こす
//E Wake up sleeping rigid bodies
PfxRigidState &stateA = states[pfxGetObjectIdA(outRemovePairs[i])];
PfxRigidState &stateB = states[pfxGetObjectIdB(outRemovePairs[i])];
if(stateA.isAsleep()) {
stateA.wakeup();
}
if(stateB.isAsleep()) {
stateB.wakeup();
}
}
//J 新規ペアのコンタクトのリンクと初期化
//E Add new contacts and initialize
for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
int cId = 0;
if(numContactIdPool > 0) {
cId = contactIdPool[--numContactIdPool];
}
else {
cId = numContacts++;
}
if(cId >= NUM_CONTACTS) {
cId = 0;
}
SCE_PFX_ASSERT(cId < NUM_CONTACTS);
pfxSetContactId(outNewPairs[i],cId);
PfxContactManifold &contact = contacts[cId];
contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i]));
//J 寝てる剛体を起こす
//E Wake up sleeping rigid bodies
PfxRigidState &stateA = states[pfxGetObjectIdA(outNewPairs[i])];
PfxRigidState &stateB = states[pfxGetObjectIdB(outNewPairs[i])];
if(stateA.isAsleep()) {
stateA.wakeup();
}
if(stateB.isAsleep()) {
stateB.wakeup();
}
}
//J 新規ペアと維持ペアを合成
//E Merge 'new' and 'keep' pairs
numCurrentPairs = 0;
for(PfxUInt32 i=0;i<numOutKeepPairs;i++) {
currentPairs[numCurrentPairs++] = outKeepPairs[i];
}
for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
currentPairs[numCurrentPairs++] = outNewPairs[i];
}
pool.deallocate(decomposePairsParam.pairBuff);
pool.deallocate(findPairsParam.pairBuff);
}
{
int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;
void *workBuff = pool.allocate(workBytes);
pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);
pool.deallocate(workBuff);
}
}
void broadphase()
{
pairSwap = 1-pairSwap;
unsigned int &numPreviousPairs = numPairs[1-pairSwap];
unsigned int &numCurrentPairs = numPairs[pairSwap];
PfxBroadphasePair *previousPairs = pairsBuff[1-pairSwap];
PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
//J 剛体が最も分散している軸を見つける
//E Find the axis along which all rigid bodies are most widely positioned
int axis = 0;
{
PfxVector3 s(0.0f),s2(0.0f);
for(int i=0;i<numRigidBodies;i++) {
PfxVector3 c = states[i].getPosition();
s += c;
s2 += mulPerElem(c,c);
}
PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;
if(v[1] > v[0]) axis = 1;
if(v[2] > v[axis]) axis = 2;
}
//J ブロードフェーズプロキシの更新
//E Create broadpahse proxies
{
for(int i=0;i<numRigidBodies;i++) {
pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis);
}
int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies;
void *workBuff = pool.allocate(workBytes);
pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes);
pool.deallocate(workBuff);
}
//J 交差ペア探索
//E Find overlapped pairs
{
PfxFindPairsParam findPairsParam;
findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS);
findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes);
findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS);
findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes);
findPairsParam.proxies = proxies;
findPairsParam.numProxies = numRigidBodies;
findPairsParam.maxPairs = NUM_CONTACTS;
findPairsParam.axis = axis;
PfxFindPairsResult findPairsResult;
int ret = pfxFindPairs(findPairsParam,findPairsResult);
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);
pool.deallocate(findPairsParam.workBuff);
curNumPairs = findPairsResult.numPairs;
//J 交差ペア合成
//E Decompose overlapped pairs into 3 arrays
PfxDecomposePairsParam decomposePairsParam;
decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes);
decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes);
decomposePairsParam.previousPairs = previousPairs;
decomposePairsParam.numPreviousPairs = numPreviousPairs;
decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs()
decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs()
PfxDecomposePairsResult decomposePairsResult;
ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);
if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);
pool.deallocate(decomposePairsParam.workBuff);
PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs;
PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs;
PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs;
PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs;
PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs;
PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs;
//J 廃棄ペアのコンタクトをプールに戻す
//E Put removed contacts into the contact pool
for(PfxUInt32 i=0;i<numOutRemovePairs;i++) {
contactIdPool[numContactIdPool++] = pfxGetContactId(outRemovePairs[i]);
}
//J 新規ペアのコンタクトのリンクと初期化
//E Add new contacts and initialize
for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
int cId = 0;
if(numContactIdPool > 0) {
cId = contactIdPool[--numContactIdPool];
}
else {
cId = numContacts++;
}
if(cId >= NUM_CONTACTS) {
cId = 0;
}
SCE_PFX_ASSERT(cId < NUM_CONTACTS);
pfxSetContactId(outNewPairs[i],cId);
PfxContactManifold &contact = contacts[cId];
contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i]));
}
//J 新規ペアと維持ペアを合成
//E Merge 'new' and 'keep' pairs
numCurrentPairs = 0;
for(PfxUInt32 i=0;i<numOutKeepPairs;i++) {
currentPairs[numCurrentPairs++] = outKeepPairs[i];
}
for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
currentPairs[numCurrentPairs++] = outNewPairs[i];
}
bool verboseStats = true;
if (verboseStats)
{
printf("===============================================\n");
printf("num bodies/states = %d\n", physics_get_num_rigidbodies());
for (int i=0;i<physics_get_num_rigidbodies();i++)
{
PfxVector3 pos = physics_get_state(i).getPosition();
printf("body %d has position %f,%f,%f\n",i,pos.getX(),pos.getY(),pos.getZ());
}
printf("numCurrentPairs (total) = %d\n", numCurrentPairs);
for (int i=0;i<numCurrentPairs;i++)
{
int idA = pfxGetObjectIdA(currentPairs[i]);
int idB = pfxGetObjectIdB(currentPairs[i]);
printf("pfx pair[%d] idA = %d, idB = %d\n", i, idA,idB);
int cId = pfxGetContactId(currentPairs[i]);
printf("contact duration = %d\n", contacts[cId].getDuration());
if (1)
{
printf("num contacts = %d\n", contacts[cId].getNumContacts());
for (int c=0;c<contacts[cId].getNumContacts();c++)
{
const PfxContactPoint& cp = contacts[cId].getContactPoint(c);
printf("localPosA = %f,%f,%f. ", cp.m_localPointA[0],cp.m_localPointA[1],cp.m_localPointA[2]);
printf("localPosB = %f,%f,%f. ", cp.m_localPointB[0],cp.m_localPointB[1],cp.m_localPointB[2]);
for (int r=0;r<3;r++)
{
printf("row %d accumImpulse = %f. ", r, cp.m_constraintRow[r].m_accumImpulse);
printf("row %d normal = %f,%f,%f. ", r, cp.m_constraintRow[r].m_normal[0],cp.m_constraintRow[r].m_normal[1],cp.m_constraintRow[r].m_normal[2]);
printf("row %d distance %f and duration %d\n", r, cp.m_distance1,cp.m_duration);
}
}
}
}
}
//printf("numOutRemovePairs = %d\n", numOutRemovePairs);
//printf("numOutNewPairs = %d\n",numOutNewPairs);
pool.deallocate(decomposePairsParam.pairBuff);
pool.deallocate(findPairsParam.pairBuff);
}
{
int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;
void *workBuff = pool.allocate(workBytes);
pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);
pool.deallocate(workBuff);
}
}
static inline float2 sign_not_zero(const float2& v)
{
return subtract(mulPerElem(step(float2(0, 0), v), 2.0), float2(1, 1));
}
PfxFloat pfxContactBoxCapsule(
PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
void *shapeA,const PfxTransform3 &transformA,
void *shapeB,const PfxTransform3 &transformB,
PfxFloat distanceThreshold)
{
PfxBox boxA = *((PfxBox*)shapeA);
PfxCapsule capsuleB = *((PfxCapsule*)shapeB);
PfxVector3 ident[3] = {
PfxVector3(1.0,0.0,0.0),
PfxVector3(0.0,1.0,0.0),
PfxVector3(0.0,0.0,1.0),
};
// get capsule position and direction in box's coordinate system
PfxMatrix3 matrixA = transformA.getUpper3x3();
PfxMatrix3 matrixAinv = transpose(matrixA);
PfxVector3 directionB = transformB.getUpper3x3().getCol0();
PfxVector3 translationB = transformB.getTranslation();
PfxVector3 capsDirection = matrixAinv * directionB;
PfxVector3 absCapsDirection = absPerElem(capsDirection);
PfxVector3 offsetAB = matrixAinv * (translationB - transformA.getTranslation());
// find separating axis with largest gap between projections
BoxCapsSepAxisType axisType;
PfxVector3 axisA;
PfxFloat maxGap;
int faceDimA = 0, edgeDimA = 0;
// face axes
// can compute all the gaps at once with VU0
PfxVector3 gapsA = absPerElem(offsetAB) - boxA.m_half - absCapsDirection * capsuleB.m_halfLen;
AaxisTest( 0, X, true );
AaxisTest( 1, Y, false );
AaxisTest( 2, Z, false );
// cross product axes
// compute gaps on all cross product axes using some VU0 math. suppose there's a tradeoff
// between doing this with SIMD all at once or without SIMD in each cross product test, since
// some test might exit early.
PfxVector3 lsqrs, projOffset, projAhalf;
PfxMatrix3 crossProdMat = crossMatrix(capsDirection) * PfxMatrix3::identity();
PfxMatrix3 crossProdMatT = crossMatrix(-capsDirection) * PfxMatrix3::identity();
lsqrs = mulPerElem( crossProdMatT.getCol0(), crossProdMatT.getCol0() ) +
mulPerElem( crossProdMatT.getCol1(), crossProdMatT.getCol1() ) +
mulPerElem( crossProdMatT.getCol2(), crossProdMatT.getCol2() );
projOffset = crossProdMatT * offsetAB;
projAhalf = absPerElem(crossProdMatT) * boxA.m_half;
PfxVector3 gapsAxB = absPerElem(projOffset) - projAhalf;
CrossAxisTest( 0, X );
CrossAxisTest( 1, Y );
CrossAxisTest( 2, Z );
// make axis point from box center towards capsule center.
if ( dot(axisA,offsetAB) < 0.0f )
axisA = -axisA;
// find the face on box whose normal best matches the separating axis. will use the entire
// face only in degenerate cases.
//
// to make things simpler later, change the coordinate system so that the face normal is the z
// direction. if an edge cross product axis was chosen above, also align the box edge to the y
// axis. this saves the later tests from having to know which face was chosen. changing the
// coordinate system involves permuting vector elements, so construct a permutation matrix.
// I believe this is a faster way to permute a bunch of vectors than using arrays.
int dimA[3];
if ( axisType == CROSS_AXIS ) {
PfxVector3 absAxisA = PfxVector3(absPerElem(axisA));
dimA[1] = edgeDimA;
if ( edgeDimA == 0 ) {
if ( absAxisA[1] > absAxisA[2] ) {
dimA[0] = 2;
dimA[2] = 1;
} else {
dimA[0] = 1;
dimA[2] = 2;
}
} else if ( edgeDimA == 1 ) {
if ( absAxisA[2] > absAxisA[0] ) {
dimA[0] = 0;
dimA[2] = 2;
} else {
dimA[0] = 2;
dimA[2] = 0;
}
} else {
if ( absAxisA[0] > absAxisA[1] ) {
dimA[0] = 1;
dimA[2] = 0;
} else {
dimA[0] = 0;
dimA[2] = 1;
}
}
} else {
dimA[2] = faceDimA;
dimA[0] = (faceDimA+1)%3;
dimA[1] = (faceDimA+2)%3;
}
PfxMatrix3 aperm_col;
aperm_col.setCol0(ident[dimA[0]]);
aperm_col.setCol1(ident[dimA[1]]);
aperm_col.setCol2(ident[dimA[2]]);
PfxMatrix3 aperm_row = transpose(aperm_col);
// permute vectors to be in face coordinate system.
PfxVector3 offsetAB_perm = aperm_row * offsetAB;
PfxVector3 halfA_perm = aperm_row * boxA.m_half;
PfxVector3 signsA_perm = copySignPerElem(PfxVector3(1.0f), aperm_row * axisA);
PfxVector3 scalesA_perm = mulPerElem( signsA_perm, halfA_perm );
PfxVector3 capsDirection_perm = aperm_row * capsDirection;
PfxFloat signB = (-dot(capsDirection,axisA) > 0.0f)? 1.0f : -1.0f;
PfxFloat scaleB = signB * capsuleB.m_halfLen;
// compute the vector between the center of the box face and the capsule center
offsetAB_perm.setZ( offsetAB_perm.getZ() - scalesA_perm.getZ() );
// if box and capsule overlap, this will separate them for finding points of penetration.
if ( maxGap < 0.0f ) {
offsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;
}
// for each vertex/face or edge/edge pair of box face and line segment, find the closest
// points.
//
// these points each have an associated feature (vertex, edge, or face). if each
// point is in the external Voronoi region of the other's feature, they are the
// closest points of the objects, and the algorithm can exit.
//
// the feature pairs are arranged so that in the general case, the first test will
// succeed. degenerate cases (line segment parallel to face) may require up to all tests
// in the worst case.
//
// if for some reason no case passes the Voronoi test, the features with the minimum
// distance are returned.
PfxVector3 closestPtsVec_perm;
PfxPoint3 localPointA_perm;
PfxFloat minDistSqr;
PfxFloat segmentParamB;
PfxBool done;
localPointA_perm.setZ( scalesA_perm.getZ() );
scalesA_perm.setZ(0.0f);
PfxVector3 hA_perm( halfA_perm );
int otherFaceDimA;
if ( axisType == CROSS_AXIS ) {
EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
otherFaceDimA,
hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm,
scalesA_perm, true );
if ( !done ) {
VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, false );
}
} else {
VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, true );
if ( !done ) {
EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,
otherFaceDimA,
hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm,
scalesA_perm, false );
}
}
// compute normal
PfxBool centerInside = ( signsA_perm.getZ() * closestPtsVec_perm.getZ() < 0.0f );
if ( centerInside || ( minDistSqr < lenSqrTol ) ) {
normal = matrixA * axisA;
} else {
PfxVector3 closestPtsVec = aperm_col * closestPtsVec_perm;
normal = matrixA * ( closestPtsVec * (1.0f/sqrtf( minDistSqr )) );
}
// compute box point
pointA = PfxPoint3( aperm_col * PfxVector3( localPointA_perm ) );
// compute capsule point
pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( directionB * segmentParamB - normal * capsuleB.m_radius ) );
if ( centerInside ) {
return (-sqrtf( minDistSqr ) - capsuleB.m_radius);
} else {
return (sqrtf( minDistSqr ) - capsuleB.m_radius);
}
}
