void pfxGetShapeAabbLargeTriMesh(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
const PfxLargeTriMesh *largemesh = shape.getLargeTriMesh();
PfxVector3 half = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * largemesh->m_half;
aabbMin = shape.getOffsetPosition() - half;
aabbMax = shape.getOffsetPosition() + half;
}
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 pfxGetShapeAabbConvexMesh(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
const PfxConvexMesh *convex = shape.getConvexMesh();
PfxVector3 half = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * convex->m_half;
aabbMin = shape.getOffsetPosition() - half;
aabbMax = shape.getOffsetPosition() + half;
}
//-------------------------------------------------------
void voxAABBShape::getNormal (const coAABB& _aabb, coVec3& _normal) const
{
const coVec3 center = _aabb.getCenter();
const coVec3 absMinDelta = absPerElem(center - m_aabb.getMin());
const coVec3 absMaxDelta = absPerElem(center - m_aabb.getMax());
coUint axisIndex[2];
axisIndex[0] = coMath_f::minIndex(absMinDelta.getX(), absMinDelta.getY(), absMinDelta.getZ());
axisIndex[1] = coMath_f::minIndex(absMaxDelta.getX(), absMaxDelta.getY(), absMaxDelta.getZ());
const coUint minAxisIndex = coMath_f::minIndex(absMinDelta[axisIndex[0]], absMaxDelta[axisIndex[1]]);
switch (axisIndex[minAxisIndex])
{
case 0: _normal = minAxisIndex ? coVec3::sGetXAxis() : -coVec3::sGetXAxis(); break;
case 1: _normal = minAxisIndex ? coVec3::sGetYAxis() : -coVec3::sGetYAxis(); break;
case 2: _normal = minAxisIndex ? coVec3::sGetZAxis() : -coVec3::sGetZAxis(); break;
default: break;
}
}
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;
}
PfxInt32 pfxCreateLargeTriMesh(PfxLargeTriMesh &lmesh,const PfxCreateLargeTriMeshParam ¶m)
{
// Check input
if(param.numVerts == 0 || param.numTriangles == 0 || !param.verts || !param.triangles)
return SCE_PFX_ERR_INVALID_VALUE;
if(param.islandsRatio < 0.0f || param.islandsRatio > 1.0f)
return SCE_PFX_ERR_OUT_OF_RANGE;
if(param.numFacetsLimit == 0 || param.numFacetsLimit > SCE_PFX_NUMMESHFACETS)
return SCE_PFX_ERR_OUT_OF_RANGE;
const PfxFloat epsilon = 0.00001f;
PfxArray<PfxMcVert> vertList(param.numVerts); // 頂点配列
PfxArray<PfxMcFacet> facetList(param.numTriangles); // 面配列
PfxArray<PfxMcEdge> edgeList(param.numTriangles*3); // エッジ配列
PfxArray<PfxMcEdge*> edgeHead(param.numTriangles*3);
//J 頂点配列作成
for(PfxUInt32 i=0;i<param.numVerts;i++) {
PfxFloat *vtx = (PfxFloat*)((uintptr_t)param.verts + param.vertexStrideBytes * i);
PfxMcVert mcv;
mcv.flag = 0;
mcv.i = i;
mcv.coord = pfxReadVector3(vtx);
vertList.push(mcv);
}
// 面配列作成
for(PfxUInt32 i=0;i<param.numTriangles;i++) {
void *ids = (void*)((uintptr_t)param.triangles + param.triangleStrideBytes * i);
PfxUInt32 idx[3];
if(param.flag & SCE_PFX_MESH_FLAG_32BIT_INDEX) {
if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
idx[0] = ((PfxUInt32*)ids)[2];
idx[1] = ((PfxUInt32*)ids)[1];
idx[2] = ((PfxUInt32*)ids)[0];
}
else {
idx[0] = ((PfxUInt32*)ids)[0];
idx[1] = ((PfxUInt32*)ids)[1];
idx[2] = ((PfxUInt32*)ids)[2];
}
}
else if(param.flag & SCE_PFX_MESH_FLAG_16BIT_INDEX) {
if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
idx[0] = ((PfxUInt16*)ids)[2];
idx[1] = ((PfxUInt16*)ids)[1];
idx[2] = ((PfxUInt16*)ids)[0];
}
else {
idx[0] = ((PfxUInt16*)ids)[0];
idx[1] = ((PfxUInt16*)ids)[1];
idx[2] = ((PfxUInt16*)ids)[2];
}
}
else {
return SCE_PFX_ERR_INVALID_FLAG;
}
const PfxVector3 pnts[3] = {
vertList[idx[0]].coord,
vertList[idx[1]].coord,
vertList[idx[2]].coord,
};
// 面積が０の面を排除
PfxFloat area = lengthSqr(cross(pnts[1]-pnts[0],pnts[2]-pnts[0]));
if((param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) && area < 0.00001f) {
continue;
}
PfxMcFacet facet;
facet.v[0] = &vertList[idx[0]];
facet.v[1] = &vertList[idx[1]];
facet.v[2] = &vertList[idx[2]];
facet.e[0] = facet.e[1] = facet.e[2] = NULL;
facet.n = normalize(cross(pnts[2]-pnts[1],pnts[0]-pnts[1]));
facet.area = area;
facet.thickness = param.defaultThickness;
facet.neighbor[0] = facet.neighbor[1] = facet.neighbor[2] = -1;
facet.neighborEdgeId[0] = facet.neighborEdgeId[1] = facet.neighborEdgeId[2] = -1;
facetList.push(facet);
}
const PfxUInt32 numTriangles = facetList.size();
{
PfxArray<PfxMcTriList> triEntry(numTriangles*3);
PfxArray<PfxMcTriList*> triHead(numTriangles*3); // 頂点から面への参照リスト
PfxInt32 cnt = 0;
PfxMcTriList* nl = NULL;
triEntry.assign(numTriangles*3,PfxMcTriList());
triHead.assign(numTriangles*3,nl);
// 頂点から面への参照リストを作成
for(PfxUInt32 i=0;i<numTriangles;i++) {
for(PfxUInt32 v=0;v<3;v++) {
PfxUInt32 vertId = facetList[i].v[v]->i;
triEntry[cnt].facet = &facetList[i];
triEntry[cnt].next = triHead[vertId];
triHead[vertId] = &triEntry[cnt++];
}
}
// 同一頂点をまとめる
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) {
for(PfxUInt32 i=0;i<param.numVerts;i++) {
if(vertList[i].flag == 1) continue;
for(PfxUInt32 j=i+1;j<param.numVerts;j++) {
if(vertList[j].flag == 1) continue;
PfxFloat lenSqr = lengthSqr(vertList[i].coord-vertList[j].coord);
if(lenSqr < epsilon) {
//SCE_PFX_PRINTF("same position %d,%d\n",i,j);
vertList[j].flag = 1; // 同一点なのでフラグを立てる
for(PfxMcTriList *f=triHead[j];f!=NULL;f=f->next) {
for(PfxInt32 k=0;k<3;k++) {
if(f->facet->v[k] == &vertList[j]) {
f->facet->v[k] = &vertList[i]; // 頂点を付け替える
break;
}
}
}
}
}
}
}
}
// 接続面間の角度を算出して面にセット
PfxMcEdge *nl = NULL;
edgeHead.assign(numTriangles*3,nl);
edgeList.assign(numTriangles*3,PfxMcEdge());
// エッジ配列の作成
PfxUInt32 ecnt = 0;
for(PfxUInt32 i=0;i<numTriangles;i++) {
PfxMcFacet &f = facetList[i];
for(PfxUInt32 v=0;v<3;v++) {
uintptr_t vp1 = ((uintptr_t)f.v[v]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert);
uintptr_t vp2 = ((uintptr_t)f.v[(v+1)%3]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert);
PfxUInt32 viMin = SCE_PFX_MIN(vp1,vp2);
PfxUInt32 viMax = SCE_PFX_MAX(vp1,vp2);
PfxInt32 key = ((0x8da6b343*viMin+0xd8163841*viMax)%(numTriangles*3));
for(PfxMcEdge *e = edgeHead[key];;e=e->next) {
if(!e) {
edgeList[ecnt].vertId[0] = viMin;
edgeList[ecnt].vertId[1] = viMax;
edgeList[ecnt].facetId[0] = i;
edgeList[ecnt].edgeId[0] = v;
edgeList[ecnt].numFacets = 1;
edgeList[ecnt].next = edgeHead[key];
edgeList[ecnt].angleType = SCE_PFX_EDGE_CONVEX;
edgeList[ecnt].angle = 0.0f;
edgeHead[key] = &edgeList[ecnt];
f.e[v] = &edgeList[ecnt];
ecnt++;
break;
}
if(e->vertId[0] == viMin && e->vertId[1] == viMax) {
SCE_PFX_ALWAYS_ASSERT_MSG(e->numFacets == 1,"An edge connected with over 2 triangles is invalid");
e->facetId[1] = i;
e->edgeId[1] = v;
e->numFacets = 2;
f.e[v] = e;
f.neighbor[v] = e->facetId[0];
f.neighborEdgeId[v] = e->edgeId[0];
facetList[e->facetId[0]].neighbor[e->edgeId[0]] = i;
facetList[e->facetId[0]].neighborEdgeId[e->edgeId[0]] = e->edgeId[1];
break;
}
}
}
}
// 角度を計算
for(PfxUInt32 i=0;i<numTriangles;i++) {
PfxMcFacet &facetA = facetList[i];
PfxQueue<PfxMcFacetLink> cqueue(ecnt);
for(PfxUInt32 j=0;j<3;j++) {
if(facetA.neighbor[j] >= 0) {
cqueue.push(PfxMcFacetLink(
j,
facetA.e[j]->vertId[0],facetA.e[j]->vertId[1],
i,j,
facetA.neighbor[j],facetA.neighborEdgeId[j]));
}
}
while(!cqueue.empty()) {
PfxMcFacetLink link = cqueue.front();
cqueue.pop();
PfxMcFacet &ofacet = facetList[link.ofacetId];
PfxMcEdge *edge = ofacet.e[link.oedgeId];
// facetAとのなす角を計算
{
// 面に含まれるが、このエッジに含まれない点
PfxUInt32 ids[3] = {2,0,1};
PfxVector3 v1 = facetA.v[ids[link.baseEdgeId]]->coord;
PfxVector3 v2 = ofacet.v[ids[link.oedgeId]]->coord;
// エッジの凹凸判定
PfxVector3 midPnt = (v1 + v2) * 0.5f;
PfxVector3 pntOnEdge = facetA.v[link.baseEdgeId]->coord;
PfxFloat chk1 = dot(facetA.n,midPnt-pntOnEdge);
PfxFloat chk2 = dot(ofacet.n,midPnt-pntOnEdge);
if(chk1 < -epsilon && chk2 < -epsilon) {
if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONVEX;
// 厚み角の判定に使う角度をセット
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
edge->angle = 0.5f*acosf(dot(facetA.n,ofacet.n));
}
}
else if(chk1 > epsilon && chk2 > epsilon) {
if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONCAVE;
}
else {
if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_FLAT;
}
}
// 次の接続面を登録（コメントアウトすると頂点で接続された面を考慮しない）
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
PfxInt32 nextEdgeId = (link.oedgeId+1)%3;
PfxMcEdge *nextEdge = ofacet.e[nextEdgeId];
if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i &&
((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 ||
(PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) {
cqueue.push(PfxMcFacetLink(
link.baseEdgeId,
link.vid1,link.vid2,
link.ofacetId,link.iedgeId,
ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId]));
}
nextEdgeId = (link.oedgeId+2)%3;
nextEdge = ofacet.e[nextEdgeId];
if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i &&
((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 ||
(PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) {
cqueue.push(PfxMcFacetLink(
link.baseEdgeId,
link.vid1,link.vid2,
link.ofacetId,link.iedgeId,
ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId]));
}
}
}
}
// 面に厚みを付ける
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
for(PfxUInt32 i=0;i<numTriangles;i++) {
PfxMcFacet &facetA = facetList[i];
for(PfxUInt32 j=0;j<numTriangles;j++) {
// 隣接面は比較対象にしない
if( i==j ||
j == (PfxInt32)facetA.e[0]->facetId[0] ||
j == (PfxInt32)facetA.e[0]->facetId[1] ||
j == (PfxInt32)facetA.e[1]->facetId[0] ||
j == (PfxInt32)facetA.e[1]->facetId[1] ||
j == (PfxInt32)facetA.e[2]->facetId[0] ||
j == (PfxInt32)facetA.e[2]->facetId[1]) {
continue;
}
PfxMcFacet &facetB = facetList[j];
// 交差判定
PfxFloat closestDistance=0;
if(intersect(facetA,facetB,closestDistance)) {
// 最近接距離/2を厚みとして採用
facetA.thickness = SCE_PFX_MAX(param.defaultThickness,SCE_PFX_MIN(facetA.thickness,closestDistance * 0.5f));
}
}
}
}
// 面の面積によって３種類に分類する
PfxFloat areaMin=SCE_PFX_FLT_MAX,areaMax=-SCE_PFX_FLT_MAX;
for(PfxUInt32 f=0;f<(PfxUInt32)numTriangles;f++) {
PfxVector3 pnts[3] = {
facetList[f].v[0]->coord,
facetList[f].v[1]->coord,
facetList[f].v[2]->coord,
};
areaMin = SCE_PFX_MIN(areaMin,facetList[f].area);
areaMax = SCE_PFX_MAX(areaMax,facetList[f].area);
// 面のAABBを算出
facetList[f].aabbMin = minPerElem(pnts[2],minPerElem(pnts[1],pnts[0]));
facetList[f].aabbMax = maxPerElem(pnts[2],maxPerElem(pnts[1],pnts[0]));
}
PfxFloat areaDiff = (areaMax-areaMin)/3.0f;
PfxFloat areaLevel0,areaLevel1;
areaLevel0 = areaMin + areaDiff;
areaLevel1 = areaMin + areaDiff * 2.0f;
PfxArray<PfxMcFacetPtr> facetsLv0(numTriangles);
PfxArray<PfxMcFacetPtr> facetsLv1(numTriangles);
PfxArray<PfxMcFacetPtr> facetsLv2(numTriangles);
for(PfxUInt32 f=0;f<numTriangles;f++) {
PfxFloat area = facetList[f].area;
PfxMcFacet *fct = &facetList[f];
if(area < areaLevel0) {
facetsLv0.push(fct);
}
else if(area > areaLevel1) {
facetsLv2.push(fct);
}
else {
facetsLv1.push(fct);
}
}
// アイランドの配列
PfxMcIslands islands;
PfxVector3 lmeshSize;
// レベル毎にPfxTriMeshを作成
if(!facetsLv0.empty()) {
// 全体のAABBを求める
PfxVector3 aabbMin,aabbMax,center,half;
aabbMin =facetsLv0[0]->aabbMin;
aabbMax = facetsLv0[0]->aabbMax;
for(PfxUInt32 f=1;f<facetsLv0.size();f++) {
aabbMin = minPerElem(facetsLv0[f]->aabbMin,aabbMin);
aabbMax = maxPerElem(facetsLv0[f]->aabbMax,aabbMax);
}
center = ( aabbMin + aabbMax ) * 0.5f;
half = ( aabbMax - aabbMin ) * 0.5f;
// 再帰的に処理
divideMeshes(
param.numFacetsLimit,param.islandsRatio,
islands,
facetsLv0,
center,half);
lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
}
if(!facetsLv1.empty()) {
// 全体のAABBを求める
PfxVector3 aabbMin,aabbMax,center,half;
aabbMin =facetsLv1[0]->aabbMin;
aabbMax = facetsLv1[0]->aabbMax;
for(PfxUInt32 f=1;f<facetsLv1.size();f++) {
aabbMin = minPerElem(facetsLv1[f]->aabbMin,aabbMin);
aabbMax = maxPerElem(facetsLv1[f]->aabbMax,aabbMax);
}
center = ( aabbMin + aabbMax ) * 0.5f;
half = ( aabbMax - aabbMin ) * 0.5f;
// 再帰的に処理
divideMeshes(
param.numFacetsLimit,param.islandsRatio,
islands,
facetsLv1,
center,half);
lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
}
if(!facetsLv2.empty()) {
// 全体のAABBを求める
PfxVector3 aabbMin,aabbMax,center,half;
aabbMin =facetsLv2[0]->aabbMin;
aabbMax = facetsLv2[0]->aabbMax;
for(PfxUInt32 f=1;f<facetsLv2.size();f++) {
aabbMin = minPerElem(facetsLv2[f]->aabbMin,aabbMin);
aabbMax = maxPerElem(facetsLv2[f]->aabbMax,aabbMax);
}
center = ( aabbMin + aabbMax ) * 0.5f;
half = ( aabbMax - aabbMin ) * 0.5f;
// 再帰的に処理
divideMeshes(
param.numFacetsLimit,param.islandsRatio,
islands,
facetsLv2,
center,half);
lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
}
lmesh.m_half = lmeshSize;
// Check Islands
//for(PfxInt32 i=0;i<islands.numIslands;i++) {
// SCE_PFX_PRINTF("island %d\n",i);
// for(PfxInt32 f=0;f<islands.facetsInIsland[i].size();f++) {
// PfxMcFacet *facet = islands.facetsInIsland[i][f];
// SCE_PFX_PRINTF(" %d %d %d\n",facet->v[0]->i,facet->v[1]->i,facet->v[2]->i);
// }
//}
// PfxLargeTriMeshの生成
if(islands.numIslands > 0) {
lmesh.m_numIslands = 0;
lmesh.m_aabbList = (PfxAabb16*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxAabb16)*islands.numIslands);
lmesh.m_islands = (PfxTriMesh*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxTriMesh)*islands.numIslands);
PfxInt32 maxFacets=0,maxVerts=0,maxEdges=0;
for(PfxUInt32 i=0;i<islands.numIslands;i++) {
PfxTriMesh island;
createIsland(island,islands.facetsInIsland[i]);
addIslandToLargeTriMesh(lmesh,island);
maxFacets = SCE_PFX_MAX(maxFacets,island.m_numFacets);
maxVerts = SCE_PFX_MAX(maxVerts,island.m_numVerts);
maxEdges = SCE_PFX_MAX(maxEdges,island.m_numEdges);
//SCE_PFX_PRINTF("island %d verts %d edges %d facets %d\n",i,island.m_numVerts,island.m_numEdges,island.m_numFacets);
}
SCE_PFX_PRINTF("generate completed!\n\tinput mesh verts %d triangles %d\n\tislands %d max triangles %d verts %d edges %d\n",
param.numVerts,param.numTriangles,
lmesh.m_numIslands,maxFacets,maxVerts,maxEdges);
SCE_PFX_PRINTF("\tsizeof(PfxLargeTriMesh) %d sizeof(PfxTriMesh) %d\n",sizeof(PfxLargeTriMesh),sizeof(PfxTriMesh));
}
else {
SCE_PFX_PRINTF("islands overflow! %d/%d\n",islands.numIslands,SCE_PFX_LARGETRIMESH_MAX_ISLANDS);
return SCE_PFX_ERR_OUT_OF_RANGE;
}
return SCE_PFX_OK;
}
void pfxGetShapeAabbBox(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
PfxVector3 boxSize = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * shape.getBox().m_half;
aabbMin = shape.getOffsetPosition() - boxSize;
aabbMax = shape.getOffsetPosition() + boxSize;
}
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 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 Physics::BroadPhase(RigidbodyState* states, Collider* colliders, unsigned int numRigidbodies,
const Pair *oldPairs, const unsigned int numOldPairs, Pair *newPairs, unsigned int &numNewPairs, const unsigned int maxPairs,
DefaultAllocator* allocator, void *userData, BroadPhaseCallback callback = NULL
){
assert(states);
assert(colliders);
assert(oldPairs);
assert(newPairs);
assert(allocator);
numNewPairs = 0;
// AABB交差ペアを見つける（総当たり）
// TODO:高速化
for (unsigned int i = 0; i<numRigidbodies; i++) {
for (unsigned int j = i + 1; j<numRigidbodies; j++) {
const RigidbodyState &stateA = states[i];
const Collider &collidableA = colliders[i];
const RigidbodyState &stateB = states[j];
const Collider &collidableB = colliders[j];
if (callback && !callback(i, j, userData)) {
continue;
}
Matrix3 orientationA(stateA.m_orientation);
Vector3 centerA = stateA.m_position + orientationA * collidableA.m_center;
Vector3 halfA = absPerElem(orientationA) * (collidableA.m_half + Vector3(AABB_EXPAND));// AABBサイズを若干拡張
Matrix3 orientationB(stateB.m_orientation);
Vector3 centerB = stateB.m_position + orientationB * collidableB.m_center;
Vector3 halfB = absPerElem(orientationB) * (collidableB.m_half + Vector3(AABB_EXPAND));// AABBサイズを若干拡張
if (IntersectAABB(centerA, halfA, centerB, halfB) && numNewPairs < maxPairs) {
//衝突情報を管理するクラスを作成
Pair &newPair = newPairs[numNewPairs++];
newPair.rigidBodyA = i<j ? i : j;
newPair.rigidBodyB = i<j ? j : i;
newPair.contact = NULL;
}
}
}
// ソート
{
Pair *sortBuff = (Pair*)allocator->allocate(sizeof(Pair)*numNewPairs);
Sort<Pair>(newPairs, sortBuff, numNewPairs);
allocator->deallocate(sortBuff);
}
// 新しく検出したペアと過去のペアを比較
Pair *outNewPairs = (Pair*)allocator->allocate(sizeof(Pair)*numNewPairs);
Pair *outKeepPairs = (Pair*)allocator->allocate(sizeof(Pair)*numOldPairs);
assert(outNewPairs);
assert(outKeepPairs);
unsigned int nNew = 0;
unsigned int nKeep = 0;
unsigned int oldId = 0, newId = 0;
while (oldId<numOldPairs&&newId<numNewPairs) {
if (newPairs[newId].key > oldPairs[oldId].key) {
// remove
allocator->deallocate(oldPairs[oldId].contact);
oldId++;
}
else if (newPairs[newId].key == oldPairs[oldId].key) {
// keep
assert(nKeep <= numOldPairs);
outKeepPairs[nKeep] = oldPairs[oldId];
nKeep++;
oldId++;
newId++;
}
else {
// new
assert(nNew <= numNewPairs);
outNewPairs[nNew] = newPairs[newId];
nNew++;
newId++;
}
};
if (newId<numNewPairs) {
// all new
for (; newId<numNewPairs; newId++, nNew++) {
assert(nNew <= numNewPairs);
outNewPairs[nNew] = newPairs[newId];
}
}
else if (oldId<numOldPairs) {
// all remove
for (; oldId<numOldPairs; oldId++) {
allocator->deallocate(oldPairs[oldId].contact);
}
}
for (unsigned int i = 0; i<nNew; i++) {
outNewPairs[i].contact = (Contact*)allocator->allocate(sizeof(Contact));
outNewPairs[i].contact->Reset();
}
for (unsigned int i = 0; i<nKeep; i++) {
outKeepPairs[i].contact->Refresh(
states[outKeepPairs[i].rigidBodyA].m_position,
states[outKeepPairs[i].rigidBodyA].m_orientation,
states[outKeepPairs[i].rigidBodyB].m_position,
states[outKeepPairs[i].rigidBodyB].m_orientation);
}
numNewPairs = 0;
for (unsigned int i = 0; i<nKeep; i++) {
outKeepPairs[i].type = PairTypeKeep;
newPairs[numNewPairs++] = outKeepPairs[i];
}
for (unsigned int i = 0; i<nNew; i++) {
outNewPairs[i].type = PairTypeNew;
newPairs[numNewPairs++] = outNewPairs[i];
}
allocator->deallocate(outKeepPairs);
allocator->deallocate(outNewPairs);
// ソート
{
Pair *sortBuff = (Pair*)allocator->allocate(sizeof(Pair)*numNewPairs);
Sort<Pair>(newPairs, sortBuff, numNewPairs);
allocator->deallocate(sortBuff);
}
}
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);
}
}
