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);
}
}
