PfxFloat pfxContactCapsuleCapsule(
PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
void *shapeA,const PfxTransform3 &transformA,
void *shapeB,const PfxTransform3 &transformB,
PfxFloat distanceThreshold)
{
PfxCapsule &capsuleA = *((PfxCapsule*)shapeA);
PfxCapsule &capsuleB = *((PfxCapsule*)shapeB);
PfxVector3 directionA = transformA.getUpper3x3().getCol0();
PfxVector3 translationA = transformA.getTranslation();
PfxVector3 directionB = transformB.getUpper3x3().getCol0();
PfxVector3 translationB = transformB.getTranslation();
// translation between centers
PfxVector3 translation = translationB - translationA;
// compute the closest points of the capsule line segments
PfxVector3 ptsVector; // the vector between the closest points
PfxVector3 offsetA, offsetB; // offsets from segment centers to their closest points
PfxFloat tA, tB; // parameters on line segment
segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
directionA, capsuleA.m_halfLen, directionB, capsuleB.m_halfLen );
PfxFloat distance = length(ptsVector) - capsuleA.m_radius - capsuleB.m_radius;
if ( distance > distanceThreshold )
return distance;
// compute the contact normal
segmentsNormal( normal, ptsVector );
// compute points on capsules
pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) );
pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( offsetB - normal * capsuleB.m_radius ) );
return distance;
}
PfxFloat pfxContactCapsuleSphere(
PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,
void *shapeA,const PfxTransform3 &transformA,
void *shapeB,const PfxTransform3 &transformB,
PfxFloat distanceThreshold)
{
PfxCapsule &capsuleA = *((PfxCapsule*)shapeA);
PfxSphere &sphereB = *((PfxSphere*)shapeB);
PfxVector3 directionA = transformA.getUpper3x3().getCol0();
PfxVector3 translationA = transformA.getTranslation();
PfxVector3 translationB = transformB.getTranslation();
// translation between centers of capsule and sphere
PfxVector3 translation = translationB - translationA;
// compute the closest point on the capsule line segment to the sphere center
PfxVector3 ptsVector;
PfxVector3 offsetA;
PfxFloat tA;
segmentPointClosestPoints( ptsVector, offsetA, tA, translation, directionA, capsuleA.m_halfLen );
PfxFloat distance = length(ptsVector) - capsuleA.m_radius - sphereB.m_radius;
if ( distance > distanceThreshold )
return distance;
// compute the contact normal
segmentPointNormal( normal, ptsVector );
// compute points on capsule and sphere
pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) );
pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -normal * sphereB.m_radius ) );
return distance;
}
PfxBool pfxIntersectRayBox(const PfxRayInput &ray,PfxRayOutput &out,const PfxBox &box,const PfxTransform3 &transform)
{
// レイをBoxのローカル座標へ変換
PfxTransform3 transformBox = orthoInverse(transform);
PfxVector3 rayStartPosition = transformBox.getUpper3x3() * ray.m_startPosition + transformBox.getTranslation();
PfxVector3 rayDirection = transformBox.getUpper3x3() * ray.m_direction;
// 交差判定
PfxFloat tmpVariable=0.0f;
PfxVector3 tmpNormal(0.0f);
if(pfxIntersectRayAABB(rayStartPosition,rayDirection,PfxVector3(0.0f),box.m_half,tmpVariable,tmpNormal)) {
if(tmpVariable > 0.0f && tmpVariable < out.m_variable) {
out.m_contactFlag = true;
out.m_variable = tmpVariable;
out.m_contactPoint = ray.m_startPosition + tmpVariable * ray.m_direction;
out.m_contactNormal = transform.getUpper3x3() * tmpNormal;
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
return false;
}
static void convertToBullet(void)
{
cleanupBullet();
m_config = new btDefaultCollisionConfiguration();
btHashedOverlappingPairCache* pairCache = new btHashedOverlappingPairCache();
//m_broadphase = new btDbvtBroadphase();
btLowLevelData* lowLevelData = new btLowLevelData;
lowLevelData->m_maxContacts = NUM_CONTACTS;//8024;
lowLevelData->m_contactIdPool = (int*) btAlignedAlloc(sizeof(int)*lowLevelData->m_maxContacts ,16);
lowLevelData->m_contacts = (PfxContactManifold*) btAlignedAlloc(sizeof(PfxContactManifold)*lowLevelData->m_maxContacts,16);
lowLevelData->m_maxPairs = lowLevelData->m_maxContacts;//??
lowLevelData->m_pairsBuff[0] = (PfxBroadphasePair*) btAlignedAlloc(sizeof(PfxBroadphasePair)*lowLevelData->m_maxPairs,16);
lowLevelData->m_pairsBuff[1] = (PfxBroadphasePair*) btAlignedAlloc(sizeof(PfxBroadphasePair)*lowLevelData->m_maxPairs,16);
#define USE_LL_BP
#ifdef USE_LL_BP
btLowLevelBroadphase* llbp = new btLowLevelBroadphase(lowLevelData,pairCache);
m_broadphase = llbp;
#else //USE_LL_BP
m_broadphase = new btDbvtBroadphase();
#endif //USE_LL_BP
//m_dispatcher = new btCollisionDispatcher(m_config);
m_dispatcher = new btLowLevelCollisionDispatcher(lowLevelData,m_config,NUM_CONTACTS);
//#ifdef USE_PARALLEL_SOLVER
// m_solver = new btSequentialImpulseConstraintSolver();
//#else
//sThreadSupport = createSolverThreadSupport(4);
//m_solver = new btLowLevelConstraintSolver(sThreadSupport);
m_solver = new btLowLevelConstraintSolver2(lowLevelData);
//#endif //USE_PARALLEL_SOLVER
//m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_config);
m_dynamicsWorld = new btBulletPhysicsEffectsWorld(lowLevelData, m_dispatcher,llbp,m_solver,m_config,0);
PEDebugDrawer* drawer = new PEDebugDrawer();
drawer->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawAabb);
m_dynamicsWorld->setDebugDrawer(drawer);
m_dynamicsWorld ->getSolverInfo().m_splitImpulse = true;
for(int i=0;i<physics_get_num_rigidbodies();i++) {
btRigidBody* body = 0;
btAlignedObjectArray<btCollisionShape*> shapes;
btAlignedObjectArray<btTransform> childTtransforms;
PfxRigidState &state = states[i];//physics_get_state(i);
state.setUserData(0);
const PfxCollidable &coll = physics_get_collidable(i);
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(int j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
btTransform childTransform;
childTransform.setIdentity();
childTransform.setOrigin(getBtVector3(offsetT.getTranslation()));
PfxQuat quat(offsetT.getUpper3x3());
childTransform.setBasis(btMatrix3x3(getBtQuat(quat)));
childTtransforms.push_back(childTransform);
switch(shape.getType()) {
case kPfxShapeSphere:
{
btSphereShape* sphere = new btSphereShape(shape.getSphere().m_radius);
shapes.push_back(sphere);
//render_sphere( worldT, Vectormath::Aos::Vector3(1,1,1),Vectormath::floatInVec(shape.getSphere().m_radius));
}
break;
case kPfxShapeBox:
{
btBoxShape* box = new btBoxShape(getBtVector3(shape.getBox().m_half));
shapes.push_back(box);
}
//render_box( worldT, Vectormath::Aos::Vector3(1,1,1), shape.getBox().m_half);
break;
case kPfxShapeCapsule:
shapes.push_back(new btCapsuleShapeX(shape.getCapsule().m_radius,2.f*shape.getCapsule().m_halfLen));
//render_capsule( worldT, Vectormath::Aos::Vector3(1,1,1), Vectormath::floatInVec(shape.getCapsule().m_radius), Vectormath::floatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
shapes.push_back(new btCylinderShapeX(btVector3(shape.getCylinder().m_halfLen,shape.getCylinder().m_radius,shape.getCylinder().m_radius)));
//render_cylinder( worldT, Vectormath::Aos::Vector3(1,1,1), Vectormath::floatInVec(shape.getCylinder().m_radius),Vectormath::floatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
{
const PfxConvexMesh* convex = shape.getConvexMesh();
const btScalar* vertices = (const btScalar*)&convex->m_verts[0];
btConvexHullShape* convexHull = new btConvexHullShape(vertices,convex->m_numVerts, sizeof(PfxVector3));
shapes.push_back(convexHull);
}
break;
case kPfxShapeLargeTriMesh:
{
const PfxLargeTriMesh* mesh = shape.getLargeTriMesh();
btTriangleIndexVertexArray* meshInterface = new btTriangleIndexVertexArray();
int i;
for (i= 0; i < mesh->m_numIslands;i++)
{
PfxTriMesh* island = &mesh->m_islands[i];
//mesh->m_islands
//mesh->m_numIslands
btIndexedMesh indexedMesh;
indexedMesh.m_indexType = PHY_UCHAR;
indexedMesh.m_numTriangles = island->m_numFacets;
indexedMesh.m_triangleIndexBase = &island->m_facets[0].m_vertIds[0];
indexedMesh.m_triangleIndexStride = sizeof(PfxFacet);
indexedMesh.m_vertexBase = (const unsigned char*) &island->m_verts[0];
indexedMesh.m_numVertices = island->m_numVerts;//unused
indexedMesh.m_vertexStride = sizeof(PfxVector3);
meshInterface->addIndexedMesh(indexedMesh,PHY_UCHAR);
}
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true);
shapes.push_back(trimesh);
}
break;
default:
{
printf("unknown\n");
}
break;
}
}
if(shapes.size()>0)
{
printf("shapes!\n");
btCollisionShape* colShape = 0;
if (shapes.size()==1 && childTtransforms[0].getOrigin().fuzzyZero())
//todo: also check orientation
{
colShape = shapes[0];
}
else
{
btCompoundShape* compound = new btCompoundShape();
for (int i=0;i<shapes.size();i++)
{
compound->addChildShape(childTtransforms[i],shapes[i]);
}
colShape = compound;
}
btTransform worldTransform;
worldTransform.setIdentity();
worldTransform.setOrigin(getBtVector3(rbT.getTranslation()));
PfxQuat quat(rbT.getUpper3x3());
worldTransform.setBasis(btMatrix3x3(getBtQuat(quat)));
btScalar mass = physics_get_body(i).getMass();
btRigidBody* body = addRigidBody(colShape,mass,worldTransform);
void* ptr = (void*)&state;
body->setUserPointer(ptr);
state.setUserData(body);
}
}
//very basic joint conversion (only limits of PFX_JOINT_SWINGTWIST joint)
for (int i=0;i<numJoints;i++)
{
PfxJoint& joint = joints[i];
switch (joint.m_type)
{
case kPfxJointSwingtwist:
{
//btConeTwistConstraint* coneTwist = new btConeTwistConstraint(rbA,rbB,frameA,frameB);
bool referenceA = true;
btRigidBody* bodyA = (btRigidBody*)states[joint.m_rigidBodyIdA].getUserData();
btRigidBody* bodyB = (btRigidBody*)states[joint.m_rigidBodyIdB].getUserData();
if (bodyA&&bodyB)
{
btTransform frameA,frameB;
frameA.setIdentity();
frameB.setIdentity();
frameA.setOrigin(getBtVector3(joint.m_anchorA));
frameB.setOrigin(getBtVector3(joint.m_anchorB));
bool referenceA = false;
btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*bodyA,*bodyB,frameA,frameB,referenceA);
for (int i=0;i<joint.m_numConstraints;i++)
{
switch (joint.m_constraints[i].m_lock)
{
case SCE_PFX_JOINT_LOCK_FIX:
{
dof6->setLimit(i,0,0);
break;
}
case SCE_PFX_JOINT_LOCK_FREE:
{
dof6->setLimit(i,1,0);
break;
}
case SCE_PFX_JOINT_LOCK_LIMIT:
{
//deal with the case where angular limits of Y-axis are free and/or beyond -PI/2 and PI/2
if ((i==4) && ((joint.m_constraints[i].m_movableLowerLimit<-SIMD_PI/2)||(joint.m_constraints[i].m_movableUpperLimit>SIMD_PI/2)))
{
printf("error with joint limits, forcing DOF to fixed\n");
dof6->setLimit(i,0,0);
} else
{
dof6->setLimit(i,joint.m_constraints[i].m_movableLowerLimit,joint.m_constraints[i].m_movableUpperLimit);
}
break;
}
default:
{
printf("unknown joint lock state\n");
}
}
}
m_dynamicsWorld->addConstraint(dof6,true);
} else
{
printf("error: missing bodies during joint conversion\n");
}
break;
};
case kPfxJointBall:
case kPfxJointHinge:
case kPfxJointSlider:
case kPfxJointFix:
case kPfxJointUniversal:
case kPfxJointAnimation:
default:
{
printf("unknown joint\n");
}
}
}
//create a large enough buffer. There is no method to pre-calculate the buffer size yet.
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
m_dynamicsWorld->serialize(serializer);
FILE* file = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file);
fclose(file);
}
void graphics_from_physics(GLInstancingRenderer& renderer, bool syncTransformsOnly)
{
int cubeShapeIndex = -1;
int strideInBytes = sizeof(float)*9;
if (!syncTransformsOnly)
{
int numVertices = sizeof(cube_vertices)/strideInBytes;
int numIndices = sizeof(cube_indices)/sizeof(int);
cubeShapeIndex = renderer.registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
}
int curGraphicsIndex=0;
for(int i=0;i<physics_get_num_rigidbodies();i++)
{
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
const PfxRigidBody& body = physics_get_body(i);
float color[4]={0,0,0,1};
if (body.getMass()==0.f)
{
color[0]=1.f;
} else
{
color[1]=1.f;
}
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(int j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
PfxQuat ornWorld( worldT.getUpper3x3());
switch(shape.getType()) {
case kPfxShapeSphere:
printf("render sphere\n");
/*render_sphere(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getSphere().m_radius));
*/
break;
case kPfxShapeBox:
{
// printf("render box\n");
float cubeScaling[4] = {shape.getBox().m_half.getX(),shape.getBox().m_half.getY(),shape.getBox().m_half.getZ(),1};
float rotOrn[4] = {ornWorld.getX(),ornWorld.getY(),ornWorld.getZ(),ornWorld.getW()};
float position[4]={worldT.getTranslation().getX(),worldT.getTranslation().getY(),worldT.getTranslation().getZ(),0};
if (!syncTransformsOnly)
{
renderer.registerGraphicsInstance(cubeShapeIndex,position,rotOrn,color,cubeScaling);
}
else
{
renderer.writeSingleInstanceTransformToCPU(position,rotOrn,curGraphicsIndex);
}
curGraphicsIndex++;
/* render_box(
worldT,
PfxVector3(1,1,1),
shape.getBox().m_half);
*/
break;
}
case kPfxShapeCapsule:
printf("render_capsule\n");
/*render_capsule(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
*/
break;
case kPfxShapeCylinder:
printf("render_cylinder\n");
/* render_cylinder(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
*/
break;
case kPfxShapeConvexMesh:
printf("render_mesh\n");
/*
render_mesh(
worldT,
PfxVector3(1,1,1),
convexMeshId);
*/
break;
case kPfxShapeLargeTriMesh:
printf("render_mesh\n");
/*
render_mesh(
worldT,
PfxVector3(1,1,1),
landscapeMeshId);
*/
break;
default:
break;
}
}
}
}
PfxInt32 pfxContactLargeTriMesh(
PfxContactCache &contacts,
const PfxLargeTriMesh *lmeshA,
const PfxTransform3 &transformA,
const PfxShape &shapeB,
const PfxTransform3 &transformB,
PfxFloat distanceThreshold)
{
PfxTransform3 transformAB;
PfxMatrix3 matrixAB;
PfxVector3 offsetAB;
// Bローカル→Aローカルへの変換
transformAB = orthoInverse(transformA) * transformB;
matrixAB = transformAB.getUpper3x3();
offsetAB = transformAB.getTranslation();
// -----------------------------------------------------
// LargeTriMeshに含まれるTriMeshのAABBと凸体のAABBを判定し、
// 交差するものを個別に衝突判定する。※LargeMesh座標系
PfxVector3 shapeHalf(0.0f);
PfxVector3 shapeCenter = offsetAB;
switch(shapeB.getType()) {
case kPfxShapeSphere:
shapeHalf = PfxVector3(shapeB.getSphere().m_radius);
break;
case kPfxShapeCapsule:
{
PfxCapsule capsule = shapeB.getCapsule();
shapeHalf = absPerElem(matrixAB) * PfxVector3(capsule.m_halfLen+capsule.m_radius,capsule.m_radius,capsule.m_radius);
}
break;
case kPfxShapeCylinder:
{
PfxCylinder cylinder = shapeB.getCylinder();
shapeHalf = absPerElem(matrixAB) * PfxVector3(cylinder.m_halfLen,cylinder.m_radius,cylinder.m_radius);
}
break;
case kPfxShapeBox:
shapeHalf = absPerElem(matrixAB) * shapeB.getBox().m_half;
break;
case kPfxShapeConvexMesh:
shapeHalf = absPerElem(matrixAB) * shapeB.getConvexMesh()->m_half;
break;
default:
break;
}
// -----------------------------------------------------
// アイランドとの衝突判定
PfxVecInt3 aabbMinL,aabbMaxL;
lmeshA->getLocalPosition((shapeCenter-shapeHalf),(shapeCenter+shapeHalf),aabbMinL,aabbMaxL);
PfxUInt32 numIslands = lmeshA->m_numIslands;
{
for(PfxUInt32 i=0;i<numIslands;i++) {
// AABBチェック
PfxAabb16 aabbB = lmeshA->m_aabbList[i];
if(aabbMaxL.getX() < pfxGetXMin(aabbB) || aabbMinL.getX() > pfxGetXMax(aabbB)) continue;
if(aabbMaxL.getY() < pfxGetYMin(aabbB) || aabbMinL.getY() > pfxGetYMax(aabbB)) continue;
if(aabbMaxL.getZ() < pfxGetZMin(aabbB) || aabbMinL.getZ() > pfxGetZMax(aabbB)) continue;
PfxTriMesh *island = &lmeshA->m_islands[i];
// 衝突判定
PfxContactCache localContacts;
switch(shapeB.getType()) {
case kPfxShapeSphere:
pfxContactTriMeshSphere(localContacts,island,transformA,shapeB.getSphere(),transformB,distanceThreshold);
break;
case kPfxShapeCapsule:
pfxContactTriMeshCapsule(localContacts,island,transformA,shapeB.getCapsule(),transformB,distanceThreshold);
break;
case kPfxShapeBox:
pfxContactTriMeshBox(localContacts,island,transformA,shapeB.getBox(),transformB,distanceThreshold);
break;
case kPfxShapeCylinder:
pfxContactTriMeshCylinder(localContacts,island,transformA,shapeB.getCylinder(),transformB,distanceThreshold);
break;
case kPfxShapeConvexMesh:
pfxContactTriMeshConvex(localContacts,island,transformA,*shapeB.getConvexMesh(),transformB,distanceThreshold);
break;
default:
break;
}
// 衝突点を追加
for(int j=0;j<localContacts.getNumContacts();j++) {
PfxSubData subData = localContacts.getSubData(j);
subData.setIslandId(i);
contacts.addContactPoint(
localContacts.getDistance(j),
localContacts.getNormal(j),
localContacts.getLocalPointA(j),
localContacts.getLocalPointB(j),
subData);
}
}
}
return contacts.getNumContacts();
}
PfxBool pfxIntersectRayCapsule(const PfxRayInput &ray,PfxRayOutput &out,const PfxCapsule &capsule,const PfxTransform3 &transform)
{
// レイをCapsuleのローカル座標へ変換
PfxTransform3 transformCapsule = orthoInverse(transform);
PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();
PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;
PfxFloat radSqr = capsule.m_radius * capsule.m_radius;
// 始点がカプセルの内側にあるか判定
{
PfxFloat h = fabsf(startPosL[0]);
if(h > capsule.m_halfLen) h = capsule.m_halfLen;
PfxVector3 Px(out.m_variable,0,0);
PfxFloat sqrLen = lengthSqr(startPosL-Px);
if(sqrLen <= radSqr) return false;
}
// カプセルの胴体との交差判定
do {
PfxVector3 P(startPosL);
PfxVector3 D(rayDirL);
P[0] = 0.0f;
D[0] = 0.0f;
PfxFloat a = dot(D,D);
PfxFloat b = dot(P,D);
PfxFloat c = dot(P,P) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f || fabs(a) < 0.00001f) return false;
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f)
break;
else if(tt > 1.0f)
return false;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
if(fabsf(cp[0]) <= capsule.m_halfLen) {
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));
out.m_contactNormal = transform.getUpper3x3() * normalize(cp);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
} while(0);
// カプセルの両端にある球体との交差判定
PfxFloat a = dot(rayDirL,rayDirL);
if(fabs(a) < 0.00001f) return false;
do {
PfxVector3 center(capsule.m_halfLen,0.0f,0.0f);
PfxVector3 v = startPosL - center;
PfxFloat b = dot(v,rayDirL);
PfxFloat c = dot(v,v) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f) break;
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f || tt > 1.0f) break;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
} while(0);
{
PfxVector3 center(-capsule.m_halfLen,0.0f,0.0f);
PfxVector3 v = startPosL - center;
PfxFloat b = dot(v,rayDirL);
PfxFloat c = dot(v,v) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f) return false;
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f || tt > 1.0f) return false;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + out.m_variable * rayDirL;
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
return false;
}
PfxBool pfxIntersectRayCylinder(const PfxRayInput &ray,PfxRayOutput &out,const PfxCylinder &cylinder,const PfxTransform3 &transform)
{
// レイを円柱のローカル座標へ変換
PfxTransform3 transformCapsule = orthoInverse(transform);
PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();
PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;
PfxFloat radSqr = cylinder.m_radius * cylinder.m_radius;
// 始点が円柱の内側にあるか判定
{
PfxFloat h = startPosL[0];
if(-cylinder.m_halfLen <= h && h <= cylinder.m_halfLen) {
PfxVector3 Px(h,0,0);
PfxFloat sqrLen = lengthSqr(startPosL-Px);
if(sqrLen <= radSqr) return false;
}
}
// 円柱の胴体との交差判定
do {
PfxVector3 P(startPosL);
PfxVector3 D(rayDirL);
P[0] = 0.0f;
D[0] = 0.0f;
PfxFloat a = dot(D,D);
PfxFloat b = dot(P,D);
PfxFloat c = dot(P,P) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f) return false; // レイは逸れている
if(pfxAbsf(a) < 0.00001f) break; // レイがX軸に平行
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f || tt > 1.0f) break;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
if(pfxAbsf(cp[0]) <= cylinder.m_halfLen) {
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));
out.m_contactNormal = transform.getUpper3x3() * normalize(cp);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
} while(0);
// 円柱の両端にある平面との交差判定
{
if(pfxAbsf(rayDirL[0]) < 0.00001f) return false;
PfxFloat t1 = ( cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];
PfxFloat t2 = ( - cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];
PfxFloat tt = SCE_PFX_MIN(t1,t2);
if(tt < 0.0f || tt > 1.0f) return false;
PfxVector3 p = startPosL + tt * rayDirL;
p[0] = 0.0f;
if(lengthSqr(p) < radSqr && tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
out.m_contactNormal = transform.getUpper3x3() * ((cp[0]>0.0f)?PfxVector3(1.0,0.0,0.0):PfxVector3(-1.0,0.0,0.0));
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
return false;
}
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);
}
}