void createSceneRayFilter()
{
int idA = createBrick(PfxVector3(0.0f,2.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);
int idB = createBrick(PfxVector3(0.0f,4.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);
int idC = createBrick(PfxVector3(0.0f,6.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);
int idD = createBrick(PfxVector3(0.0f,8.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);
states[idA].setMotionType(kPfxMotionTypeFixed);
states[idB].setMotionType(kPfxMotionTypeFixed);
states[idC].setMotionType(kPfxMotionTypeFixed);
states[idD].setMotionType(kPfxMotionTypeFixed);
states[idA].setContactFilterSelf(0x01);
states[idB].setContactFilterSelf(0x02);
states[idC].setContactFilterSelf(0x04);
states[idD].setContactFilterSelf(0x08);
numRays = 5;
for(int i=0;i<numRays;i++) {
rayInputs[i].reset();
rayInputs[i].m_startPosition = PfxVector3(-2.0f+i,10.0f,0.0f);
rayInputs[i].m_direction = PfxVector3(0.0f,-10.0f,2.0f);
rayInputs[i].m_contactFilterTarget = 1<<i;
}
}
void render_begin()
{
wglMakeCurrent(hDC, hRC);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glFrontFace(GL_CCW);
glDepthFunc(GL_LESS);
glCullFace(GL_BACK);
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMultMatrixf((GLfloat*)&g_pMat);
// create view matrix
g_viewPos =
PfxMatrix3::rotationY(viewRadY) *
PfxMatrix3::rotationX(viewRadX) *
PfxVector3(0,0,viewRadius);
g_lightPos =
PfxMatrix3::rotationY(lightRadY) *
PfxMatrix3::rotationX(lightRadX) *
PfxVector3(0,0,lightRadius);
PfxMatrix4 viewMtx = PfxMatrix4::lookAt(PfxPoint3(g_viewTgt + g_viewPos),PfxPoint3(g_viewTgt),PfxVector3(0,1,0));
g_vMat = g_pMat * viewMtx;
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glMultMatrixf((GLfloat*)&viewMtx);
}
void pfxGetShapeAabbCapsule(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
PfxVector3 dir = rotate(shape.getOffsetOrientation(),PfxVector3(1.0f,0.0f,0.0f));
PfxVector3 capSize = absPerElem(dir) * shape.getCapsule().m_halfLen +
PfxVector3(shape.getCapsule().m_radius);
aabbMin = shape.getOffsetPosition() - capSize;
aabbMax = shape.getOffsetPosition() + capSize;
}
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);
}
void render_capsule(
const PfxTransform3 &transform,
const PfxVector3 &color,
const PfxFloatInVec &radius,
const PfxFloatInVec &halfLength)
{
PfxTransform3 tr1 = PfxTransform3::translation(PfxVector3(-halfLength,0.0f,0.0f));
PfxTransform3 tr2 = PfxTransform3::translation(PfxVector3(halfLength,0.0f,0.0f));
render_sphere(transform*tr1,color,radius);
render_sphere(transform*tr2,color,radius);
render_cylinder(transform,color,radius,halfLength);
}
void createSceneStacking()
{
const float cubeSize = 1.0f;
/*
createPyramid(PfxVector3(-20.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
createWall(PfxVector3(-2.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
createWall(PfxVector3(4.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
createWall(PfxVector3(10.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
createTowerCircle(PfxVector3(25.0f,0.0f,0.0f),8,24,PfxVector3(cubeSize,cubeSize,cubeSize));
*/
//createTowerCircle(PfxVector3(0.0f,0.0f,0.0f),48,24,PfxVector3(1));
createStack(PfxVector3(0.0f,0.0,0.0f),2,PfxVector3(cubeSize,cubeSize,cubeSize));
}
void pfxGetShapeAabbCylinder(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
PfxVector3 capSize = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) *
PfxVector3(shape.getCylinder().m_halfLen,shape.getCylinder().m_radius,shape.getCylinder().m_radius);
aabbMin = shape.getOffsetPosition() - capSize;
aabbMax = shape.getOffsetPosition() + capSize;
}
void render_sphere(
const PfxTransform3 &transform,
const PfxVector3 &color,
const PfxFloatInVec &radius)
{
PfxMatrix4 wMtx = PfxMatrix4(transform) * PfxMatrix4::scale(PfxVector3(radius));
glPushMatrix();
glMultMatrixf((GLfloat*)&wMtx);
glEnableClientState(GL_VERTEX_ARRAY);
glVertexPointer(3,GL_FLOAT,24,sphere_vtx);
glColor3fv((float*)&color);
glEnable(GL_POLYGON_OFFSET_FILL);
glPolygonOffset(1.0f,1.0f);
glDrawElements(GL_TRIANGLES,NUM_SPHERE_IDX,GL_UNSIGNED_SHORT,sphere_idx);
glDisable(GL_POLYGON_OFFSET_FILL);
glColor3f(0.0f,0.0f,0.0f);
glDrawElements(GL_LINES,NUM_SPHERE_IDX*2,GL_UNSIGNED_SHORT,sphere_wire_idx);
glDisableClientState(GL_VERTEX_ARRAY);
glPopMatrix();
}
void createSceneLandscape()
{
PfxCreateLargeTriMeshParam param;
param.verts = LargeMeshVtx;
param.numVerts = LargeMeshVtxCount;
param.vertexStrideBytes = sizeof(float)*6;
param.triangles = LargeMeshIdx;
param.numTriangles = LargeMeshIdxCount/3;
param.triangleStrideBytes = sizeof(unsigned short)*3;
if(gLargeMesh.m_numIslands > 0) {
pfxReleaseLargeTriMesh(gLargeMesh);
}
PfxInt32 ret = pfxCreateLargeTriMesh(gLargeMesh,param);
if(ret != SCE_PFX_OK) {
SCE_PFX_PRINTF("Can't create large mesh.\n");
}
int id = numRigidBodies++;
PfxShape shape;
shape.reset();
shape.setLargeTriMesh(&gLargeMesh);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,-5.0f,0.0f));
states[id].setOrientation(PfxQuat::rotationX(0.5f)*PfxQuat::rotationY(0.7f));
states[id].setMotionType(kPfxMotionTypeFixed);
states[id].setRigidBodyId(id);
}
void pfxGetSupportVertexBox(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)
{
PfxBox *box = (PfxBox*)shape;
PfxVector3 boxHalf = box->m_half + PfxVector3(SCE_PFX_GJK_MARGIN);
supportVertex[0] = seperatingAxis[0]>0.0f?boxHalf[0]:-boxHalf[0];
supportVertex[1] = seperatingAxis[1]>0.0f?boxHalf[1]:-boxHalf[1];
supportVertex[2] = seperatingAxis[2]>0.0f?boxHalf[2]:-boxHalf[2];
}
void physics_simulate()
{
PfxPerfCounter pc;
for(int i=1;i<numRigidBodies;i++) {
pfxApplyExternalForce(states[i],bodies[i],bodies[i].getMass()*PfxVector3(0.0f,-9.8f,0.0f),PfxVector3(0.0f),timeStep);
}
perf_push_marker("broadphase");
pc.countBegin("broadphase");
broadphase();
pc.countEnd();
perf_pop_marker();
perf_push_marker("collision");
pc.countBegin("collision");
collision();
pc.countEnd();
perf_pop_marker();
perf_push_marker("solver");
pc.countBegin("solver");
constraintSolver();
pc.countEnd();
perf_pop_marker();
perf_push_marker("sleepCheck");
pc.countBegin("sleepCheck");
sleepOrWakeup();
pc.countEnd();
perf_pop_marker();
perf_push_marker("integrate");
pc.countBegin("integrate");
integrate();
pc.countEnd();
perf_pop_marker();
perf_push_marker("castRays");
pc.countBegin("castRays");
castRays();
pc.countEnd();
perf_pop_marker();
frame++;
if(frame%100 == 0) {
float broadphaseTime = pc.getCountTime(0);
float collisionTime = pc.getCountTime(2);
float solverTime = pc.getCountTime(4);
float sleepTime = pc.getCountTime(6);
float integrateTime = pc.getCountTime(8);
float raycastTime = pc.getCountTime(10);
SCE_PFX_PRINTF("frame %3d broadphase %.2f collision %.2f solver %.2f sleepCheck %.2f integrate %.2f raycast %.2f | total %.2f\n",frame,
broadphaseTime,collisionTime,solverTime,sleepTime,integrateTime,raycastTime,
broadphaseTime+collisionTime+solverTime+sleepTime+integrateTime+raycastTime);
}
}
inline
PfxFloat
VertexBFaceATest(
PfxBool& inVoronoi,
PfxFloat& t0,
PfxFloat& t1,
PfxVector3& ptsVec,
const PfxVector3& hA,
PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,
PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,
PfxFloat signB,
PfxFloat scaleB )
{
// compute endpoint of capsule in box's coordinate system
PfxVector3 endpoint = PfxVector3( offsetAB + capsDirection * scaleB );
// compute the parameters of the point on the box face closest to this corner.
t0 = endpoint[0];
t1 = endpoint[1];
if ( t0 > hA[0] )
t0 = hA[0];
else if ( t0 < -hA[0] )
t0 = -hA[0];
if ( t1 > hA[1] )
t1 = hA[1];
else if ( t1 < -hA[1] )
t1 = -hA[1];
// get vector from face point to capsule endpoint
endpoint[0] -= t0;
endpoint[1] -= t1;
ptsVec = PfxVector3(endpoint);
// do the Voronoi test: already know the point on B is in the Voronoi region of the
// point on A, check the reverse.
inVoronoi = ( -signB * dot(ptsVec,capsDirection) >= voronoiTol );
return (lengthSqr(ptsVec));
}
void createSceneJoints()
{
const int n = 10;
int startId = numRigidBodies;
PfxVector3 boxSize(1.0f);
PfxFloat boxMass = 1.0f;
for(int i=0;i<n;i++) {
createBrick(numRigidBodies++,PfxVector3(0,3.0f+i*2.5f*boxSize[1],0),PfxQuat::identity(),boxSize,boxMass);
}
for(int i=startId;i<startId+n;i++) {
PfxRigidState &stateA = states[i];
PfxRigidState &stateB = states[(i+1)%numRigidBodies];
PfxVector3 anchor;
if(i == numRigidBodies-1) {
anchor = stateA.getPosition() + PfxVector3(0,boxSize[1],0);
}
else {
anchor = ( stateA.getPosition() + stateB.getPosition() ) * 0.5f;
}
PfxSwingTwistJointInitParam jparam;
jparam.anchorPoint = anchor;
jparam.twistAxis = PfxVector3(0,1,0);
pfxInitializeSwingTwistJoint(joints[numJoints],stateA,stateB,jparam);
joints[numJoints].m_constraints[4].m_damping = 0.1f;
joints[numJoints].m_constraints[5].m_damping = 0.1f;
SCE_PFX_ASSERT(numJoints<NUM_JOINTS);
numJoints++;
}
states[startId].setLinearVelocity(PfxVector3(0,0,5));
states[startId].setLinearDamping(0.95f);
states[startId].setAngularDamping(0.95f);
}
void render_init()
{
screen_width = DISPLAY_WIDTH;
screen_height = DISPLAY_HEIGHT;
// initalize matrix
g_pMat = PfxMatrix4::perspective(3.1415f/4.0f, (float)screen_width/(float)screen_height,0.1f, 1000.0f);
// initalize parameters
lightRadius = 40.0f;
lightRadX = -0.6f;
lightRadY = 0.6f;
viewRadius = 40.0f;
viewRadX = -0.01f;
viewRadY = 0.0f;
viewHeight = 1.0f;
g_viewTgt = PfxVector3(0.0f,viewHeight,0.0f);
box_wire_idx = new unsigned short [NUM_BOX_IDX*2];
sphere_wire_idx = new unsigned short [NUM_SPHERE_IDX*2];
cylinder_wire_idx = new unsigned short [NUM_CYLINDER_IDX*2];
for(int i=0;i<NUM_BOX_IDX/3;i++) {
box_wire_idx[i*6 ] = box_idx[i*3 ];
box_wire_idx[i*6+1] = box_idx[i*3+1];
box_wire_idx[i*6+2] = box_idx[i*3+1];
box_wire_idx[i*6+3] = box_idx[i*3+2];
box_wire_idx[i*6+4] = box_idx[i*3+2];
box_wire_idx[i*6+5] = box_idx[i*3 ];
}
for(int i=0;i<NUM_SPHERE_IDX/3;i++) {
sphere_wire_idx[i*6 ] = sphere_idx[i*3 ];
sphere_wire_idx[i*6+1] = sphere_idx[i*3+1];
sphere_wire_idx[i*6+2] = sphere_idx[i*3+1];
sphere_wire_idx[i*6+3] = sphere_idx[i*3+2];
sphere_wire_idx[i*6+4] = sphere_idx[i*3+2];
sphere_wire_idx[i*6+5] = sphere_idx[i*3 ];
}
for(int i=0;i<NUM_CYLINDER_IDX/3;i++) {
cylinder_wire_idx[i*6 ] = cylinder_idx[i*3 ];
cylinder_wire_idx[i*6+1] = cylinder_idx[i*3+1];
cylinder_wire_idx[i*6+2] = cylinder_idx[i*3+1];
cylinder_wire_idx[i*6+3] = cylinder_idx[i*3+2];
cylinder_wire_idx[i*6+4] = cylinder_idx[i*3+2];
cylinder_wire_idx[i*6+5] = cylinder_idx[i*3 ];
}
numMesh = 0;
}
void pfxSetupBallJoint(
PfxJoint &joint,
const PfxRigidState &stateA,
const PfxRigidState &stateB,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB,
PfxFloat timeStep
)
{
PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
PfxVector3 vAB = vA-vB;
PfxVector3 distance = (stateA.getPosition() + rA) - (stateB.getPosition() + rB);
PfxMatrix3 worldFrameA,worldFrameB;
worldFrameA = PfxMatrix3(solverBodyA.m_orientation) * joint.m_frameA;
worldFrameB = PfxMatrix3(solverBodyB.m_orientation) * joint.m_frameB;
// Linear Constraint
PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(solverBodyA.m_massInv + solverBodyB.m_massInv)) -
crossMatrix(rA) * solverBodyA.m_inertiaInv * crossMatrix(rA) -
crossMatrix(rB) * solverBodyB.m_inertiaInv * crossMatrix(rB);
for(int c=0;c<3;c++) {
PfxJointConstraint &jointConstraint = joint.m_constraints[c];
PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
PfxVector3 normal = worldFrameA[c];
PfxFloat posErr = dot(distance,-normal);
PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
PfxFloat upperLimit = jointConstraint.m_maxImpulse;
PfxFloat velocityAmp = 1.0f;
pfxCalcLinearLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);
PfxFloat denom = dot(K*normal,normal);
constraint.m_rhs = -velocityAmp*dot(vAB,normal);
constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep;
constraint.m_rhs *= jointConstraint.m_weight/denom;
constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom;
constraint.m_lowerLimit = lowerLimit;
constraint.m_upperLimit = upperLimit;
pfxStoreVector3(normal,constraint.m_normal);
}
}
void physics_create_scene(int sceneId)
{
const int numScenes = 4;
int sid = sceneId % numScenes;
numRigidBodies= 0;
pairSwap = 0;
numPairs[0] = 0;
numPairs[1] = 0;
numContacts = 0;
numContactIdPool = 0;
numJoints = 0;
island = NULL;
frame = 0;
doAreaRaycast = false;
switch(sid) {
case 0: // simple primitives
createSceneBoxGround();
createScenePrimitives();
initRays(PfxVector3(15.0f,5.0f,0.0f),PfxVector3(3.0f,1.0f,0.0f));
break;
case 1: // landscape
createSceneLandscape();
createScenePrimitives();
initRays(PfxVector3(15.0f,2.5f,0.0f),PfxVector3(3.0f,-2.5f,0.0f));
break;
case 2: // raycast filtering
createSceneRayFilter();
break;
case 3: // area raycast
createSceneBoxGround();
createSceneFalling();
initRays(PfxVector3(15.0f,5.0f,0.0f),PfxVector3(14.0f,-1.0f,0.0f));
doAreaRaycast = true;
break;
}
SCE_PFX_PRINTF("----- Size of rigid body buffer ------\n");
SCE_PFX_PRINTF(" size * num = total\n");
SCE_PFX_PRINTF("PfxRigidState %5d * %5d = %5d bytes\n",sizeof(PfxRigidState),numRigidBodies,sizeof(PfxRigidState)*numRigidBodies);
SCE_PFX_PRINTF("PfxRigidBody %5d * %5d = %5d bytes\n",sizeof(PfxRigidBody),numRigidBodies,sizeof(PfxRigidBody)*numRigidBodies);
SCE_PFX_PRINTF("PfxCollidable %5d * %5d = %5d bytes\n",sizeof(PfxCollidable),numRigidBodies,sizeof(PfxCollidable)*numRigidBodies);
SCE_PFX_PRINTF("PfxJoint %5d * %5d = %5d bytes\n",sizeof(PfxJoint),numJoints,sizeof(PfxJoint)*numJoints);
SCE_PFX_PRINTF("PfxSolverBody %5d * %5d = %5d bytes\n",sizeof(PfxSolverBody),numRigidBodies,sizeof(PfxSolverBody)*numRigidBodies);
SCE_PFX_PRINTF("PfxBroadphaseProxy %5d * %5d = %5d bytes\n",sizeof(PfxBroadphaseProxy),numRigidBodies,sizeof(PfxBroadphaseProxy)*numRigidBodies);
SCE_PFX_PRINTF("PfxContactManifold %5d * %5d = %5d bytes\n",sizeof(PfxContactManifold),NUM_CONTACTS,sizeof(PfxContactManifold)*NUM_CONTACTS);
SCE_PFX_PRINTF("PfxBroadphasePair %5d * %5d = %5d bytes\n",sizeof(PfxBroadphasePair),NUM_CONTACTS,sizeof(PfxBroadphasePair)*NUM_CONTACTS);
int totalBytes =
(sizeof(PfxRigidState) + sizeof(PfxRigidBody) + sizeof(PfxCollidable) + sizeof(PfxSolverBody) + sizeof(PfxBroadphaseProxy)) * numRigidBodies +
(sizeof(PfxContactManifold) + sizeof(PfxBroadphasePair)) * NUM_CONTACTS;
SCE_PFX_PRINTF("----------------------------------------------------------\n");
SCE_PFX_PRINTF("Total %5d bytes\n",totalBytes);
}
static SCE_PFX_FORCE_INLINE
bool pfxContactTriangleConvex(PfxContactCache &contacts,PfxUInt32 facetId,
const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
PfxUInt32 edgeChk,
const PfxConvexMesh &convex)
{
PfxVector3 facetPnts[6] = {
p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
};
PfxPoint3 pA(0.0f),pB(0.0f);
PfxVector3 nml(0.0f);
PfxGjkSolver gjk;
gjk.setup((void*)facetPnts,(void*)&convex,pfxGetSupportVertexTriangleWithThickness,pfxGetSupportVertexConvex);
PfxFloat d = gjk.collide(nml,pA,pB,PfxTransform3::identity(),PfxTransform3::identity(),SCE_PFX_FLT_MAX);
if(d >= 0.0f) return false;
PfxVector3 pointsOnTriangle = PfxVector3(pA);
PfxVector3 pointsOnConvex = PfxVector3(pB);
PfxVector3 axis = nml;
// 面上の最近接点が凸エッジ上でない場合は法線を変える
if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
axis=-normal;
}
PfxSubData subData;
subData.setFacetId(facetId);
contacts.addContactPoint(-length(pointsOnTriangle-pointsOnConvex),axis,pA,pB,subData);
return true;
}
void createSceneBoxGround()
{
int id = numRigidBodies++;
PfxBox box(150.0f,2.5f,150.0f);
PfxShape shape;
shape.reset();
shape.setBox(box);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,-2.5f,0.0f));
states[id].setMotionType(kPfxMotionTypeFixed);
states[id].setRigidBodyId(id);
}
void createTowerCircle(const PfxVector3 &offsetPosition,int stackSize,int rotSize,const PfxVector3 &boxSize)
{
PfxFloat radius = 1.3f * rotSize * boxSize[0] / SCE_PFX_PI;
// create active boxes
PfxQuat rotY = PfxQuat::identity();
PfxFloat posY = boxSize[1];
for(int i=0;i<stackSize;i++) {
for(int j=0;j<rotSize;j++) {
createBrick(numRigidBodies++,offsetPosition+rotate(rotY,PfxVector3(0.0f , posY, radius)),rotY,boxSize,0.5f);
rotY *= PfxQuat::rotationY(SCE_PFX_PI/(rotSize*0.5f));
}
posY += boxSize[1] * 2.0f;
rotY *= PfxQuat::rotationY(SCE_PFX_PI/(PfxFloat)rotSize);
}
}
void createSceneFalling()
{
int size = 6;
const float cubeSize = 1.0f;
float spacing = cubeSize * 5.0f;
PfxVector3 pos(0.0f, cubeSize * 2, 0.0f);
float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
for(int k=0;k<5;k++) {
for(int j=0;j<size;j++) {
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for(int i=0;i<size;i++) {
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
createBrick(pos,PfxQuat::identity(),PfxVector3(cubeSize),1.0f);
}
}
offset -= 0.05f * spacing * (size-1);
spacing *= 1.01f;
pos[1] += (cubeSize * 2.0f + spacing);
}
}
void castRays()
{
PfxRayCastParam param;
if(doAreaRaycast) {
static PfxFloat deltaRotY = 0.0f;
PfxQuat rotY = PfxQuat::rotationY(deltaRotY);
areaCenter = rotate(rotY,PfxVector3(15.0f,0.0f,0.0f));
deltaRotY += 0.02f;
int num = gatherBroadphaseProxiesInArea(areaCenter,areaExtent);
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.numProxies = num;
param.rangeCenter = areaCenter;
param.rangeExtent = areaExtent;
}
else {
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.numProxies = numRigidBodies;
param.rangeCenter = worldCenter;
param.rangeExtent = worldExtent;
}
pfxCastRays(rayInputs,rayOutputs,numRays,param);
}
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;
}
void render(void)
{
render_begin();
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
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;
switch(shape.getType()) {
case kPfxShapeSphere:
render_sphere(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getSphere().m_radius));
break;
case kPfxShapeBox:
render_box(
worldT,
PfxVector3(1,1,1),
shape.getBox().m_half);
break;
case kPfxShapeCapsule:
render_capsule(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
render_cylinder(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
render_mesh(
worldT,
PfxVector3(1,1,1),
convexMeshId);
break;
case kPfxShapeLargeTriMesh:
render_mesh(
worldT,
PfxVector3(1,1,1),
landscapeMeshId);
break;
default:
break;
}
}
}
render_end();
}
void createSceneStacking()
{
createTowerCircle(PfxVector3(0.0f,0.0f,0.0f),8,24,PfxVector3(1));
}
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();
}
void createScenePrimitives()
{
// sphere
{
int id = numRigidBodies++;
PfxSphere sphere(1.0f);
PfxShape shape;
shape.reset();
shape.setSphere(sphere);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(1.0f);
bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));
states[id].reset();
states[id].setPosition(PfxVector3(-5.0f,5.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// box
{
int id = numRigidBodies++;
PfxBox box(1.0f,1.0f,1.0f);
PfxShape shape;
shape.reset();
shape.setBox(box);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(1.0f);
bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(1.0f),1.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,5.0f,5.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// capsule
{
int id = numRigidBodies++;
PfxCapsule capsule(1.5f,0.5f);
PfxShape shape;
shape.reset();
shape.setCapsule(capsule);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(2.0f);
bodies[id].setInertia(pfxCalcInertiaCylinderX(2.0f,0.5f,2.0f));
states[id].reset();
states[id].setPosition(PfxVector3(5.0f,5.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// cylinder
{
int id = numRigidBodies++;
PfxCylinder cylinder(0.5f,1.5f);
PfxShape shape;
shape.reset();
shape.setCylinder(cylinder);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(3.0f);
bodies[id].setInertia(pfxCalcInertiaCylinderX(0.5f,1.5f,3.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,10.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// convex mesh
{
PfxCreateConvexMeshParam param;
param.verts = BarrelVtx;
param.numVerts = BarrelVtxCount;
param.vertexStrideBytes = sizeof(float)*6;
param.triangles = BarrelIdx;
param.numTriangles = BarrelIdxCount/3;
param.triangleStrideBytes = sizeof(unsigned short)*3;
PfxInt32 ret = pfxCreateConvexMesh(gConvex,param);
if(ret != SCE_PFX_OK) {
SCE_PFX_PRINTF("Can't create gConvex mesh.\n");
}
int id = numRigidBodies++;
PfxShape shape;
shape.reset();
shape.setConvexMesh(&gConvex);
collidables[id].reset();
collidables[id].addShape(shape);
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(3.0f);
bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,15.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
// combined primitives
{
int id = numRigidBodies++;
//E Both shapes and incides buffer have to be kept when creating a combined shape.
static PfxShape shapes[3];
PfxUInt16 shapeIds[3]={0,1,2};
collidables[id].reset(shapes,shapeIds,3);
{
PfxBox box(0.5f,0.5f,1.5f);
PfxShape shape;
shape.reset();
shape.setBox(box);
shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f));
collidables[id].addShape(shape);
}
{
PfxBox box(0.5f,1.5f,0.5f);
PfxShape shape;
shape.reset();
shape.setBox(box);
shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f));
collidables[id].addShape(shape);
}
{
PfxCapsule cap(1.5f,0.5f);
PfxShape shape;
shape.reset();
shape.setCapsule(cap);
collidables[id].addShape(shape);
}
collidables[id].finish();
bodies[id].reset();
bodies[id].setMass(3.0f);
bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(2.5f,1.0f,1.0f),3.0f));
states[id].reset();
states[id].setPosition(PfxVector3(0.0f,5.0f,0.0f));
states[id].setMotionType(kPfxMotionTypeActive);
states[id].setRigidBodyId(id);
}
}
PfxVector3 physics_pick_start(const PfxVector3 &p1,const PfxVector3 &p2)
{
return PfxVector3(0.0f);
}
void pfxGetShapeAabbSphere(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
aabbMin = shape.getOffsetPosition() - PfxVector3(shape.getSphere().m_radius);
aabbMax = shape.getOffsetPosition() + PfxVector3(shape.getSphere().m_radius);
}
void render(void)
{
render_begin();
const PfxVector3 colorWhite(1.0f);
const PfxVector3 colorGray(0.7f);
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 color = state.isAsleep()?colorGray:colorWhite;
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
switch(shape.getType()) {
case kPfxShapeSphere:
render_sphere(
worldT,
color,
PfxFloatInVec(shape.getSphere().m_radius));
break;
case kPfxShapeBox:
render_box(
worldT,
color,
shape.getBox().m_half);
break;
case kPfxShapeCapsule:
render_capsule(
worldT,
color,
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
render_cylinder(
worldT,
color,
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
render_mesh(
worldT,
color,
convexMeshId);
break;
case kPfxShapeLargeTriMesh:
render_mesh(
worldT,
color,
landscapeMeshId);
break;
default:
break;
}
}
}
render_debug_begin();
#ifdef ENABLE_DEBUG_DRAW_CONTACT
for(int i=0;i<physics_get_num_contacts();i++) {
const PfxContactManifold &contact = physics_get_contact(i);
const PfxRigidState &stateA = physics_get_state(contact.getRigidBodyIdA());
const PfxRigidState &stateB = physics_get_state(contact.getRigidBodyIdB());
for(int j=0;j<contact.getNumContacts();j++) {
const PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 pA = stateA.getPosition()+rotate(stateA.getOrientation(),pfxReadVector3(cp.m_localPointA));
const float w = 0.05f;
render_debug_line(pA+PfxVector3(-w,0.0f,0.0f),pA+PfxVector3(w,0.0f,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,-w,0.0f),pA+PfxVector3(0.0f,w,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,0.0f,-w),pA+PfxVector3(0.0f,0.0f,w),PfxVector3(0,0,1));
}
}
#endif
#ifdef ENABLE_DEBUG_DRAW_AABB
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
render_debug_box(center,half,PfxVector3(1,0,0));
}
#endif
#ifdef ENABLE_DEBUG_DRAW_ISLAND
const PfxIsland *island = physics_get_islands();
if(island) {
for(PfxUInt32 i=0;i<pfxGetNumIslands(island);i++) {
PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,i);
PfxVector3 aabbMin(SCE_PFX_FLT_MAX);
PfxVector3 aabbMax(-SCE_PFX_FLT_MAX);
for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {
const PfxRigidState &state = physics_get_state(pfxGetUnitId(islandUnit));
const PfxCollidable &coll = physics_get_collidable(pfxGetUnitId(islandUnit));
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
aabbMin = minPerElem(aabbMin,center-half);
aabbMax = maxPerElem(aabbMax,center+half);
}
render_debug_box((aabbMax+aabbMin)*0.5f,(aabbMax-aabbMin)*0.5f,PfxVector3(0,1,0));
}
}
#endif
for(int i=0;i<physics_get_num_rays();i++) {
const PfxRayInput& rayInput = physics_get_rayinput(i);
const PfxRayOutput& rayOutput = physics_get_rayoutput(i);
if(rayOutput.m_contactFlag) {
render_debug_line(
rayInput.m_startPosition,
rayOutput.m_contactPoint,
PfxVector3(1.0f,0.0f,1.0f));
render_debug_line(
rayOutput.m_contactPoint,
rayOutput.m_contactPoint+rayOutput.m_contactNormal,
PfxVector3(1.0f,0.0f,1.0f));
}
else {
render_debug_line(rayInput.m_startPosition,
rayInput.m_startPosition+rayInput.m_direction,
PfxVector3(0.5f,0.0f,0.5f));
}
}
extern bool doAreaRaycast;
extern PfxVector3 areaCenter;
extern PfxVector3 areaExtent;
if(doAreaRaycast) {
render_debug_box(areaCenter,areaExtent,PfxVector3(0,0,1));
}
render_debug_end();
render_end();
}
PfxInt32 pfxContactTriMeshSphere(
PfxContactCache &contacts,
const PfxTriMesh *meshA,
const PfxTransform3 &transformA,
const PfxSphere &sphereB,
const PfxTransform3 &transformB,
PfxFloat distanceThreshold)
{
(void) distanceThreshold;
PfxTransform3 transformAB,transformBA;
PfxMatrix3 matrixBA;
PfxVector3 offsetBA;
// Bローカル→Aローカルへの変換
transformAB = orthoInverse(transformA) * transformB;
// Aローカル→Bローカルへの変換
transformBA = orthoInverse(transformAB);
matrixBA = transformBA.getUpper3x3();
offsetBA = transformBA.getTranslation();
//-------------------------------------------
// 判定する面を絞り込む
PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = {0};
PfxUInt32 numSelFacets = 0;
PfxVector3 aabbB(sphereB.m_radius);
numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
if(numSelFacets == 0) {
return 0;
}
//-----------------------------------------------
// 判定
PfxContactCache localContacts;
// TriangleMeshの面->sphereの判定
// ※TriangleMesh座標系
{
for(PfxUInt32 f = 0; f < numSelFacets; f++ ) {
const PfxFacet &facet = meshA->m_facets[selFacets[f]];
const PfxVector3 facetNormal = pfxReadVector3(facet.m_normal);
const PfxVector3 facetPnts[3] = {
meshA->m_verts[facet.m_vertIds[0]],
meshA->m_verts[facet.m_vertIds[1]],
meshA->m_verts[facet.m_vertIds[2]],
};
const PfxEdge *edge[3] = {
&meshA->m_edges[facet.m_edgeIds[0]],
&meshA->m_edges[facet.m_edgeIds[1]],
&meshA->m_edges[facet.m_edgeIds[2]],
};
PfxVector3 sepAxis,pntA,pntB;
PfxUInt32 edgeChk =
((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |
((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |
((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);
pfxContactTriangleSphere(localContacts,selFacets[f],
facetNormal,facetPnts[0],facetPnts[1],facetPnts[2],
facet.m_thickness,
0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),
0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),
0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),
edgeChk,
sphereB.m_radius,transformAB.getTranslation());
}
}
for(int i=0;i<localContacts.getNumContacts();i++) {
PfxSubData subData = localContacts.getSubData(i);
const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
PfxTriangle triangleA(
meshA->m_verts[facet.m_vertIds[0]],
meshA->m_verts[facet.m_vertIds[1]],
meshA->m_verts[facet.m_vertIds[2]]);
PfxFloat s=0.0f,t=0.0f;
pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);
subData.m_type = PfxSubData::MESH_INFO;
subData.setFacetLocalS(s);
subData.setFacetLocalT(t);
contacts.addContactPoint(
localContacts.getDistance(i),
transformA.getUpper3x3() * localContacts.getNormal(i),
localContacts.getLocalPointA(i),
transformBA * localContacts.getLocalPointB(i),
subData);
}
return contacts.getNumContacts();
}
