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, }; // 面積が0の面を排除 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)); } } } } // 面の面積によって3種類に分類する 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; }