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;
}
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();
}
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();
}
EpxBool epxConvexConvexContact_local(
const EpxConvexMesh &convexA,const EpxTransform3 &transformA,
const EpxConvexMesh &convexB,const EpxTransform3 &transformB,
EpxVector3 &normal,
EpxFloat &penetrationDepth,
EpxVector3 &contactPointA,
EpxVector3 &contactPointB)
{
EpxTransform3 transformAB,transformBA;
EpxMatrix3 matrixAB,matrixBA;
EpxVector3 offsetAB,offsetBA;
// Bローカル→Aローカルへの変換
transformAB = orthoInverse(transformA) * transformB;
matrixAB = transformAB.getUpper3x3();
offsetAB = transformAB.getTranslation();
// Aローカル→Bローカルへの変換
transformBA = orthoInverse(transformAB);
matrixBA = transformBA.getUpper3x3();
offsetBA = transformBA.getTranslation();
// 最も浅い貫通深度とそのときの分離軸
EpxFloat distanceMin = -EPX_FLT_MAX;
EpxVector3 axisMin(0.0f);
EpxSatType satType = EpxSatTypeEdgeEdge;
EpxBool axisFlip;
//----------------------------------------------------------------------------
// 分離軸判定
int satCount = 0;
// 面法線の判定を優先させたいので、エッジ外積→面法線の順に判定
// ConvexAとConvexBの外積を分離軸とする
EpxUInt32 edgeIdMinA,edgeIdMinB;
for(EpxUInt32 eA=0;eA<convexA.m_numEdges;eA++) {
const EpxEdge &edgeA = convexA.m_edges[eA];
if(edgeA.type != EpxEdgeTypeConvex) continue;
const EpxVector3 edgeVecA = convexA.m_vertices[edgeA.vertId[1]] - convexA.m_vertices[edgeA.vertId[0]];
for(EpxUInt32 eB=0;eB<convexB.m_numEdges;eB++) {
const EpxEdge &edgeB = convexB.m_edges[eB];
if(edgeB.type != EpxEdgeTypeConvex) continue;
const EpxVector3 edgeVecB = matrixAB * (convexB.m_vertices[edgeB.vertId[1]] - convexB.m_vertices[edgeB.vertId[0]]);
// Gauss map algorithm from GDC 2013 physics tutorial
EpxVector3 eA0 = convexA.m_facets[edgeA.facetId[0]].normal;
EpxVector3 eA1 = convexA.m_facets[edgeA.facetId[1]].normal;
EpxVector3 eB0 = -matrixAB * convexB.m_facets[edgeB.facetId[0]].normal;
EpxVector3 eB1 = -matrixAB * convexB.m_facets[edgeB.facetId[1]].normal;
if(!isValidEdge(eA0,eA1,eB0,eB1)) continue;
EpxVector3 separatingAxis = cross(edgeVecA,edgeVecB);
if(lengthSqr(separatingAxis) < EPX_EPSILON*EPX_EPSILON) continue;
separatingAxis = normalize(separatingAxis);
EpxVector3 pA = convexA.m_vertices[edgeA.vertId[0]];
EpxVector3 pB = offsetAB + matrixAB * convexB.m_vertices[edgeB.vertId[0]];
if(dot(separatingAxis,pA) > 0.0f) { // 原点は必ずConvexの内側に存在すること
separatingAxis = -separatingAxis;
}
EpxFloat d = dot(separatingAxis,pA - pB);
satCount++;
if(d >= 0.0f) {
return false;
}
if(distanceMin < d) {
distanceMin = d;
axisMin = separatingAxis;
satType = EpxSatTypeEdgeEdge;
edgeIdMinA = eA;
edgeIdMinB = eB;
}
}
}
// ConvexAの面法線を分離軸とする
for(EpxUInt32 f=0;f<convexA.m_numFacets;f++) {
const EpxFacet &facet = convexA.m_facets[f];
const EpxVector3 separatingAxis = facet.normal;
EpxVector3 axisB = matrixBA * separatingAxis;
EpxVector3 pA = offsetBA + matrixBA * convexA.m_vertices[facet.vertId[0]];
EpxFloat minB = EPX_FLT_MAX;
for(EpxUInt32 i=0;i<convexB.m_numVertices;i++) {
EpxFloat prj = dot(axisB,convexB.m_vertices[i] - pA);
minB = EPX_MIN(minB,prj);
}
satCount++;
if(minB >= 0.0f) {
return false;
}
if(distanceMin < minB) {
distanceMin = minB;
axisMin = -separatingAxis;
satType = EpxSatTypePointBFacetA;
axisFlip = true;
}
}
// ConvexBの面法線を分離軸とする
for(EpxUInt32 f=0;f<convexB.m_numFacets;f++) {
const EpxFacet &facet = convexB.m_facets[f];
const EpxVector3 separatingAxis = matrixAB * facet.normal;
EpxVector3 pB = offsetAB + matrixAB * convexB.m_vertices[facet.vertId[0]];
EpxFloat minA = EPX_FLT_MAX;
for(EpxUInt32 i=0;i<convexA.m_numVertices;i++) {
EpxFloat prj = dot(separatingAxis,convexA.m_vertices[i] - pB);
minA = EPX_MIN(minA,prj);
}
satCount++;
if(minA >= 0.0f) {
return false;
}
if(distanceMin < minA) {
distanceMin = minA;
axisMin = separatingAxis;
satType = EpxSatTypePointAFacetB;
axisFlip = false;
}
}
// ここまで到達したので、２つの凸メッシュは交差している。
// また、反発ベクトル(axisMin)と貫通深度(distanceMin)が求まった。
// 反発ベクトルはＡを押しだす方向をプラスにとる。
//int satTotal = convexA.m_numFacets + convexB.m_numFacets + convexA.m_numEdges * convexB.m_numEdges;
//epxPrintf("sat check count %d / %d\n",satCount,satTotal);
//----------------------------------------------------------------------------
// 衝突座標検出
int collCount = 0;
EpxFloat closestMinSqr = EPX_FLT_MAX;
EpxVector3 closestPointA,closestPointB;
EpxVector3 separation = 1.1f * fabs(distanceMin) * axisMin;
if(satType == EpxSatTypeEdgeEdge) {
const EpxEdge &edgeA = convexA.m_edges[edgeIdMinA];
const EpxEdge &edgeB = convexB.m_edges[edgeIdMinB];
EpxVector3 sA,sB;
epxGetClosestTwoSegments(
separation + convexA.m_vertices[edgeA.vertId[0]],
separation + convexA.m_vertices[edgeA.vertId[1]],
offsetAB + matrixAB * convexB.m_vertices[edgeB.vertId[0]],
offsetAB + matrixAB * convexB.m_vertices[edgeB.vertId[1]],
sA,sB);
EpxFloat dSqr = lengthSqr(sA-sB);
closestMinSqr = dSqr;
closestPointA = sA;
closestPointB = sB;
collCount++;
}
else {
// 分離平面を挟んで向かい合う面を抽出
EpxUInt8 facetsA[EPX_CONVEX_MESH_MAX_FACETS];
EpxUInt8 facetsB[EPX_CONVEX_MESH_MAX_FACETS];
EpxUInt8 numFacetsA = 0;
EpxUInt8 numFacetsB = 0;
if(satType == EpxSatTypePointBFacetA) {
for(EpxUInt8 fA=0;fA<convexA.m_numFacets;fA++) {
const EpxFacet &facetA = convexA.m_facets[fA];
EpxFloat checkA = dot(facetA.normal,-axisMin);
if(checkA < 0.99f && axisFlip) {
// 判定軸が面Aの法線のとき、向きの違うAの面は判定しない
continue;
}
if(checkA < 0.0f) {
// 衝突面と逆に向いている面は判定しない
continue;
}
facetsA[numFacetsA++] = (EpxUInt8)fA;
}
EpxFloat checkBMax = -1.0f;
for(EpxUInt8 fB=0;fB<convexB.m_numFacets;fB++) {
const EpxFacet &facetB = convexB.m_facets[fB];
EpxFloat checkB = dot(facetB.normal,matrixBA * axisMin);
if(checkB > checkBMax) {
checkBMax = checkB;
facetsB[0] = fB;
numFacetsB = 1;
}
else if(checkB > checkBMax - EPX_EPSILON) { // checkB == checkBMax
facetsB[numFacetsB++] = fB;
}
}
}
else { // satType == EpxSatTypePointAFacetB
for(EpxUInt8 fB=0;fB<convexB.m_numFacets;fB++) {
const EpxFacet &facetB = convexB.m_facets[fB];
EpxFloat checkB = dot(facetB.normal,matrixBA * axisMin);
if(checkB < 0.99f && !axisFlip) {
// 判定軸が面Bの法線のとき、向きの違うBの面は判定しない
continue;
}
if(checkB < 0.0f) {
// 衝突面と逆に向いている面は判定しない
continue;
}
facetsB[numFacetsB++] = (EpxUInt8)fB;
}
EpxFloat checkAMax = -1.0f;
for(EpxUInt8 fA=0;fA<convexA.m_numFacets;fA++) {
const EpxFacet &facetA = convexA.m_facets[fA];
EpxFloat checkA = dot(facetA.normal,-axisMin);
if(checkA > checkAMax) {
checkAMax = checkA;
facetsA[0] = fA;
numFacetsA = 1;
}
else if(checkA > checkAMax - EPX_EPSILON) { // checkA == checkAMax
facetsA[numFacetsA++] = fA;
}
}
}
for(EpxUInt8 fA=0;fA<numFacetsA;fA++) {
const EpxFacet &facetA = convexA.m_facets[facetsA[fA]];
for(EpxUInt8 fB=0;fB<numFacetsB;fB++) {
const EpxFacet &facetB = convexB.m_facets[facetsB[fB]];
collCount++;
// 面Ａと面Ｂの最近接点を求める
EpxVector3 triangleA[3] = {
separation + convexA.m_vertices[facetA.vertId[0]],
separation + convexA.m_vertices[facetA.vertId[1]],
separation + convexA.m_vertices[facetA.vertId[2]],
};
EpxVector3 triangleB[3] = {
offsetAB + matrixAB * convexB.m_vertices[facetB.vertId[0]],
offsetAB + matrixAB * convexB.m_vertices[facetB.vertId[1]],
offsetAB + matrixAB * convexB.m_vertices[facetB.vertId[2]],
};
// 頂点Ａ→面Ｂの最近接点算出
for(int i=0;i<3;i++) {
EpxVector3 s;
epxGetClosestPointTriangle(triangleA[i],triangleB[0],triangleB[1],triangleB[2],matrixAB * facetB.normal,s);
EpxFloat dSqr = lengthSqr(triangleA[i]-s);
if(dSqr < closestMinSqr) {
closestMinSqr = dSqr;
closestPointA = triangleA[i];
closestPointB = s;
}
}
// 頂点Ｂ→面Ａの最近接点算出
for(int i=0;i<3;i++) {
EpxVector3 s;
epxGetClosestPointTriangle(triangleB[i],triangleA[0],triangleA[1],triangleA[2],facetA.normal,s);
EpxFloat dSqr = lengthSqr(triangleB[i]-s);
if(dSqr < closestMinSqr) {
closestMinSqr = dSqr;
closestPointA = s;
closestPointB = triangleB[i];
}
}
}
}
}
//epxPrintf("intersection check count %d\n",collCount);
normal = transformA.getUpper3x3() * axisMin;
penetrationDepth = distanceMin;
contactPointA = closestPointA - separation;
contactPointB = offsetBA + matrixBA * closestPointB;
return true;
}
PfxBool pfxIntersectRayCapsule(const PfxRayInput &ray,PfxRayOutput &out,const PfxCapsule &capsule,const PfxTransform3 &transform)
{
// レイをCapsuleのローカル座標へ変換
PfxTransform3 transformCapsule = orthoInverse(transform);
PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();
PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;
PfxFloat radSqr = capsule.m_radius * capsule.m_radius;
// 始点がカプセルの内側にあるか判定
{
PfxFloat h = fabsf(startPosL[0]);
if(h > capsule.m_halfLen) h = capsule.m_halfLen;
PfxVector3 Px(out.m_variable,0,0);
PfxFloat sqrLen = lengthSqr(startPosL-Px);
if(sqrLen <= radSqr) return false;
}
// カプセルの胴体との交差判定
do {
PfxVector3 P(startPosL);
PfxVector3 D(rayDirL);
P[0] = 0.0f;
D[0] = 0.0f;
PfxFloat a = dot(D,D);
PfxFloat b = dot(P,D);
PfxFloat c = dot(P,P) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f || fabs(a) < 0.00001f) return false;
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f)
break;
else if(tt > 1.0f)
return false;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
if(fabsf(cp[0]) <= capsule.m_halfLen) {
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));
out.m_contactNormal = transform.getUpper3x3() * normalize(cp);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
} while(0);
// カプセルの両端にある球体との交差判定
PfxFloat a = dot(rayDirL,rayDirL);
if(fabs(a) < 0.00001f) return false;
do {
PfxVector3 center(capsule.m_halfLen,0.0f,0.0f);
PfxVector3 v = startPosL - center;
PfxFloat b = dot(v,rayDirL);
PfxFloat c = dot(v,v) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f) break;
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f || tt > 1.0f) break;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
} while(0);
{
PfxVector3 center(-capsule.m_halfLen,0.0f,0.0f);
PfxVector3 v = startPosL - center;
PfxFloat b = dot(v,rayDirL);
PfxFloat c = dot(v,v) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f) return false;
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f || tt > 1.0f) return false;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + out.m_variable * rayDirL;
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
return false;
}
PfxBool pfxIntersectRayCylinder(const PfxRayInput &ray,PfxRayOutput &out,const PfxCylinder &cylinder,const PfxTransform3 &transform)
{
// レイを円柱のローカル座標へ変換
PfxTransform3 transformCapsule = orthoInverse(transform);
PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();
PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;
PfxFloat radSqr = cylinder.m_radius * cylinder.m_radius;
// 始点が円柱の内側にあるか判定
{
PfxFloat h = startPosL[0];
if(-cylinder.m_halfLen <= h && h <= cylinder.m_halfLen) {
PfxVector3 Px(h,0,0);
PfxFloat sqrLen = lengthSqr(startPosL-Px);
if(sqrLen <= radSqr) return false;
}
}
// 円柱の胴体との交差判定
do {
PfxVector3 P(startPosL);
PfxVector3 D(rayDirL);
P[0] = 0.0f;
D[0] = 0.0f;
PfxFloat a = dot(D,D);
PfxFloat b = dot(P,D);
PfxFloat c = dot(P,P) - radSqr;
PfxFloat d = b * b - a * c;
if(d < 0.0f) return false; // レイは逸れている
if(pfxAbsf(a) < 0.00001f) break; // レイがX軸に平行
PfxFloat tt = ( -b - sqrtf(d) ) / a;
if(tt < 0.0f || tt > 1.0f) break;
if(tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
if(pfxAbsf(cp[0]) <= cylinder.m_halfLen) {
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));
out.m_contactNormal = transform.getUpper3x3() * normalize(cp);
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
} while(0);
// 円柱の両端にある平面との交差判定
{
if(pfxAbsf(rayDirL[0]) < 0.00001f) return false;
PfxFloat t1 = ( cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];
PfxFloat t2 = ( - cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];
PfxFloat tt = SCE_PFX_MIN(t1,t2);
if(tt < 0.0f || tt > 1.0f) return false;
PfxVector3 p = startPosL + tt * rayDirL;
p[0] = 0.0f;
if(lengthSqr(p) < radSqr && tt < out.m_variable) {
PfxVector3 cp = startPosL + tt * rayDirL;
out.m_contactFlag = true;
out.m_variable = tt;
out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;
out.m_contactNormal = transform.getUpper3x3() * ((cp[0]>0.0f)?PfxVector3(1.0,0.0,0.0):PfxVector3(-1.0,0.0,0.0));
out.m_subData.m_type = PfxSubData::NONE;
return true;
}
}
return false;
}
