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); }
PfxFloat pfxContactCapsuleCapsule( PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB, void *shapeA,const PfxTransform3 &transformA, void *shapeB,const PfxTransform3 &transformB, PfxFloat distanceThreshold) { PfxCapsule &capsuleA = *((PfxCapsule*)shapeA); PfxCapsule &capsuleB = *((PfxCapsule*)shapeB); PfxVector3 directionA = transformA.getUpper3x3().getCol0(); PfxVector3 translationA = transformA.getTranslation(); PfxVector3 directionB = transformB.getUpper3x3().getCol0(); PfxVector3 translationB = transformB.getTranslation(); // translation between centers PfxVector3 translation = translationB - translationA; // compute the closest points of the capsule line segments PfxVector3 ptsVector; // the vector between the closest points PfxVector3 offsetA, offsetB; // offsets from segment centers to their closest points PfxFloat tA, tB; // parameters on line segment segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, directionA, capsuleA.m_halfLen, directionB, capsuleB.m_halfLen ); PfxFloat distance = length(ptsVector) - capsuleA.m_radius - capsuleB.m_radius; if ( distance > distanceThreshold ) return distance; // compute the contact normal segmentsNormal( normal, ptsVector ); // compute points on capsules pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) ); pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( offsetB - normal * capsuleB.m_radius ) ); return distance; }
PfxFloat pfxContactCapsuleSphere( PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB, void *shapeA,const PfxTransform3 &transformA, void *shapeB,const PfxTransform3 &transformB, PfxFloat distanceThreshold) { PfxCapsule &capsuleA = *((PfxCapsule*)shapeA); PfxSphere &sphereB = *((PfxSphere*)shapeB); PfxVector3 directionA = transformA.getUpper3x3().getCol0(); PfxVector3 translationA = transformA.getTranslation(); PfxVector3 translationB = transformB.getTranslation(); // translation between centers of capsule and sphere PfxVector3 translation = translationB - translationA; // compute the closest point on the capsule line segment to the sphere center PfxVector3 ptsVector; PfxVector3 offsetA; PfxFloat tA; segmentPointClosestPoints( ptsVector, offsetA, tA, translation, directionA, capsuleA.m_halfLen ); PfxFloat distance = length(ptsVector) - capsuleA.m_radius - sphereB.m_radius; if ( distance > distanceThreshold ) return distance; // compute the contact normal segmentPointNormal( normal, ptsVector ); // compute points on capsule and sphere pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) ); pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -normal * sphereB.m_radius ) ); return distance; }
static SCE_PFX_FORCE_INLINE bool pfxContactTriangleSphere(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, PfxFloat sphereRadius,const PfxVector3 &spherePos) { PfxVector3 facetPnts[3] = { p0,p1,p2, }; // 早期判定 { PfxPlane planeA(normal,p0); PfxFloat len1 = planeA.onPlane(spherePos); if(len1 >= sphereRadius || len1 < -thickness-sphereRadius) return false; } // 球と面の最近接点を計算 { PfxTriangle triangleA(p0,p1,p2); PfxVector3 pntA; // pfxClosestPointTriangle(spherePos,triangleA,pntA); bool insideTriangle = false; while(1) { PfxVector3 ab = p1 - p0; PfxVector3 ac = p2 - p0; PfxVector3 ap = spherePos - p0; PfxFloat d1 = dot(ab, ap); PfxFloat d2 = dot(ac, ap); if(d1 <= 0.0f && d2 <= 0.0f) { pntA = p0; break; } PfxVector3 bp = spherePos - p1; PfxFloat d3 = dot(ab, bp); PfxFloat d4 = dot(ac, bp); if (d3 >= 0.0f && d4 <= d3) { pntA = p1; break; } PfxFloat vc = d1*d4 - d3*d2; if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { PfxFloat v = d1 / (d1 - d3); pntA = p0 + v * ab; break; } PfxVector3 cp = spherePos - p2; PfxFloat d5 = dot(ab, cp); PfxFloat d6 = dot(ac, cp); if (d6 >= 0.0f && d5 <= d6) { pntA = p2; break; } PfxFloat vb = d5*d2 - d1*d6; if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { PfxFloat w = d2 / (d2 - d6); pntA = p0 + w * ac; break; } PfxFloat va = d3*d6 - d5*d4; if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); pntA = p1 + w * (p2 - p1); break; } PfxFloat den = 1.0f / (va + vb + vc); PfxFloat v = vb * den; PfxFloat w = vc * den; pntA = p0 + ab * v + ac * w; insideTriangle = true; break; } PfxVector3 distVec = pntA - spherePos; PfxFloat l = length(distVec); if(!insideTriangle && l >= sphereRadius) return false; // 分離軸 PfxVector3 sepAxis = (l < 0.00001f || insideTriangle) ? -normal : distVec / l; // 球上の衝突点 PfxVector3 pointsOnSphere = spherePos + sphereRadius * sepAxis; PfxVector3 pointsOnTriangle = pntA; // 面上の最近接点が凸エッジ上でない場合は法線を変える if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) || ((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) || ((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) { sepAxis=-normal; } PfxSubData subData; subData.setFacetId(facetId); contacts.addContactPoint(-length(pointsOnSphere-pointsOnTriangle),sepAxis,PfxPoint3(pointsOnTriangle),PfxPoint3(pointsOnSphere),subData); } 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; }
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); } }