#include "../Include/MathUtil.h" #include namespace CrossM{ namespace Math{ // 삼각형이 AABB 안에 완전히 포함되는지 검사 bool IsTriangleInAabb(const VECTOR3& vAabbMin, const VECTOR3& vAabbMax, const VECTOR3& vTri0, const VECTOR3& vTri1, const VECTOR3& vTri2) { if (vAabbMin.x <= vTri0.x && vTri0.x <= vAabbMax.x && vAabbMin.y <= vTri0.y && vTri0.y <= vAabbMax.y && vAabbMin.z <= vTri0.z && vTri0.z <= vAabbMax.z && vAabbMin.x <= vTri1.x && vTri1.x <= vAabbMax.x && vAabbMin.y <= vTri1.y && vTri1.y <= vAabbMax.y && vAabbMin.z <= vTri1.z && vTri1.z <= vAabbMax.z && vAabbMin.x <= vTri2.x && vTri2.x <= vAabbMax.x && vAabbMin.y <= vTri2.y && vTri2.y <= vAabbMax.y && vAabbMin.z <= vTri2.z && vTri2.z <= vAabbMax.z) { return true; } return false; } // 삼각형과 AABB 가 만나는지를 검사 bool CheckAabbTriangleIntersection(const VECTOR3& vAabbMin, const VECTOR3& vAabbMax, const VECTOR3& vTri0, const VECTOR3& vTri1, const VECTOR3& vTri2) { // Separation of Axes 에 따른 AABB - triangle intersection test 구현 // 6 축에 대해 AABB 와 삼각형을 projection 한 뒤, 둘이 겹치는지 여부를 확인한다 // 모든 축에 대해 겹치면 둘은 만나는것이고, 한 축에 대해서라도 겹치지 않는다면 만나지 않는 것이다 float fBoxProjectionMin, fBoxProjectionMax; float fTriProjectionMin, fTriProjectionMax; // AABB 의 세 축에 대한 projection 을 검사 // X 축 fBoxProjectionMin = vAabbMin.x; fBoxProjectionMax = vAabbMax.x; fTriProjectionMin = std::min(vTri0.x, std::min(vTri1.x, vTri2.x)); fTriProjectionMax = std::max(vTri0.x, std::max(vTri1.x, vTri2.x)); if (false == IsRangeOverlap(fTriProjectionMin, fTriProjectionMax, fBoxProjectionMin, fBoxProjectionMax)) { return false; } // Y 축 fBoxProjectionMin = vAabbMin.y; fBoxProjectionMax = vAabbMax.y; fTriProjectionMin = std::min(vTri0.y, std::min(vTri1.y, vTri2.y)); fTriProjectionMax = std::max(vTri0.y, std::max(vTri1.y, vTri2.y)); if (false == IsRangeOverlap(fTriProjectionMin, fTriProjectionMax, fBoxProjectionMin, fBoxProjectionMax)) { return false; } // Z 축 fBoxProjectionMin = vAabbMin.z; fBoxProjectionMax = vAabbMax.z; fTriProjectionMin = std::min(vTri0.z, std::min(vTri1.z, vTri2.z)); fTriProjectionMax = std::max(vTri0.z, std::max(vTri1.z, vTri2.z)); if (false == IsRangeOverlap(fTriProjectionMin, fTriProjectionMax, fBoxProjectionMin, fBoxProjectionMax)) { return false; } // 삼각형의 의 세 축에 대한 projection 을 검사 VECTOR3 avAxis[3]; // 삼각형의 세 모서리 Math::Subtract(avAxis[0], vTri1, vTri0); Math::Subtract(avAxis[1], vTri2, vTri0); Math::Subtract(avAxis[2], vTri2, vTri1); // 삼각형 각 점을 축에 projection 한 값 float fProjectionTri0, fProjectionTri1, fProjectionTri2; for (int i = 0; i < 3; ++i) { VECTOR3& vAxis = avAxis[i]; // AABB 의 max/min 점의 xyz 는 각각의 최대/최소값이므로, // axis 의 각 컴포넌트와 내적했을때 최대값을 구하려면 // x,y,z 각각의 곱이 큰 쪽 끼리 골라서 더하면 된다. // (어차피 AABB 여덟점이 xyz 각각의 최대/최소간의 조합이므로, // x,y,z 를 각각 임의로 최대/최소를 선택한다 해도 어뎗 점 안에 모두 포함되는 조합이다.) fBoxProjectionMin = ((vAxis.x > 0) ? vAabbMin.x : vAabbMax.x) * vAxis.x + ((vAxis.y > 0) ? vAabbMin.y : vAabbMax.y) * vAxis.y + ((vAxis.z > 0) ? vAabbMin.z : vAabbMax.z) * vAxis.z; fBoxProjectionMax = ((vAxis.x > 0) ? vAabbMax.x : vAabbMin.x) * vAxis.x + ((vAxis.y > 0) ? vAabbMax.y : vAabbMin.y) * vAxis.y + ((vAxis.z > 0) ? vAabbMax.z : vAabbMin.z) * vAxis.z; fProjectionTri0 = Math::DotProduct(vTri0, vAxis); fProjectionTri1 = Math::DotProduct(vTri1, vAxis); fProjectionTri2 = Math::DotProduct(vTri2, vAxis); fTriProjectionMin = std::min(fProjectionTri0, std::min(fProjectionTri1, fProjectionTri2)); fTriProjectionMax = std::max(fProjectionTri0, std::max(fProjectionTri1, fProjectionTri2)); if (false == IsRangeOverlap(fTriProjectionMin, fTriProjectionMax, fBoxProjectionMin, fBoxProjectionMax)) { return false; } } return true; } // 두 AABB 가 만나는지 검사 bool CheckAabbAabbIntersection(const VECTOR3& vAabb1Min, const VECTOR3& vAabb1Max, const VECTOR3& vAabb2Min, const VECTOR3& vAabb2Max) { if (false ==IsRangeOverlap(vAabb1Min.x, vAabb1Max.x, vAabb2Min.x, vAabb2Max.x)) return false; if (false ==IsRangeOverlap(vAabb1Min.y, vAabb1Max.y, vAabb2Min.y, vAabb2Max.y)) return false; if (false ==IsRangeOverlap(vAabb1Min.z, vAabb1Max.z, vAabb2Min.z, vAabb2Max.z)) return false; return true; } // CheckTriangleSweepingSphereCollision 의 부속 함수. // ax^2 + bx + c 의 해 중 min~max 범위 내의 가장 작은 해를 반환한다. 만족하는 해가 없으면 false 반환 static bool GetLowestRootInRange(const float a, const float b, const float c, const float fMinRoot, const float fMaxRoot, float& fRoot) { // 실수해가 존재하는지 판별식 계산 float fDeterminant = b*b - 4.0f*a*c; // 실수해가 존재하지 않으면 해 없음 if (fDeterminant < 0.0f) return false; // 두개의 해를 계산한다 float fSqrtD = sqrtf(fDeterminant); float r1 = (-b - fSqrtD) / (2*a); float r2 = (-b + fSqrtD) / (2*a); // r1 < r2 의 크기가 되도록 정렬 if (r1 > r2) std::swap(r1, r2); // 작은쪽부터 범위 내인지 판별해, 범위내일경우 해로서 리턴 if (r1 > fMinRoot && r1 < fMaxRoot) { fRoot = r1; return true; } // 작은 해 쪽이 범위내에 들지 않으면, 큰 해 쪽이 범위내인지 검사한다 if (r2 > fMinRoot && r2 < fMaxRoot) { fRoot = r2; return true; } return false; } bool CheckTriangleSweepingSphereCollision(float &fOutT, VECTOR3& vOutContactPoint, bool& bOutContactInsideTriangle, const TriangSweepingSphere& triAndSphere) { // 약간의 alias const Math::VECTOR3 &vBasePoint = triAndSphere.m_vSphereSweepStart; const Math::VECTOR3 &vTri0 = triAndSphere.m_vTri0; const Math::VECTOR3 &vTri1 = triAndSphere.m_vTri1; const Math::VECTOR3 &vTri2 = triAndSphere.m_vTri2; const Math::VECTOR3 &vPath = triAndSphere.m_vSphereSweepPath; // 삼각형의 세 모서리 Math::VECTOR3 vTriEdge01, vTriEdge02, vTriEdge12; // 삼각형을 포함하는 평면. 추후 plane 클래스 등으로 만들어야 할 듯 Math::VECTOR3 vTriPlaneNormal; float fTriPlaneConstant; // 삼각형의 세 모서리 벡터를 구하고.. Math::Subtract(vTriEdge01, vTri1, vTri0); Math::Subtract(vTriEdge02, vTri2, vTri0); Math::Subtract(vTriEdge12, vTri2, vTri1); // 삼각형이 포함된 평면의 파라미터들을 구함 Math::CrossProduct(vTriPlaneNormal, vTriEdge01, vTriEdge02); Math::Normalize(vTriPlaneNormal, vTriPlaneNormal); fTriPlaneConstant = -Math::DotProduct(vTriPlaneNormal, vTri0); // sweeping path 와 충돌평면법선 내적 float fNormalDotPath = Math::DotProduct(vTriPlaneNormal, vPath); // sphere 의 진행방향이 같은 방향이면 체크안함 if (fNormalDotPath > 0.0f) { return false; } float t0, t1; bool bEmbededInPlane = false; float fSignedDistBaseToTriPlane = Math::DotProduct(vTriPlaneNormal, vBasePoint) + fTriPlaneConstant; if (0.0f == fNormalDotPath) { // sphere 가 삼각형면과 평행하게 진행 if (fabs(fSignedDistBaseToTriPlane) >= triAndSphere.m_fSphereRadius) { return false; // 삼각형면에서 멀리 떨어져서 평행이동중 } else { bEmbededInPlane = true; t0 = 0.0f; t1 = 1.0f; } } else { t0 = (-triAndSphere.m_fSphereRadius-fSignedDistBaseToTriPlane)/fNormalDotPath; t1 = (triAndSphere.m_fSphereRadius-fSignedDistBaseToTriPlane)/fNormalDotPath; // t0 < t1 으로 소트 if (t0 > t1) { std::swap(t0, t1); } // t 구간이 sphere 이동경로 선분 내에 있지 않다 if (t0 > 1.0f || t1 <0.0f) { return false; } // t 값을 0~1 구간내로 클램핑 t0 = std::max(t0, 0.0f); t1 = std::min(t1, 1.0f); } VECTOR3 vContactPoint; bool bFoundCollision = false; float t = 1.0f; // 삼각형 내부와 체크 if (!bEmbededInPlane) { vContactPoint = vBasePoint + (vPath*t0) - vTriPlaneNormal; if (IsPointInTriangle(vContactPoint, vTri0, vTri1, vTri2)) { bFoundCollision = true; t = t0; } } // 아직 충돌점을 찾지 못했다면 모서리와 꼭지점에 대해 테스트를 해야한다 if (!bFoundCollision) { float fSQuaredPathLength = GetSquaredLength(vPath); float a, b, c; float newT; float fSquaredRadius = triAndSphere.m_fSphereRadius*triAndSphere.m_fSphereRadius; a = fSQuaredPathLength; // vTri0 b = 2.0f * (DotProduct(vPath, vBasePoint - vTri0)); c = (Math::GetSquaredLength(vTri0 - vBasePoint) - fSquaredRadius); if (GetLowestRootInRange(a, b, c, 0.0f, t, newT)) { t = newT; bFoundCollision = true; vContactPoint = vTri0; } // vTri1 if (bFoundCollision) { b = 2.0f * (DotProduct(vPath, vBasePoint - vTri1)); c = (Math::GetSquaredLength(vTri1 - vBasePoint) - fSquaredRadius); if (GetLowestRootInRange(a, b, c, 0.0f, t, newT)) { t = newT; bFoundCollision = true; vContactPoint = vTri1; } } // vTri2 if (bFoundCollision) { b = 2.0f * (DotProduct(vPath, vBasePoint - vTri2)); c = (Math::GetSquaredLength(vTri2 - vBasePoint) - fSquaredRadius); if (GetLowestRootInRange(a, b, c, 0.0f, t, newT)) { t = newT; bFoundCollision = true; vContactPoint = vTri0; } } // 모서리에 대해 테스트 VECTOR3 vBaseToVertex; float fEdgeSquaredLength; float fEdgeDotPath; float fEdgeDotBaseToVertex; // vTri0 ~ vTri1 vBaseToVertex = vTri0 - vBasePoint; fEdgeSquaredLength = GetSquaredLength(vTriEdge01); fEdgeDotPath = DotProduct(vTriEdge01, vPath); fEdgeDotBaseToVertex = DotProduct(vTriEdge01, vBaseToVertex); a = fEdgeSquaredLength* -fSQuaredPathLength + fEdgeDotPath*fEdgeDotPath; b = fEdgeSquaredLength* (2.0f*Math::DotProduct(vPath, vBaseToVertex)) - 2.0f*fEdgeDotPath*fEdgeDotBaseToVertex; c = (fEdgeSquaredLength* (1.0f - Math::GetSquaredLength(vBaseToVertex)) + fEdgeDotBaseToVertex*fEdgeDotBaseToVertex); if (GetLowestRootInRange(a, b, c, 0.0f, t, newT)) { float f = (fEdgeDotPath*t - fEdgeDotBaseToVertex) / fEdgeSquaredLength; if (f >= 0.0f && f <= 1.0f) { t = newT; bFoundCollision = true; vContactPoint = vTri0 + vTriEdge01*f; } } // vTri0 ~ vTri2 vBaseToVertex = vTri0 - vBasePoint; fEdgeSquaredLength = GetSquaredLength(vTriEdge02); fEdgeDotPath = DotProduct(vTriEdge02, vPath); fEdgeDotBaseToVertex = DotProduct(vTriEdge02, vBaseToVertex); a = fEdgeSquaredLength* -fSQuaredPathLength + fEdgeDotPath*fEdgeDotPath; b = fEdgeSquaredLength* (2.0f*Math::DotProduct(vPath, vBaseToVertex)) - 2.0f*fEdgeDotPath*fEdgeDotBaseToVertex; c = (fEdgeSquaredLength* (1.0f - Math::GetSquaredLength(vBaseToVertex)) + fEdgeDotBaseToVertex*fEdgeDotBaseToVertex); if (GetLowestRootInRange(a, b, c, 0.0f, t, newT)) { float f = (fEdgeDotPath*t - fEdgeDotBaseToVertex) / fEdgeSquaredLength; if (f >= 0.0f && f <= 1.0f) { t = newT; bFoundCollision = true; vContactPoint = vTri0 + vTriEdge02*f; } } // vTri1 ~ vTri2 vBaseToVertex = vTri1 - vBasePoint; fEdgeSquaredLength = GetSquaredLength(vTriEdge12); fEdgeDotPath = DotProduct(vTriEdge12, vPath); fEdgeDotBaseToVertex = DotProduct(vTriEdge12, vBaseToVertex); a = fEdgeSquaredLength* -fSQuaredPathLength + fEdgeDotPath*fEdgeDotPath; b = fEdgeSquaredLength* (2.0f*Math::DotProduct(vPath, vBaseToVertex)) - 2.0f*fEdgeDotPath*fEdgeDotBaseToVertex; c = (fEdgeSquaredLength* (1.0f - Math::GetSquaredLength(vBaseToVertex)) + fEdgeDotBaseToVertex*fEdgeDotBaseToVertex); if (GetLowestRootInRange(a, b, c, 0.0f, t, newT)) { float f = (fEdgeDotPath*t - fEdgeDotBaseToVertex) / fEdgeSquaredLength; if (f >= 0.0f && f <= 1.0f) { t = newT; bFoundCollision = true; vContactPoint = vTri1 + vTriEdge12*f; } } } if (bFoundCollision) { fOutT = t; vOutContactPoint = vContactPoint; return true; } return false; } // 지정된 점이 삼각형 내에 포함되는지 확인하는 코드 // Fauerby 의 글에 Keidy 라는 사람이 기고한 가장 빠른 루틴이라고 한다 // 수학적인 증명은.. 모르겠다-_- 일단 이용 bool IsPointInTriangle(const VECTOR3& p, const VECTOR3& vTri0, const VECTOR3& vTri1, const VECTOR3& vTri2) { // 원점을 낀 두 삼각형 모서리, 원점을 기준으로 한 p 의 위치벡터 VECTOR3 vEdge1, vEdge2, pTri; Math::Subtract(vEdge1, vTri1, vTri0); Math::Subtract(vEdge2, vTri2, vTri0); Math::Subtract(pTri, p, vTri0); float a = Math::DotProduct(vEdge1, vEdge1); float b = Math::DotProduct(vEdge1, vEdge2); float c = Math::DotProduct(vEdge2, vEdge2); float d = Math::DotProduct(pTri, vEdge1); float e = Math::DotProduct(pTri, vEdge2); float x = d*c - e*b; float y = e*a - d*b; float z = x + y - (a*c - b*b); return ( ((unsigned int&)z) & ~ ( ((unsigned int&)x) | ((unsigned int&)y) ) & 0x80000000) != 0 ? true : false; } }}