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 pfxSetupContactConstraint(
PfxConstraintRow &constraintResponse,
PfxConstraintRow &constraintFriction1,
PfxConstraintRow &constraintFriction2,
PfxFloat penetrationDepth,
PfxFloat restitution,
PfxFloat friction,
const PfxVector3 &contactNormal,
const PfxVector3 &contactPointA,
const PfxVector3 &contactPointB,
const PfxRigidState &stateA,
const PfxRigidState &stateB,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB,
PfxFloat separateBias,
PfxFloat timeStep
)
{
(void)friction;
PfxVector3 rA = rotate(solverBodyA.m_orientation,contactPointA);
PfxVector3 rB = rotate(solverBodyB.m_orientation,contactPointB);
PfxFloat massInvA = solverBodyA.m_massInv;
PfxFloat massInvB = solverBodyB.m_massInv;
PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {
massInvB = 0.0f;
inertiaInvB = PfxMatrix3(0.0f);
}
if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {
massInvA = 0.0f;
inertiaInvA = PfxMatrix3(0.0f);
}
PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(massInvA + massInvB)) -
crossMatrix(rA) * inertiaInvA * crossMatrix(rA) -
crossMatrix(rB) * inertiaInvB * crossMatrix(rB);
PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
PfxVector3 vAB = vA-vB;
PfxVector3 tangent1,tangent2;
pfxGetPlaneSpace(contactNormal,tangent1,tangent2);
// Contact Constraint
{
PfxVector3 normal = contactNormal;
PfxFloat denom = dot(K*normal,normal);
constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error
constraintResponse.m_rhs -= (separateBias * SCE_PFX_MIN(0.0f,penetrationDepth+SCE_PFX_CONTACT_SLOP)) / timeStep; // position error
constraintResponse.m_rhs /= denom;
constraintResponse.m_jacDiagInv = 1.0f/denom;
constraintResponse.m_lowerLimit = 0.0f;
constraintResponse.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintResponse.m_normal);
}
// Friction Constraint 1
{
PfxVector3 normal = tangent1;
PfxFloat denom = dot(K*normal,normal);
constraintFriction1.m_jacDiagInv = 1.0f/denom;
constraintFriction1.m_rhs = -dot(vAB,normal);
constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;
constraintFriction1.m_lowerLimit = 0.0f;
constraintFriction1.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintFriction1.m_normal);
}
// Friction Constraint 2
{
PfxVector3 normal = tangent2;
PfxFloat denom = dot(K*normal,normal);
constraintFriction2.m_jacDiagInv = 1.0f/denom;
constraintFriction2.m_rhs = -dot(vAB,normal);
constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;
constraintFriction2.m_lowerLimit = 0.0f;
constraintFriction2.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintFriction2.m_normal);
}
}
static
void createIsland(PfxTriMesh &island,const PfxArray<PfxMcFacetPtr> &facets)
{
if(facets.empty()) return;
island.m_numFacets = facets.size();
PfxUInt32 vertsFlag[(0xff*SCE_PFX_NUMMESHFACETS*3+31)/32];
memset(vertsFlag,0,sizeof(PfxUInt32)*((0xff*SCE_PFX_NUMMESHFACETS*3+31)/32));
PfxArray<PfxMcEdgeEntry*> edgeHead(facets.size()*3);
PfxArray<PfxMcEdgeEntry> edgeList(facets.size()*3);
PfxMcEdgeEntry* nl = NULL;
edgeHead.assign(facets.size()*3,nl);
edgeList.assign(facets.size()*3,PfxMcEdgeEntry());
int vcnt = 0;
int ecnt = 0;
for(PfxUInt32 f=0;f<facets.size();f++) {
PfxMcFacet &iFacet = *facets[f];
PfxMcEdge *iEdge[3] = {
iFacet.e[0],
iFacet.e[1],
iFacet.e[2],
};
PfxFacet &oFacet = island.m_facets[f];
oFacet.m_half[0] = oFacet.m_half[1] = oFacet.m_half[2] = 0.0f;
oFacet.m_center[0] = oFacet.m_center[1] = oFacet.m_center[2] = 0.0f;
pfxStoreVector3(iFacet.n,oFacet.m_normal);
oFacet.m_thickness = iFacet.thickness;
// Vertex
for(int v=0;v<3;v++) {
PfxMcVert *vert = facets[f]->v[v];
PfxUInt32 idx = vert->i;
PfxUInt32 mask = 1 << (idx & 31);
if((vertsFlag[idx>>5] & mask) == 0) {
SCE_PFX_ASSERT(vcnt<SCE_PFX_NUMMESHVERTICES);
vertsFlag[idx>>5] |= mask;
island.m_verts[vcnt] = vert->coord;
vert->flag = vcnt;// 新しいインデックス
vcnt++;
}
oFacet.m_vertIds[v] = (PfxUInt8)vert->flag;
}
// Edge
for(int v=0;v<3;v++) {
PfxUInt8 viMin = SCE_PFX_MIN(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
PfxUInt8 viMax = SCE_PFX_MAX(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
int key = ((0x8da6b343*viMin+0xd8163841*viMax)%(island.m_numFacets*3));
for(PfxMcEdgeEntry *e=edgeHead[key];;e=e->next) {
if(!e) {
edgeList[ecnt].vertId[0] = viMin;
edgeList[ecnt].vertId[1] = viMax;
edgeList[ecnt].facetId[0] = f;
edgeList[ecnt].numFacets = 1;
edgeList[ecnt].edgeNum[0] = v;
edgeList[ecnt].edgeId = ecnt;
edgeList[ecnt].dir = normalize(island.m_verts[viMax]-island.m_verts[viMin]);
edgeList[ecnt].next = edgeHead[key];
edgeHead[key] = &edgeList[ecnt];
PfxEdge edge;
edge.m_angleType = iEdge[v]->angleType;
// 厚み角の設定 0～πを0～255の整数値に変換して格納
edge.m_tilt = (PfxUInt8)((iEdge[v]->angle/(0.5f*SCE_PFX_PI))*255.0f);
edge.m_vertId[0] = viMin;
edge.m_vertId[1] = viMax;
oFacet.m_edgeIds[v] = ecnt;
island.m_edges[ecnt] = edge;
SCE_PFX_ASSERT(ecnt <= SCE_PFX_NUMMESHEDGES);
ecnt++;
break;
}
if(e->vertId[0] == viMin && e->vertId[1] == viMax) {
SCE_PFX_ASSERT(e->numFacets==1);
e->facetId[1] = f;
e->edgeNum[1] = v;
e->numFacets = 2;
oFacet.m_edgeIds[v] = e->edgeId;
break;
}
}
}
}
island.m_numEdges = ecnt;
island.m_numVerts = vcnt;
island.updateAABB();
}
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;
}
