diff --git a/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp b/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp index 5d13acc34c..4f143cde53 100644 --- a/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp +++ b/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp @@ -8,778 +8,936 @@ #include -using namespace AZ; -using namespace Intersect; - -//========================================================================= -// IntersectSegmentTriangleCCW -// [10/21/2009] -//========================================================================= -bool Intersect::IntersectSegmentTriangleCCW( - const Vector3& p, const Vector3& q, const Vector3& a, const Vector3& b, const Vector3& c, - /*float &u, float &v, float &w,*/ Vector3& normal, float& t) +namespace AZ { - float v, w; // comment this and enable input params if we need the barycentric coordinates - - Vector3 ab = b - a; - Vector3 ac = c - a; - Vector3 qp = p - q; - - // Compute triangle normal. Can be pre-calculated/cached if - // intersecting multiple segments against the same triangle - normal = ab.Cross(ac); // Right hand CCW - - // Compute denominator d. If d <= 0, segment is parallel to or points - // away from triangle, so exit early - float d = qp.Dot(normal); - if (d <= 0.0f) + //========================================================================= + // IntersectSegmentTriangleCCW + // [10/21/2009] + //========================================================================= + bool Intersect::IntersectSegmentTriangleCCW( + const Vector3& p, + const Vector3& q, + const Vector3& a, + const Vector3& b, + const Vector3& c, + /*float &u, float &v, float &w,*/ Vector3& normal, + float& t) { - return false; - } + float v, w; // comment this and enable input params if we need the barycentric coordinates - // Compute intersection t value of pq with plane of triangle. A ray - // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay - // dividing by d until intersection has been found to pierce triangle - Vector3 ap = p - a; - t = ap.Dot(normal); + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 qp = p - q; - // range segment check t[0,1] (it this case [0,d]) - if (t < 0.0f || t > d) - { - return false; - } + // Compute triangle normal. Can be pre-calculated/cached if + // intersecting multiple segments against the same triangle + normal = ab.Cross(ac); // Right hand CCW - // Compute barycentric coordinate components and test if within bounds - Vector3 e = qp.Cross(ap); - v = ac.Dot(e); - if (v < 0.0f || v > d) - { - return false; - } - w = -ab.Dot(e); - if (w < 0.0f || v + w > d) - { - return false; - } - - // Segment/ray intersects triangle. Perform delayed division and - // compute the last barycentric coordinate component - float ood = 1.0f / d; - t *= ood; - /*v *= ood; - w *= ood; - u = 1.0f - v - w;*/ - - normal.Normalize(); - - return true; -} - -//========================================================================= -// IntersectSegmentTriangle -// [10/21/2009] -//========================================================================= -bool -Intersect::IntersectSegmentTriangle( - const Vector3& p, const Vector3& q, const Vector3& a, const Vector3& b, const Vector3& c, - /*float &u, float &v, float &w,*/ Vector3& normal, float& t) -{ - float v, w; // comment this and enable input params if we need the barycentric coordinates - - Vector3 ab = b - a; - Vector3 ac = c - a; - Vector3 qp = p - q; - Vector3 ap = p - a; - - // Compute triangle normal. Can be pre-calculated or cached if - // intersecting multiple segments against the same triangle - normal = ab.Cross(ac); // Right hand CCW - - // Compute denominator d. If d <= 0, segment is parallel to or points - // away from triangle, so exit early - float d = qp.Dot(normal); - Vector3 e; - if (d > Constants::FloatEpsilon) - { - // the normal is on the right side - e = qp.Cross(ap); - } - else - { - normal = -normal; - - // so either have a parallel ray or our normal is flipped - if (d >= -Constants::FloatEpsilon) + // Compute denominator d. If d <= 0, segment is parallel to or points + // away from triangle, so exit early + float d = qp.Dot(normal); + if (d <= 0.0f) { - return false; // parallel - } - d = -d; - e = ap.Cross(qp); - } - - // Compute intersection t value of pq with plane of triangle. A ray - // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay - // dividing by d until intersection has been found to pierce triangle - t = ap.Dot(normal); - - // range segment check t[0,1] (it this case [0,d]) - if (t < 0.0f || t > d) - { - return false; - } - - // Compute barycentric coordinate components and test if within bounds - v = ac.Dot(e); - if (v < 0.0f || v > d) - { - return false; - } - w = -ab.Dot(e); - if (w < 0.0f || v + w > d) - { - return false; - } - - // Segment/ray intersects the triangle. Perform delayed division and - // compute the last barycentric coordinate component - float ood = 1.0f / d; - t *= ood; - //v *= ood; - //w *= ood; - //u = 1.0f - v - w; - - normal.Normalize(); - - return true; -} - -//========================================================================= -// TestSegmentAABBOrigin -// [10/21/2009] -//========================================================================= -bool -AZ::Intersect::TestSegmentAABBOrigin(const Vector3& midPoint, const Vector3& halfVector, const Vector3& aabbExtends) -{ - const Vector3 EPSILON(0.001f); // \todo this is slow load move to a const - Vector3 absHalfVector = halfVector.GetAbs(); - Vector3 absMidpoint = midPoint.GetAbs(); - Vector3 absHalfMidpoint = absHalfVector + aabbExtends; - - // Try world coordinate axes as separating axes - if (!absMidpoint.IsLessEqualThan(absHalfMidpoint)) - { - return false; - } - - // Add in an epsilon term to counteract arithmetic errors when segment is - // (near) parallel to a coordinate axis (see text for detail) - absHalfVector += EPSILON; - - // Try cross products of segment direction vector with coordinate axes - Vector3 absMDCross = midPoint.Cross(halfVector).GetAbs(); - //Vector3 eaDCross = absHalfVector.Cross(aabbExtends); - float ex = aabbExtends.GetX(); - float ey = aabbExtends.GetY(); - float ez = aabbExtends.GetZ(); - float adx = absHalfVector.GetX(); - float ady = absHalfVector.GetY(); - float adz = absHalfVector.GetZ(); - - Vector3 ead(ey * adz + ez * ady, ex * adz + ez * adx, ex * ady + ey * adx); - if (!absMDCross.IsLessEqualThan(ead)) - { - return false; - } - - // No separating axis found; segment must be overlapping AABB - return true; -} - - -//========================================================================= -// IntersectRayAABB -// [10/21/2009] -//========================================================================= -RayAABBIsectTypes -AZ::Intersect::IntersectRayAABB( - const Vector3& rayStart, const Vector3& dir, const Vector3& dirRCP, const Aabb& aabb, - float& tStart, float& tEnd, Vector3& startNormal /*, Vector3& inter*/) -{ - // we don't need to test with all 6 normals (just 3) - - const float eps = 0.0001f; // \todo move to constant - float tmin = 0.0f; // set to -RR_FLT_MAX to get first hit on line - float tmax = std::numeric_limits::max(); // set to max distance ray can travel (for segment) - - const Vector3& aabbMin = aabb.GetMin(); - const Vector3& aabbMax = aabb.GetMax(); - - // we unroll manually because there is no way to get in efficient way vectors for - // each axis while getting it as a index - Vector3 time1 = (aabbMin - rayStart) * dirRCP; - Vector3 time2 = (aabbMax - rayStart) * dirRCP; - - // X - if (std::fabs(dir.GetX()) < eps) - { - // Ray is parallel to slab. No hit if origin not within slab - if (rayStart.GetX() < aabbMin.GetX() || rayStart.GetX() > aabbMax.GetX()) - { - return ISECT_RAY_AABB_NONE; - } - } - else - { - // Compute intersection t value of ray with near and far plane of slab - float t1 = time1.GetX(); - float t2 = time2.GetX(); - float nSign = -1.0f; - - // Make t1 be intersection with near plane, t2 with far plane - if (t1 > t2) - { - AZStd::swap(t1, t2); - nSign = 1.0f; + return false; } - // Compute the intersection of slab intersections intervals - if (tmin < t1) - { - tmin = t1; + // Compute intersection t value of pq with plane of triangle. A ray + // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay + // dividing by d until intersection has been found to pierce triangle + Vector3 ap = p - a; + t = ap.Dot(normal); - startNormal.Set(nSign, 0.0f, 0.0f); + // range segment check t[0,1] (it this case [0,d]) + if (t < 0.0f || t > d) + { + return false; } - tmax = AZ::GetMin(tmax, t2); - - // Exit with no collision as soon as slab intersection becomes empty - if (tmin > tmax) + // Compute barycentric coordinate components and test if within bounds + Vector3 e = qp.Cross(ap); + v = ac.Dot(e); + if (v < 0.0f || v > d) { - return ISECT_RAY_AABB_NONE; + return false; } - } - - // Y - if (std::fabs(dir.GetY()) < eps) - { - // Ray is parallel to slab. No hit if origin not within slab - if (rayStart.GetY() < aabbMin.GetY() || rayStart.GetY() > aabbMax.GetY()) + w = -ab.Dot(e); + if (w < 0.0f || v + w > d) { - return ISECT_RAY_AABB_NONE; - } - } - else - { - // Compute intersection t value of ray with near and far plane of slab - float t1 = time1.GetY(); - float t2 = time2.GetY(); - float nSign = -1.0f; - - // Make t1 be intersection with near plane, t2 with far plane - if (t1 > t2) - { - AZStd::swap(t1, t2); - nSign = 1.0f; + return false; } - // Compute the intersection of slab intersections intervals - if (tmin < t1) - { - tmin = t1; + // Segment/ray intersects triangle. Perform delayed division and + // compute the last barycentric coordinate component + float ood = 1.0f / d; + t *= ood; + /*v *= ood; + w *= ood; + u = 1.0f - v - w;*/ - startNormal.Set(0.0f, nSign, 0.0f); + normal.Normalize(); + + return true; + } + + //========================================================================= + // IntersectSegmentTriangle + // [10/21/2009] + //========================================================================= + bool Intersect::IntersectSegmentTriangle( + const Vector3& p, + const Vector3& q, + const Vector3& a, + const Vector3& b, + const Vector3& c, + /*float &u, float &v, float &w,*/ Vector3& normal, + float& t) + { + float v, w; // comment this and enable input params if we need the barycentric coordinates + + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 qp = p - q; + Vector3 ap = p - a; + + // Compute triangle normal. Can be pre-calculated or cached if + // intersecting multiple segments against the same triangle + normal = ab.Cross(ac); // Right hand CCW + + // Compute denominator d. If d <= 0, segment is parallel to or points + // away from triangle, so exit early + float d = qp.Dot(normal); + Vector3 e; + if (d > Constants::FloatEpsilon) + { + // the normal is on the right side + e = qp.Cross(ap); } - - tmax = AZ::GetMin(tmax, t2); - - // Exit with no collision as soon as slab intersection becomes empty - if (tmin > tmax) + else { - return ISECT_RAY_AABB_NONE; - } - } + normal = -normal; - // Z - if (std::fabs(dir.GetZ()) < eps) - { - // Ray is parallel to slab. No hit if origin not within slab - if (rayStart.GetZ() < aabbMin.GetZ() || rayStart.GetZ() > aabbMax.GetZ()) - { - return ISECT_RAY_AABB_NONE; - } - } - else - { - // Compute intersection t value of ray with near and far plane of slab - float t1 = time1.GetZ(); - float t2 = time2.GetZ(); - float nSign = -1.0f; - - // Make t1 be intersection with near plane, t2 with far plane - if (t1 > t2) - { - AZStd::swap(t1, t2); - nSign = 1.0f; - } - - // Compute the intersection of slab intersections intervals - if (tmin < t1) - { - tmin = t1; - - startNormal.Set(0.0f, 0.0f, nSign); - } - - tmax = AZ::GetMin(tmax, t2); - - // Exit with no collision as soon as slab intersection becomes empty - if (tmin > tmax) - { - return ISECT_RAY_AABB_NONE; - } - } - - tStart = tmin; - tEnd = tmax; - - if (tmin == 0.0f) // no intersect if the segments starts inside or coincident the aabb - { - return ISECT_RAY_AABB_SA_INSIDE; - } - - // Ray intersects all 3 slabs. Return point (q) and intersection t value (tmin) - //inter = rayStart + dir * tmin; - return ISECT_RAY_AABB_ISECT; -} - -//========================================================================= -// IntersectRayAABB2 -// [2/18/2011] -//========================================================================= -RayAABBIsectTypes -AZ::Intersect::IntersectRayAABB2(const Vector3& rayStart, const Vector3& dirRCP, const Aabb& aabb, float& start, float& end) -{ - float tmin, tmax, tymin, tymax, tzmin, tzmax; - Vector3 vZero = Vector3::CreateZero(); - - Vector3 min = (Vector3::CreateSelectCmpGreaterEqual(dirRCP, vZero, aabb.GetMin(), aabb.GetMax()) - rayStart) * dirRCP; - Vector3 max = (Vector3::CreateSelectCmpGreaterEqual(dirRCP, vZero, aabb.GetMax(), aabb.GetMin()) - rayStart) * dirRCP; - - tmin = min.GetX(); - tmax = max.GetX(); - tymin = min.GetY(); - tymax = max.GetY(); - - if (tmin > tymax || tymin > tmax) - { - return ISECT_RAY_AABB_NONE; - } - - if (tymin > tmin) - { - tmin = tymin; - } - - if (tymax < tmax) - { - tmax = tymax; - } - - tzmin = min.GetZ(); - tzmax = max.GetZ(); - - if (tmin > tzmax || tzmin > tmax) - { - return ISECT_RAY_AABB_NONE; - } - - if (tzmin > tmin) - { - tmin = tzmin; - } - if (tzmax < tmax) - { - tmax = tzmax; - } - - start = tmin; - end = tmax; - - return ISECT_RAY_AABB_ISECT; -} - -bool AZ::Intersect::IntersectRayDisk( - const Vector3& rayOrigin, const Vector3& rayDir, const Vector3& diskCenter, const float diskRadius, const Vector3& diskNormal, float& t) -{ - // First intersect with the plane of the disk - float planeIntersectionDistance; - int intersectionCount = IntersectRayPlane(rayOrigin, rayDir, diskCenter, diskNormal, planeIntersectionDistance); - if (intersectionCount == 1) - { - // If the plane intersection point is inside the disk radius, then it intersected the disk. - Vector3 pointOnPlane = rayOrigin + rayDir * planeIntersectionDistance; - if (pointOnPlane.GetDistance(diskCenter) < diskRadius) - { - t = planeIntersectionDistance; - return true; - } - } - return false; -} - -// Reference: Real-Time Collision Detection - 5.3.7 Intersecting Ray or Segment Against Cylinder, and the book's errata. -int AZ::Intersect::IntersectRayCappedCylinder( - const Vector3& rayOrigin, const Vector3& rayDir, - const Vector3& cylinderEnd1, const Vector3& cylinderDir, - float cylinderHeight, float cylinderRadius, float &t1, float &t2) -{ - // dr = rayDir - // dc = cylinderDir - // r = cylinderRadius - // Vector3 cylinderEnd2 = cylinderEnd1 + cylinderHeight * cylinderDir; - Vector3 m = rayOrigin - cylinderEnd1; // vector from cylinderEnd1 to rayOrigin - float dcm = cylinderDir.Dot(m); // projection of m on cylinderDir - float dcdr = cylinderDir.Dot(rayDir); // projection of rayDir on cylinderDir - float drm = rayDir.Dot(m); // projection of m on rayDir - float r2 = cylinderRadius * cylinderRadius; - - if (dcm < 0.0f && dcdr <= 0.0f) - { - return 0; // rayOrigin is outside cylinderEnd1 and rayDir is pointing away from cylinderEnd1 - } - if (dcm > cylinderHeight && dcdr >= 0.0f) - { - return 0; // rayOrigin is outside cylinderEnd2 and rayDir is pointing away from cylinderEnd2 - } - - // point RP on the ray: RP(t) = rayOrigin + t * rayDir - // point CP on the cylinder surface: |(CP - cylinderEnd1) - cylinderDir.Dot(cp - cylinderEnd1) * cylinderDir|^2 = cylinderRadius^2 - // substitute RP(t) for CP: a*t^2 + 2b*t + c = 0, solving for t = [-2b +/- sqrt(4b^2 - 4ac)] / 2a - float a = 1.0f - dcdr * dcdr; // always greater than or equal to 0 - float b = drm - dcm * dcdr; - float c = m.Dot(m) - dcm * dcm - r2; - - const float EPSILON = 0.00001f; - - if (fabsf(a) < EPSILON) // the ray is parallel to the cylinder - { - if (c > EPSILON) // the ray is outside the cylinder - { - return 0; - } - else if (dcm < 0.0f) // the ray origin is on cylinderEnd1 side and ray is pointing to cylinderEnd2 - { - t1 = -dcm; - t2 = -dcm + cylinderHeight; - return 2; - } - else if (dcm > cylinderHeight) // the ray origin is on cylinderEnd2 side and ray is pointing to cylinderEnd1 - { - t1 = dcm - cylinderHeight; - t2 = dcm; - return 2; - } - else // (dcm > 0.0f && dcm < cylinderHeight) // the ray origin is inside the cylinder - { - if (dcdr > 0.0f) // the ray is pointing to cylinderEnd2 + // so either have a parallel ray or our normal is flipped + if (d >= -Constants::FloatEpsilon) { - t1 = cylinderHeight - dcm; - return 1; + return false; // parallel } - else if (dcdr < 0.0f) // the ray is pointing to cylinderEnd1 + d = -d; + e = ap.Cross(qp); + } + + // Compute intersection t value of pq with plane of triangle. A ray + // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay + // dividing by d until intersection has been found to pierce triangle + t = ap.Dot(normal); + + // range segment check t[0,1] (it this case [0,d]) + if (t < 0.0f || t > d) + { + return false; + } + + // Compute barycentric coordinate components and test if within bounds + v = ac.Dot(e); + if (v < 0.0f || v > d) + { + return false; + } + w = -ab.Dot(e); + if (w < 0.0f || v + w > d) + { + return false; + } + + // Segment/ray intersects the triangle. Perform delayed division and + // compute the last barycentric coordinate component + float ood = 1.0f / d; + t *= ood; + // v *= ood; + // w *= ood; + // u = 1.0f - v - w; + + normal.Normalize(); + + return true; + } + + //========================================================================= + // TestSegmentAABBOrigin + // [10/21/2009] + //========================================================================= + bool Intersect::TestSegmentAABBOrigin(const Vector3& midPoint, const Vector3& halfVector, const Vector3& aabbExtends) + { + const Vector3 EPSILON(0.001f); // \todo this is slow load move to a const + Vector3 absHalfVector = halfVector.GetAbs(); + Vector3 absMidpoint = midPoint.GetAbs(); + Vector3 absHalfMidpoint = absHalfVector + aabbExtends; + + // Try world coordinate axes as separating axes + if (!absMidpoint.IsLessEqualThan(absHalfMidpoint)) + { + return false; + } + + // Add in an epsilon term to counteract arithmetic errors when segment is + // (near) parallel to a coordinate axis (see text for detail) + absHalfVector += EPSILON; + + // Try cross products of segment direction vector with coordinate axes + Vector3 absMDCross = midPoint.Cross(halfVector).GetAbs(); + // Vector3 eaDCross = absHalfVector.Cross(aabbExtends); + float ex = aabbExtends.GetX(); + float ey = aabbExtends.GetY(); + float ez = aabbExtends.GetZ(); + float adx = absHalfVector.GetX(); + float ady = absHalfVector.GetY(); + float adz = absHalfVector.GetZ(); + + Vector3 ead(ey * adz + ez * ady, ex * adz + ez * adx, ex * ady + ey * adx); + if (!absMDCross.IsLessEqualThan(ead)) + { + return false; + } + + // No separating axis found; segment must be overlapping AABB + return true; + } + + //========================================================================= + // IntersectRayAABB + // [10/21/2009] + //========================================================================= + Intersect::RayAABBIsectTypes Intersect::IntersectRayAABB( + const Vector3& rayStart, + const Vector3& dir, + const Vector3& dirRCP, + const Aabb& aabb, + float& tStart, + float& tEnd, + Vector3& startNormal /*, Vector3& inter*/) + { + // we don't need to test with all 6 normals (just 3) + + const float eps = 0.0001f; // \todo move to constant + float tmin = 0.0f; // set to -RR_FLT_MAX to get first hit on line + float tmax = std::numeric_limits::max(); // set to max distance ray can travel (for segment) + + const Vector3& aabbMin = aabb.GetMin(); + const Vector3& aabbMax = aabb.GetMax(); + + // we unroll manually because there is no way to get in efficient way vectors for + // each axis while getting it as a index + Vector3 time1 = (aabbMin - rayStart) * dirRCP; + Vector3 time2 = (aabbMax - rayStart) * dirRCP; + + // X + if (std::fabs(dir.GetX()) < eps) + { + // Ray is parallel to slab. No hit if origin not within slab + if (rayStart.GetX() < aabbMin.GetX() || rayStart.GetX() > aabbMax.GetX()) { - t2 = dcm; - return 1; + return ISECT_RAY_AABB_NONE; } - else // impossible in theory + } + else + { + // Compute intersection t value of ray with near and far plane of slab + float t1 = time1.GetX(); + float t2 = time2.GetX(); + float nSign = -1.0f; + + // Make t1 be intersection with near plane, t2 with far plane + if (t1 > t2) + { + AZStd::swap(t1, t2); + nSign = 1.0f; + } + + // Compute the intersection of slab intersections intervals + if (tmin < t1) + { + tmin = t1; + + startNormal.Set(nSign, 0.0f, 0.0f); + } + + tmax = AZ::GetMin(tmax, t2); + + // Exit with no collision as soon as slab intersection becomes empty + if (tmin > tmax) + { + return ISECT_RAY_AABB_NONE; + } + } + + // Y + if (std::fabs(dir.GetY()) < eps) + { + // Ray is parallel to slab. No hit if origin not within slab + if (rayStart.GetY() < aabbMin.GetY() || rayStart.GetY() > aabbMax.GetY()) + { + return ISECT_RAY_AABB_NONE; + } + } + else + { + // Compute intersection t value of ray with near and far plane of slab + float t1 = time1.GetY(); + float t2 = time2.GetY(); + float nSign = -1.0f; + + // Make t1 be intersection with near plane, t2 with far plane + if (t1 > t2) + { + AZStd::swap(t1, t2); + nSign = 1.0f; + } + + // Compute the intersection of slab intersections intervals + if (tmin < t1) + { + tmin = t1; + + startNormal.Set(0.0f, nSign, 0.0f); + } + + tmax = AZ::GetMin(tmax, t2); + + // Exit with no collision as soon as slab intersection becomes empty + if (tmin > tmax) + { + return ISECT_RAY_AABB_NONE; + } + } + + // Z + if (std::fabs(dir.GetZ()) < eps) + { + // Ray is parallel to slab. No hit if origin not within slab + if (rayStart.GetZ() < aabbMin.GetZ() || rayStart.GetZ() > aabbMax.GetZ()) + { + return ISECT_RAY_AABB_NONE; + } + } + else + { + // Compute intersection t value of ray with near and far plane of slab + float t1 = time1.GetZ(); + float t2 = time2.GetZ(); + float nSign = -1.0f; + + // Make t1 be intersection with near plane, t2 with far plane + if (t1 > t2) + { + AZStd::swap(t1, t2); + nSign = 1.0f; + } + + // Compute the intersection of slab intersections intervals + if (tmin < t1) + { + tmin = t1; + + startNormal.Set(0.0f, 0.0f, nSign); + } + + tmax = AZ::GetMin(tmax, t2); + + // Exit with no collision as soon as slab intersection becomes empty + if (tmin > tmax) + { + return ISECT_RAY_AABB_NONE; + } + } + + tStart = tmin; + tEnd = tmax; + + if (tmin == 0.0f) // no intersect if the segments starts inside or coincident the aabb + { + return ISECT_RAY_AABB_SA_INSIDE; + } + + // Ray intersects all 3 slabs. Return point (q) and intersection t value (tmin) + // inter = rayStart + dir * tmin; + return ISECT_RAY_AABB_ISECT; + } + + //========================================================================= + // IntersectRayAABB2 + // [2/18/2011] + //========================================================================= + Intersect::RayAABBIsectTypes Intersect::IntersectRayAABB2( + const Vector3& rayStart, const Vector3& dirRCP, const Aabb& aabb, float& start, float& end) + { + float tmin, tmax, tymin, tymax, tzmin, tzmax; + Vector3 vZero = Vector3::CreateZero(); + + Vector3 min = (Vector3::CreateSelectCmpGreaterEqual(dirRCP, vZero, aabb.GetMin(), aabb.GetMax()) - rayStart) * dirRCP; + Vector3 max = (Vector3::CreateSelectCmpGreaterEqual(dirRCP, vZero, aabb.GetMax(), aabb.GetMin()) - rayStart) * dirRCP; + + tmin = min.GetX(); + tmax = max.GetX(); + tymin = min.GetY(); + tymax = max.GetY(); + + if (tmin > tymax || tymin > tmax) + { + return ISECT_RAY_AABB_NONE; + } + + if (tymin > tmin) + { + tmin = tymin; + } + + if (tymax < tmax) + { + tmax = tymax; + } + + tzmin = min.GetZ(); + tzmax = max.GetZ(); + + if (tmin > tzmax || tzmin > tmax) + { + return ISECT_RAY_AABB_NONE; + } + + if (tzmin > tmin) + { + tmin = tzmin; + } + if (tzmax < tmax) + { + tmax = tzmax; + } + + start = tmin; + end = tmax; + + return ISECT_RAY_AABB_ISECT; + } + + bool Intersect::IntersectRayDisk( + const Vector3& rayOrigin, + const Vector3& rayDir, + const Vector3& diskCenter, + const float diskRadius, + const Vector3& diskNormal, + float& t) + { + // First intersect with the plane of the disk + float planeIntersectionDistance; + int intersectionCount = IntersectRayPlane(rayOrigin, rayDir, diskCenter, diskNormal, planeIntersectionDistance); + if (intersectionCount == 1) + { + // If the plane intersection point is inside the disk radius, then it intersected the disk. + Vector3 pointOnPlane = rayOrigin + rayDir * planeIntersectionDistance; + if (pointOnPlane.GetDistance(diskCenter) < diskRadius) + { + t = planeIntersectionDistance; + return true; + } + } + return false; + } + + // Reference: Real-Time Collision Detection - 5.3.7 Intersecting Ray or Segment Against Cylinder, and the book's errata. + int Intersect::IntersectRayCappedCylinder( + const Vector3& rayOrigin, + const Vector3& rayDir, + const Vector3& cylinderEnd1, + const Vector3& cylinderDir, + float cylinderHeight, + float cylinderRadius, + float& t1, + float& t2) + { + // dr = rayDir + // dc = cylinderDir + // r = cylinderRadius + // Vector3 cylinderEnd2 = cylinderEnd1 + cylinderHeight * cylinderDir; + Vector3 m = rayOrigin - cylinderEnd1; // vector from cylinderEnd1 to rayOrigin + float dcm = cylinderDir.Dot(m); // projection of m on cylinderDir + float dcdr = cylinderDir.Dot(rayDir); // projection of rayDir on cylinderDir + float drm = rayDir.Dot(m); // projection of m on rayDir + float r2 = cylinderRadius * cylinderRadius; + + if (dcm < 0.0f && dcdr <= 0.0f) + { + return 0; // rayOrigin is outside cylinderEnd1 and rayDir is pointing away from cylinderEnd1 + } + if (dcm > cylinderHeight && dcdr >= 0.0f) + { + return 0; // rayOrigin is outside cylinderEnd2 and rayDir is pointing away from cylinderEnd2 + } + + // point RP on the ray: RP(t) = rayOrigin + t * rayDir + // point CP on the cylinder surface: |(CP - cylinderEnd1) - cylinderDir.Dot(cp - cylinderEnd1) * cylinderDir|^2 = cylinderRadius^2 + // substitute RP(t) for CP: a*t^2 + 2b*t + c = 0, solving for t = [-2b +/- sqrt(4b^2 - 4ac)] / 2a + float a = 1.0f - dcdr * dcdr; // always greater than or equal to 0 + float b = drm - dcm * dcdr; + float c = m.Dot(m) - dcm * dcm - r2; + + const float EPSILON = 0.00001f; + + if (fabsf(a) < EPSILON) // the ray is parallel to the cylinder + { + if (c > EPSILON) // the ray is outside the cylinder { return 0; } + else if (dcm < 0.0f) // the ray origin is on cylinderEnd1 side and ray is pointing to cylinderEnd2 + { + t1 = -dcm; + t2 = -dcm + cylinderHeight; + return 2; + } + else if (dcm > cylinderHeight) // the ray origin is on cylinderEnd2 side and ray is pointing to cylinderEnd1 + { + t1 = dcm - cylinderHeight; + t2 = dcm; + return 2; + } + else // (dcm > 0.0f && dcm < cylinderHeight) // the ray origin is inside the cylinder + { + if (dcdr > 0.0f) // the ray is pointing to cylinderEnd2 + { + t1 = cylinderHeight - dcm; + return 1; + } + else if (dcdr < 0.0f) // the ray is pointing to cylinderEnd1 + { + t2 = dcm; + return 1; + } + else // impossible in theory + { + return 0; + } + } } - } - float discr = b * b - a * c; - if (discr < 0.0f) - { - return 0; - } - - float sqrt_discr = sqrt(discr); - float tt1 = (-b - sqrt_discr) / a; - float tt2 = (-b + sqrt_discr) / a; - - if (tt2 < 0.0f) // both intersections are behind the ray origin - { - return 0; - } - - // Vector3 AP2 = (rayOrigin + tt2 * rayDir) - cylinderEnd1; // vector from cylinderEnd1 to the intersecting point of parameter tt2 - // float s2 = cylinderDir.Dot(AP2); - float s2 = dcm + tt2 * dcdr; - - if (discr < EPSILON) // tt1 == tt2 - { - if (s2 >= 0.0f && s2 <= cylinderHeight) - { - t1 = tt1; - return 1; - } - } - - // Vector3 AP1 = (rayOrigin + tt1 * rayDir) - cylinderEnd1; // vector from cylinderEnd1 to the intersecting point of parameter tt1 - // float s1 = cylinderDir.Dot(AP1); - float s1 = dcm + tt1 * dcdr; - - if (s1 < 0.0f) // intersecting point of parameter tt1 is outside on cylinderEnd1 side - { - if (s2 < 0.0f) // intersecting point of parameter tt2 is outside on cylinderEnd1 side + float discr = b * b - a * c; + if (discr < 0.0f) { return 0; } - else if (s2 == 0.0f) // ray touching the brim of the cylinderEnd1 + + float sqrt_discr = sqrt(discr); + float tt1 = (-b - sqrt_discr) / a; + float tt2 = (-b + sqrt_discr) / a; + + if (tt2 < 0.0f) // both intersections are behind the ray origin { - t1 = tt2; - return 1; + return 0; } - else + + // Vector3 AP2 = (rayOrigin + tt2 * rayDir) - cylinderEnd1; // vector from cylinderEnd1 to the intersecting point of parameter tt2 + // float s2 = cylinderDir.Dot(AP2); + float s2 = dcm + tt2 * dcdr; + + if (discr < EPSILON) // tt1 == tt2 { - if (s2 > cylinderHeight) // intersecting point of parameter tt2 is outside on cylinderEnd2 side + if (s2 >= 0.0f && s2 <= cylinderHeight) { - // t2 can be computed from the equation: dot(rayOrigin + t2 * rayDir - cylinderEnd1, cylinderDir) = cylinderHeight - t2 = (cylinderHeight - dcm) / dcdr; + t1 = tt1; + return 1; } - else + } + + // Vector3 AP1 = (rayOrigin + tt1 * rayDir) - cylinderEnd1; // vector from cylinderEnd1 to the intersecting point of parameter tt1 + // float s1 = cylinderDir.Dot(AP1); + float s1 = dcm + tt1 * dcdr; + + if (s1 < 0.0f) // intersecting point of parameter tt1 is outside on cylinderEnd1 side + { + if (s2 < 0.0f) // intersecting point of parameter tt2 is outside on cylinderEnd1 side { - t2 = tt2; + return 0; } - if (dcm > 0.0f) // ray origin inside cylinder + else if (s2 == 0.0f) // ray touching the brim of the cylinderEnd1 { - t1 = t2; + t1 = tt2; return 1; } else { - // t1 can be computed from the equation: dot(rayOrigin + t1 * rayDir - cylinderEnd1, cylinderDir) = 0 - t1 = -dcm / dcdr; - return 2; + if (s2 > cylinderHeight) // intersecting point of parameter tt2 is outside on cylinderEnd2 side + { + // t2 can be computed from the equation: dot(rayOrigin + t2 * rayDir - cylinderEnd1, cylinderDir) = cylinderHeight + t2 = (cylinderHeight - dcm) / dcdr; + } + else + { + t2 = tt2; + } + if (dcm > 0.0f) // ray origin inside cylinder + { + t1 = t2; + return 1; + } + else + { + // t1 can be computed from the equation: dot(rayOrigin + t1 * rayDir - cylinderEnd1, cylinderDir) = 0 + t1 = -dcm / dcdr; + return 2; + } } } - } - else if (s1 > cylinderHeight) // intersecting point of parameter tt1 is outside on cylinderEnd2 side - { - if (s2 > cylinderHeight) // intersecting point of parameter tt2 is outside on cylinderEnd2 side + else if (s1 > cylinderHeight) // intersecting point of parameter tt1 is outside on cylinderEnd2 side { - return 0; + if (s2 > cylinderHeight) // intersecting point of parameter tt2 is outside on cylinderEnd2 side + { + return 0; + } + else if (s2 == cylinderHeight) + { + t1 = tt2; + return 1; + } + else + { + if (s2 < 0.0f) + { + t2 = -dcm / dcdr; + } + else + { + t2 = tt2; + } + if (dcm < cylinderHeight) + { + t1 = t2; + return 1; + } + else + { + t1 = (cylinderHeight - dcm) / dcdr; + return 2; + } + } } - else if (s2 == cylinderHeight) - { - t1 = tt2; - return 1; - } - else + else // intersecting point of parameter tt1 is in between two cylinder ends { if (s2 < 0.0f) { t2 = -dcm / dcdr; } + else if (s2 > cylinderHeight) + { + t2 = (cylinderHeight - dcm) / dcdr; + } else { t2 = tt2; } - if (dcm < cylinderHeight) + if (tt1 > 0.0f) + { + t1 = tt1; + return 2; + } + else { t1 = t2; return 1; } - else - { - t1 = (cylinderHeight - dcm) / dcdr; - return 2; - } } } - else // intersecting point of parameter tt1 is in between two cylinder ends + + int Intersect::IntersectRayCone( + const Vector3& rayOrigin, + const Vector3& rayDir, + const Vector3& coneApex, + const Vector3& coneDir, + float coneHeight, + float coneBaseRadius, + float& t1, + float& t2) { - if (s2 < 0.0f) + // Q = rayOrgin, A = coneApex + Vector3 AQ = rayOrigin - coneApex; + float m = coneDir.Dot(AQ); // projection of m on cylinderDir + float k = coneDir.Dot(rayDir); // projection of rayDir on cylinderDir + + if (m < 0.0f && k <= 0.0f) { - t2 = -dcm / dcdr; + // rayOrigin is outside the cone on coneApex side and rayDir is pointing away + return 0; } - else if (s2 > cylinderHeight) - { - t2 = (cylinderHeight - dcm) / dcdr; - } - else - { - t2 = tt2; - } - if (tt1 > 0.0f) - { - t1 = tt1; - return 2; - } - else - { - t1 = t2; - return 1; - } - } -} - -int AZ::Intersect::IntersectRayCone( - const Vector3& rayOrigin, const Vector3& rayDir, - const Vector3& coneApex, const Vector3& coneDir, float coneHeight, - float coneBaseRadius, float& t1, float& t2) -{ - // Q = rayOrgin, A = coneApex - Vector3 AQ = rayOrigin - coneApex; - float m = coneDir.Dot(AQ); // projection of m on cylinderDir - float k = coneDir.Dot(rayDir); // projection of rayDir on cylinderDir - - if (m < 0.0f && k <= 0.0f) - { - // rayOrigin is outside the cone on coneApex side and rayDir is pointing away - return 0; - } - if (m > coneHeight && k >= 0.0f) - { - // rayOrigin is outside the cone on coneBase side and rayDir is pointing away - return 0; - } - - float r2 = coneBaseRadius * coneBaseRadius; - float h2 = coneHeight * coneHeight; - - float m2 = m * m; - float k2 = k * k; - float q2 = AQ.Dot(AQ); - - float n = rayDir.Dot(AQ); - - const float EPSILON = 0.00001f; - - // point RP on the ray: RP(t) = rayOrigin + t * rayDir - // point CP on the cone surface: similar triangle property - // |dot(CP - A, coneDir) * coneDir| / coneHeight = |(CP - A) - (dot(CP - A, coneDir) * coneDir)| coneRadius - // substitute RP(t) for CP: a*t^2 + 2b*t + c = 0, solving for t - float a = (r2 + h2) * k2 - h2; - float b = (r2 + h2) * m * k - h2 * n; - float c = (r2 + h2) * m2 - h2 * q2; - - float discriminant = b * b - a * c; - if (discriminant < -EPSILON) - { - return 0; - } - discriminant = AZ::GetMax(discriminant, 0.0f); - - if (fabsf(a) < EPSILON) // the ray is parallel to the cone surface's tangent line - { - if (b < EPSILON && fabsf(c) < EPSILON) // ray overlapping with cone surface - { - t1 = rayDir.Dot(coneApex - rayOrigin); - } - else // ray has only one intersecting point with the cone - { - t1 = -c / (2 * b); - } - - t2 = (coneHeight - m) / k; // t2 can be computed from the equation: dot(Q + t2 * rayDir - A, coneDir) = coneHeight - - if (t1 < 0.0f && t2 < 0.0f) + if (m > coneHeight && k >= 0.0f) { + // rayOrigin is outside the cone on coneBase side and rayDir is pointing away return 0; } - if (fabsf(t1 - t2) < EPSILON) // the ray intersects the brim of the circumference of the cone base - { - return 1; - } + float r2 = coneBaseRadius * coneBaseRadius; + float h2 = coneHeight * coneHeight; - float s1 = m + t1 * k; // coneDir.Dot(rayOrigin + t1 * rayDir - coneApex); - if (s1 < 0.0f || s1 > coneHeight) + float m2 = m * m; + float k2 = k * k; + float q2 = AQ.Dot(AQ); + + float n = rayDir.Dot(AQ); + + const float EPSILON = 0.00001f; + + // point RP on the ray: RP(t) = rayOrigin + t * rayDir + // point CP on the cone surface: similar triangle property + // |dot(CP - A, coneDir) * coneDir| / coneHeight = |(CP - A) - (dot(CP - A, coneDir) * coneDir)| coneRadius + // substitute RP(t) for CP: a*t^2 + 2b*t + c = 0, solving for t + float a = (r2 + h2) * k2 - h2; + float b = (r2 + h2) * m * k - h2 * n; + float c = (r2 + h2) * m2 - h2 * q2; + + float discriminant = b * b - a * c; + if (discriminant < -EPSILON) { return 0; } - else + discriminant = AZ::GetMax(discriminant, 0.0f); + + if (fabsf(a) < EPSILON) // the ray is parallel to the cone surface's tangent line { - if (k < 0.0f) // ray shooting from base to apex + if (b < EPSILON && fabsf(c) < EPSILON) // ray overlapping with cone surface { - if (m >= coneHeight) // ray origin outside cone - { - float temp = t1; - t1 = t2; - t2 = temp; - return 2; - } - else if (t1 >= 0.0f) // ray origin inside cone - { - t1 = t2; - return 1; - } - else - { - return 0; - } + t1 = rayDir.Dot(coneApex - rayOrigin); + } + else // ray has only one intersecting point with the cone + { + t1 = -c / (2 * b); + } + + t2 = (coneHeight - m) / k; // t2 can be computed from the equation: dot(Q + t2 * rayDir - A, coneDir) = coneHeight + + if (t1 < 0.0f && t2 < 0.0f) + { + return 0; + } + + if (fabsf(t1 - t2) < EPSILON) // the ray intersects the brim of the circumference of the cone base + { + return 1; + } + + float s1 = m + t1 * k; // coneDir.Dot(rayOrigin + t1 * rayDir - coneApex); + if (s1 < 0.0f || s1 > coneHeight) + { + return 0; } else { - if (m > coneHeight) + if (k < 0.0f) // ray shooting from base to apex { - return 0; - } - if (t1 >= 0.0f) // ray origin outside cone - { - return 2; - } - else - { - t1 = t2; - return 1; - } - } - } - } - - if (discriminant < EPSILON) // two intersecting points coincide - { - if (fabsf(n * n - q2) < EPSILON) // the ray is through the apex - { - float cosineA2 = h2 / (r2 + h2); - float cosineAQ2 = cosineA2 * q2; - - if (m2 > cosineAQ2) // the ray origin is inside the cone or its mirroring counterpart - { - if (m <= 0.0f) // the ray origin outside the cone on the apex side, shooting towards the base - { - t1 = -b / a; - t2 = (coneHeight - m) / k; - return 2; - } - else if (m >= coneHeight) // the ray origin is outside the cone on the base side, shooting towards towards the apex - { - t1 = (coneHeight - m) / k; - t2 = -b / a; - return 2; - } - else - { - if (k > 0.0f) // the ray origin is inside the cone, shooting towards the base + if (m >= coneHeight) // ray origin outside cone { - t1 = (coneHeight - m) / k; + float temp = t1; + t1 = t2; + t2 = temp; + return 2; + } + else if (t1 >= 0.0f) // ray origin inside cone + { + t1 = t2; return 1; } - else // the ray origin is inside the cone, shooting towards the apex + else + { + return 0; + } + } + else + { + if (m > coneHeight) + { + return 0; + } + if (t1 >= 0.0f) // ray origin outside cone + { + return 2; + } + else + { + t1 = t2; + return 1; + } + } + } + } + + if (discriminant < EPSILON) // two intersecting points coincide + { + if (fabsf(n * n - q2) < EPSILON) // the ray is through the apex + { + float cosineA2 = h2 / (r2 + h2); + float cosineAQ2 = cosineA2 * q2; + + if (m2 > cosineAQ2) // the ray origin is inside the cone or its mirroring counterpart + { + if (m <= 0.0f) // the ray origin outside the cone on the apex side, shooting towards the base { t1 = -b / a; + t2 = (coneHeight - m) / k; + return 2; + } + else if (m >= coneHeight) // the ray origin is outside the cone on the base side, shooting towards towards the apex + { + t1 = (coneHeight - m) / k; + t2 = -b / a; + return 2; + } + else + { + if (k > 0.0f) // the ray origin is inside the cone, shooting towards the base + { + t1 = (coneHeight - m) / k; + return 1; + } + else // the ray origin is inside the cone, shooting towards the apex + { + t1 = -b / a; + return 1; + } + } + } + else // the ray origin is outside the cone + { + t1 = -b / a; + if (t1 > 0.0f) + { return 1; } + else + { + return 0; + } } } - else // the ray origin is outside the cone + else // the ray is touching the cone surface but not through the apex { t1 = -b / a; if (t1 > 0.0f) { + float s1 = m + t1 * k; // projection length of the line segment from the apex to intersection_t1 onto the coneDir + if (s1 >= 0.0f && s1 <= coneHeight) + { + return 1; + } + } + return 0; + } + } + + float sqrtDiscr = sqrt(discriminant); + float tt1 = (-b - sqrtDiscr) / a; + float tt2 = (-b + sqrtDiscr) / a; + + /* Test s1 and s2 to see the positions of the intersecting points relative to the cylinder's two ends. */ + + // s1 = coneDir.Dot(rayOrigin + tt1 * rayDir - coneApex), which expands into the following + float s1 = m + tt1 * k; + // s2 = coneDir.Dot(rayOrigin + tt2 * rayDir - coneApex), which expands into the following + float s2 = m + tt2 * k; + + if (s1 < 0.0f) + { + if (s2 < 0.0f || s2 > coneHeight) + { + return 0; + } + else + { + if (tt2 >= 0.0f) // ray origin outside cone + { + t1 = tt2; + t2 = (coneHeight - m) / k; + return 2; + } + else if (m > coneHeight) // ray origin outside cone on the base side, the + { + return 0; + } + else + { + t1 = (coneHeight - m) / k; + return 1; + } + } + } + else if (s1 > coneHeight) + { + if (s2 < 0.0f || s2 > coneHeight) + { + return 0; + } + else + { + if (tt2 < 0.0f) + { + return 0; + } + else if (m >= coneHeight) + { + t1 = (coneHeight - m) / k; + t2 = tt2; + return 2; + } + else // ray origin inside cone + { + t1 = tt2; + return 1; + } + } + } + else + { + if (s2 < 0.0f) + { + if (m >= coneHeight) + { + t1 = (coneHeight - m) / k; + t2 = tt1; + return 2; + } + else if (tt1 >= 0.0f) // ray origin inside cone + { + t1 = tt1; + return 1; + } + else + { + return 0; + } + } + else if (s2 > coneHeight) + { + if (tt1 >= 0.0f) + { + t1 = tt1; + t2 = (coneHeight - m) / k; + return 2; + } + else if (m <= coneHeight) + { + t1 = (coneHeight - m) / k; + return 1; + } + else + { + return 0; + } + } + else + { + if (tt1 >= 0.0f) + { + t1 = tt1; + t2 = tt2; + return 2; + } + else if (tt2 >= 0.0f) + { + t1 = tt2; return 1; } else @@ -788,778 +946,670 @@ int AZ::Intersect::IntersectRayCone( } } } - else // the ray is touching the cone surface but not through the apex + } + + int Intersect::IntersectRayPlane( + const Vector3& rayOrigin, const Vector3& rayDir, const Vector3& planePos, const Vector3& planeNormal, float& t) + { + // (rayOrigin + t * rayDir - planePos).dot(planeNormal) = 0 + + const float EPSILON = 0.00001f; + + float n = rayDir.Dot(planeNormal); + if (fabsf(n) < EPSILON) { - t1 = -b / a; - if (t1 > 0.0f) - { - float s1 = m + t1 * k; // projection length of the line segment from the apex to intersection_t1 onto the coneDir - if (s1 >= 0.0f && s1 <= coneHeight) - { - return 1; - } - } return 0; } - } - - float sqrtDiscr = sqrt(discriminant); - float tt1 = (-b - sqrtDiscr) / a; - float tt2 = (-b + sqrtDiscr) / a; - /* Test s1 and s2 to see the positions of the intersecting points relative to the cylinder's two ends. */ - - // s1 = coneDir.Dot(rayOrigin + tt1 * rayDir - coneApex), which expands into the following - float s1 = m + tt1 * k; - // s2 = coneDir.Dot(rayOrigin + tt2 * rayDir - coneApex), which expands into the following - float s2 = m + tt2 * k; - - if (s1 < 0.0f) - { - if (s2 < 0.0f || s2 > coneHeight) + t = planeNormal.Dot(planePos - rayOrigin) / n; + if (t < 0.0f) { return 0; } else { - if (tt2 >= 0.0f) // ray origin outside cone - { - t1 = tt2; - t2 = (coneHeight - m) / k; - return 2; - } - else if (m > coneHeight) // ray origin outside cone on the base side, the - { - return 0; - } - else - { - t1 = (coneHeight - m) / k; - return 1; - } + return 1; } } - else if (s1 > coneHeight) + + int Intersect::IntersectRayQuad( + const Vector3& rayOrigin, + const Vector3& rayDir, + const Vector3& vertexA, + const Vector3& vertexB, + const Vector3& vertexC, + const Vector3& vertexD, + float& t) { - if (s2 < 0.0f || s2 > coneHeight ) + const float EPSILON = 0.0001f; + + Vector3 AC = vertexC - vertexA; + Vector3 AB = vertexB - vertexA; + Vector3 QA = vertexA - rayOrigin; + + Vector3 triN = AB.Cross(AC); // the normal of the triangle ABC + float dn = rayDir.Dot(triN); + + // Early-out if ray is facing away from ABC triangle + if (dn * triN.Dot(QA) < 0) { return 0; } - else + + Vector3 E = rayDir.Cross(QA); + float dnAbs = 0.0f; + + if (dn < -EPSILON) // vertices have counter-clock wise winding when looking at the quad from rayOrigin { - if (tt2 < 0.0f) - { - return 0; - } - else if (m >= coneHeight) - { - t1 = (coneHeight - m) / k; - t2 = tt2; - return 2; - } - else // ray origin inside cone - { - t1 = tt2; - return 1; - } + dnAbs = -dn; } - } - else - { - if (s2 < 0.0f) + else if (dn > EPSILON) { - if (m >= coneHeight) - { - t1 = (coneHeight - m) / k; - t2 = tt1; - return 2; - } - else if (tt1 >= 0.0f) // ray origin inside cone - { - t1 = tt1; - return 1; - } - else + E = -E; + dnAbs = dn; + } + else // the ray is parallel to the quad plane + { + return 0; + } + + // compute barycentric coordinates + float v = E.Dot(AC); + + if (v >= 0.0f && v < dnAbs) + { + float w = -E.Dot(AB); + if (w < 0.0f || v + w > dnAbs) { return 0; } } - else if (s2 > coneHeight) + else if (v < 0.0f && v > -dnAbs) { - if (tt1 >= 0.0f) - { - t1 = tt1; - t2 = (coneHeight - m) / k; - return 2; - } - else if (m <= coneHeight) - { - t1 = (coneHeight - m) / k; - return 1; - } - else + Vector3 DA = vertexA - vertexD; + float w = E.Dot(DA); + if (w > 0.0f || v + w < -dnAbs) // v, w are negative { return 0; } } else { - if (tt1 >= 0.0f) - { - t1 = tt1; - t2 = tt2; - return 2; - } - else if (tt2 >= 0.0f) - { - t1 = tt2; - return 1; - } - else - { - return 0; - } + return 0; } - } -} -int AZ::Intersect::IntersectRayPlane(const Vector3& rayOrigin, const Vector3& rayDir, const Vector3& planePos, const Vector3& planeNormal, float& t) -{ - // (rayOrigin + t * rayDir - planePos).dot(planeNormal) = 0 - - const float EPSILON = 0.00001f; - - float n = rayDir.Dot(planeNormal); - if (fabsf(n) < EPSILON) - { - return 0; - } - - t = planeNormal.Dot(planePos - rayOrigin) / n; - if (t < 0.0f) - { - return 0; - } - else - { + t = triN.Dot(QA) / dn; return 1; } -} -int AZ::Intersect::IntersectRayQuad( - const Vector3& rayOrigin, const Vector3& rayDir, const Vector3& vertexA, - const Vector3& vertexB, const Vector3& vertexC, const Vector3& vertexD, float& t) -{ - const float EPSILON = 0.0001f; - - Vector3 AC = vertexC - vertexA; - Vector3 AB = vertexB - vertexA; - Vector3 QA = vertexA - rayOrigin; - - Vector3 triN = AB.Cross(AC); // the normal of the triangle ABC - float dn = rayDir.Dot(triN); - - // Early-out if ray is facing away from ABC triangle - if (dn * triN.Dot(QA) < 0) + // reference: Real-Time Collision Detection, 5.3.3 Intersecting Ray or Segment Against Box + bool Intersect::IntersectRayBox( + const Vector3& rayOrigin, + const Vector3& rayDir, + const Vector3& boxCenter, + const Vector3& boxAxis1, + const Vector3& boxAxis2, + const Vector3& boxAxis3, + float boxHalfExtent1, + float boxHalfExtent2, + float boxHalfExtent3, + float& t) { - return 0; - } + const float EPSILON = 0.00001f; - Vector3 E = rayDir.Cross(QA); - float dnAbs = 0.0f; + float tmin = 0.0f; // the nearest to the ray origin + float tmax = AZ::Constants::FloatMax; // the farthest from the ray origin - if (dn < -EPSILON) // vertices have counter-clock wise winding when looking at the quad from rayOrigin - { - dnAbs = -dn; - } - else if (dn > EPSILON) - { - E = -E; - dnAbs = dn; - } - else // the ray is parallel to the quad plane - { - return 0; - } + Vector3 P = boxCenter - rayOrigin; // precomputed variable for calculating the vector from rayOrigin to a point on each box facet + Vector3 QAp; // vector from rayOrigin to the center of the facet of boxAxis + Vector3 QAn; // vector from rayOrigin to the center of the facet of -boxAxis + float tp = 0.0f; + float tn = 0.0f; + bool isRayOriginInsideBox = true; - // compute barycentric coordinates - float v = E.Dot(AC); + /* Test the slab_1 formed by the planes with normals boxAxis1 and -boxAxis1. */ - if (v >= 0.0f && v < dnAbs) - { - float w = -E.Dot(AB); - if (w < 0.0f || v + w > dnAbs) + Vector3 axis1 = boxHalfExtent1 * boxAxis1; + + QAp = P + axis1; + tp = QAp.Dot(boxAxis1); + + QAn = P - axis1; + tn = -QAn.Dot(boxAxis1); + + float n = rayDir.Dot(boxAxis1); + if (fabsf(n) < EPSILON) { - return 0; - } - } - else if (v < 0.0f && v > -dnAbs) - { - Vector3 DA = vertexA - vertexD; - float w = E.Dot(DA); - if (w > 0.0f || v + w < -dnAbs) // v, w are negative - { - return 0; - } - } - else - { - return 0; - } - - t = triN.Dot(QA) / dn; - return 1; -} - -// reference: Real-Time Collision Detection, 5.3.3 Intersecting Ray or Segment Against Box -bool AZ::Intersect::IntersectRayBox( - const Vector3& rayOrigin, const Vector3& rayDir, const Vector3& boxCenter, const Vector3& boxAxis1, - const Vector3& boxAxis2, const Vector3& boxAxis3, float boxHalfExtent1, float boxHalfExtent2, float boxHalfExtent3, float& t) -{ - const float EPSILON = 0.00001f; - - float tmin = 0.0f; // the nearest to the ray origin - float tmax = AZ::Constants::FloatMax; // the farthest from the ray origin - - Vector3 P = boxCenter - rayOrigin; // precomputed variable for calculating the vector from rayOrigin to a point on each box facet - Vector3 QAp; // vector from rayOrigin to the center of the facet of boxAxis - Vector3 QAn; // vector from rayOrigin to the center of the facet of -boxAxis - float tp = 0.0f; - float tn = 0.0f; - bool isRayOriginInsideBox = true; - - /* Test the slab_1 formed by the planes with normals boxAxis1 and -boxAxis1. */ - - Vector3 axis1 = boxHalfExtent1 * boxAxis1; - - QAp = P + axis1; - tp = QAp.Dot(boxAxis1); - - QAn = P - axis1; - tn = -QAn.Dot(boxAxis1); - - float n = rayDir.Dot(boxAxis1); - if (fabsf(n) < EPSILON) - { - // If the ray is parallel to the slab and the ray origin is outside, return no intersection. - if (tp < 0.0f || tn < 0.0f) - { - return false; - } - } - else - { - if (tp < 0.0f || tn < 0.0f) - { - isRayOriginInsideBox = false; - } - - float div = 1.0f / n; - float t1 = tp * div; - float t2 = tn * (-div); - if (t1 > t2) - { - AZStd::swap(t1, t2); - } - tmin = AZ::GetMax(tmin, t1); - tmax = AZ::GetMin(tmax, t2); - if (tmin > tmax) - { - return false; - } - } - - /* test the slab_2 formed by plane with normals boxAxis2 and -boxAxis2 */ - - Vector3 axis2 = boxHalfExtent2 * boxAxis2; - - QAp = P + axis2; - tp = QAp.Dot(boxAxis2); - - QAn = P - axis2; - tn = -QAn.Dot(boxAxis2); - - n = rayDir.Dot(boxAxis2); - if (fabsf(n) < EPSILON) - { - // If the ray is parallel to the slab and the ray origin is outside, return no intersection. - if (tp < 0.0f || tn < 0.0f) - { - return false; - } - } - else - { - if (tp < 0.0f || tn < 0.0f) - { - isRayOriginInsideBox = false; - } - - float div = 1.0f / n; - float t1 = tp * div; - float t2 = tn * (-div); - if (t1 > t2) - { - AZStd::swap(t1, t2); - } - tmin = AZ::GetMax(tmin, t1); - tmax = AZ::GetMin(tmax, t2); - if (tmin > tmax) - { - return false; - } - } - - /* test the slab_3 formed by plane with normals boxAxis3 and -boxAxis3 */ - - Vector3 axis3 = boxHalfExtent3 * boxAxis3; - - QAp = P + axis3; - tp = QAp.Dot(boxAxis3); - - QAn = P - axis3; - tn = -QAn.Dot(boxAxis3); - - n = rayDir.Dot(boxAxis3); - if (fabsf(n) < EPSILON) - { - // If the ray is parallel to the slab and the ray origin is outside, return no intersection. - if (tp < 0.0f || tn < 0.0f) - { - return false; - } - } - else - { - if (tp < 0.0f || tn < 0.0f) - { - isRayOriginInsideBox = false; - } - - float div = 1.0f / n; - float t1 = tp * div; - float t2 = tn * (-div); - if (t1 > t2) - { - AZStd::swap(t1, t2); - } - tmin = AZ::GetMax(tmin, t1); - tmax = AZ::GetMin(tmax, t2); - if (tmin > tmax) - { - return false; - } - } - - t = (isRayOriginInsideBox ? tmax : tmin); - return true; -} - -bool AZ::Intersect::IntersectRayObb(const Vector3& rayOrigin, const Vector3& rayDir, const Obb& obb, float& t) -{ - return AZ::Intersect::IntersectRayBox(rayOrigin, rayDir, obb.GetPosition(), - obb.GetAxisX(), obb.GetAxisY(), obb.GetAxisZ(), - obb.GetHalfLengthX(), obb.GetHalfLengthY(), obb.GetHalfLengthZ(), t); -} - -//========================================================================= -// IntersectSegmentCylinder -// [10/21/2009] -//========================================================================= -CylinderIsectTypes -AZ::Intersect::IntersectSegmentCylinder( - const Vector3& sa, const Vector3& dir, const Vector3& p, const Vector3& q, const float r, float& t) -{ - const float epsilon = 0.001f; - Vector3 d = q - p; // can be cached - Vector3 m = sa - p; // -"- - Vector3 n = /*sb - sa*/ dir; // -"- - - float md = m.Dot(d); - float nd = n.Dot(d); - float dd = d.Dot(d); - - // Test if segment fully outside either endcap of cylinder - if (md < 0.0f && md + nd < 0.0f) - { - return RR_ISECT_RAY_CYL_NONE; // Segment outside 'p' side of cylinder - } - if (md > dd && md + nd > dd) - { - return RR_ISECT_RAY_CYL_NONE; // Segment outside 'q' side of cylinder - } - float nn = n.Dot(n); - float mn = m.Dot(n); - float a = dd * nn - nd * nd; - float k = m.Dot(m) - r * r; - float c = dd * k - md * md; - if (std::fabs(a) < epsilon) - { - // Segment runs parallel to cylinder axis - if (c > 0.0f) - { - return RR_ISECT_RAY_CYL_NONE; // 'a' and thus the segment lie outside cylinder - } - // Now known that segment intersects cylinder; figure out how it intersects - if (md < 0.0f) - { - t = -mn / nn; // Intersect segment against 'p' endcap - return RR_ISECT_RAY_CYL_P_SIDE; - } - else if (md > dd) - { - t = (nd - mn) / nn; // Intersect segment against 'q' endcap - return RR_ISECT_RAY_CYL_Q_SIDE; + // If the ray is parallel to the slab and the ray origin is outside, return no intersection. + if (tp < 0.0f || tn < 0.0f) + { + return false; + } } else { - // 'a' lies inside cylinder - t = 0.0f; - return RR_ISECT_RAY_CYL_SA_INSIDE; - } - } - float b = dd * mn - nd * md; - float discr = b * b - a * c; - if (discr < 0.0f) - { - return RR_ISECT_RAY_CYL_NONE; // No real roots; no intersection - } - t = (-b - Sqrt(discr)) / a; - CylinderIsectTypes result = RR_ISECT_RAY_CYL_PQ; // default along the PQ segment + if (tp < 0.0f || tn < 0.0f) + { + isRayOriginInsideBox = false; + } - if (md + t * nd < 0.0f) - { - // Intersection outside cylinder on 'p' side - if (nd <= 0.0f) - { - return RR_ISECT_RAY_CYL_NONE; // Segment pointing away from endcap + float div = 1.0f / n; + float t1 = tp * div; + float t2 = tn * (-div); + if (t1 > t2) + { + AZStd::swap(t1, t2); + } + tmin = AZ::GetMax(tmin, t1); + tmax = AZ::GetMin(tmax, t2); + if (tmin > tmax) + { + return false; + } } - float t0 = -md / nd; - // Keep intersection if Dot(S(t) - p, S(t) - p) <= r^2 - if (k + t0 * (2.0f * mn + t0 * nn) <= 0.0f) + + /* test the slab_2 formed by plane with normals boxAxis2 and -boxAxis2 */ + + Vector3 axis2 = boxHalfExtent2 * boxAxis2; + + QAp = P + axis2; + tp = QAp.Dot(boxAxis2); + + QAn = P - axis2; + tn = -QAn.Dot(boxAxis2); + + n = rayDir.Dot(boxAxis2); + if (fabsf(n) < EPSILON) { - // if( t0 < 0.0f ) t0 = 0.0f; // it's inside the cylinder - t = t0; - result = RR_ISECT_RAY_CYL_P_SIDE; + // If the ray is parallel to the slab and the ray origin is outside, return no intersection. + if (tp < 0.0f || tn < 0.0f) + { + return false; + } } else { - return RR_ISECT_RAY_CYL_NONE; + if (tp < 0.0f || tn < 0.0f) + { + isRayOriginInsideBox = false; + } + + float div = 1.0f / n; + float t1 = tp * div; + float t2 = tn * (-div); + if (t1 > t2) + { + AZStd::swap(t1, t2); + } + tmin = AZ::GetMax(tmin, t1); + tmax = AZ::GetMin(tmax, t2); + if (tmin > tmax) + { + return false; + } } - } - else if (md + t * nd > dd) - { - // Intersection outside cylinder on 'q' side - if (nd >= 0.0f) + + /* test the slab_3 formed by plane with normals boxAxis3 and -boxAxis3 */ + + Vector3 axis3 = boxHalfExtent3 * boxAxis3; + + QAp = P + axis3; + tp = QAp.Dot(boxAxis3); + + QAn = P - axis3; + tn = -QAn.Dot(boxAxis3); + + n = rayDir.Dot(boxAxis3); + if (fabsf(n) < EPSILON) { - return RR_ISECT_RAY_CYL_NONE; // Segment pointing away from endcap - } - float t0 = (dd - md) / nd; - // Keep intersection if Dot(S(t) - q, S(t) - q) <= r^2 - if (k + dd - 2.0f * md + t0 * (2.0f * (mn - nd) + t0 * nn) <= 0.0f) - { - // if( t0 < 0.0f ) t0 = 0.0f; // it's inside the cylinder - t = t0; - result = RR_ISECT_RAY_CYL_Q_SIDE; + // If the ray is parallel to the slab and the ray origin is outside, return no intersection. + if (tp < 0.0f || tn < 0.0f) + { + return false; + } } else { - return RR_ISECT_RAY_CYL_NONE; + if (tp < 0.0f || tn < 0.0f) + { + isRayOriginInsideBox = false; + } + + float div = 1.0f / n; + float t1 = tp * div; + float t2 = tn * (-div); + if (t1 > t2) + { + AZStd::swap(t1, t2); + } + tmin = AZ::GetMax(tmin, t1); + tmax = AZ::GetMin(tmax, t2); + if (tmin > tmax) + { + return false; + } } + + t = (isRayOriginInsideBox ? tmax : tmin); + return true; } - // Segment intersects cylinder between the end-caps; t is correct - if (t > 1.0f) + bool Intersect::IntersectRayObb(const Vector3& rayOrigin, const Vector3& rayDir, const Obb& obb, float& t) { - return RR_ISECT_RAY_CYL_NONE; // Intersection lies outside segment + return Intersect::IntersectRayBox( + rayOrigin, rayDir, obb.GetPosition(), obb.GetAxisX(), obb.GetAxisY(), obb.GetAxisZ(), obb.GetHalfLengthX(), + obb.GetHalfLengthY(), obb.GetHalfLengthZ(), t); } - else if (t < 0.0f) + + //========================================================================= + // IntersectSegmentCylinder + // [10/21/2009] + //========================================================================= + Intersect::CylinderIsectTypes Intersect::IntersectSegmentCylinder( + const Vector3& sa, const Vector3& dir, const Vector3& p, const Vector3& q, const float r, float& t) { - if (c <= 0.0f) + const float epsilon = 0.001f; + Vector3 d = q - p; // can be cached + Vector3 m = sa - p; // -"- + Vector3 n = /*sb - sa*/ dir; // -"- + + float md = m.Dot(d); + float nd = n.Dot(d); + float dd = d.Dot(d); + + // Test if segment fully outside either endcap of cylinder + if (md < 0.0f && md + nd < 0.0f) { - t = 0.0f; - return RR_ISECT_RAY_CYL_SA_INSIDE; // Segment starts inside + return RR_ISECT_RAY_CYL_NONE; // Segment outside 'p' side of cylinder } - else + if (md > dd && md + nd > dd) + { + return RR_ISECT_RAY_CYL_NONE; // Segment outside 'q' side of cylinder + } + float nn = n.Dot(n); + float mn = m.Dot(n); + float a = dd * nn - nd * nd; + float k = m.Dot(m) - r * r; + float c = dd * k - md * md; + if (std::fabs(a) < epsilon) + { + // Segment runs parallel to cylinder axis + if (c > 0.0f) + { + return RR_ISECT_RAY_CYL_NONE; // 'a' and thus the segment lie outside cylinder + } + // Now known that segment intersects cylinder; figure out how it intersects + if (md < 0.0f) + { + t = -mn / nn; // Intersect segment against 'p' endcap + return RR_ISECT_RAY_CYL_P_SIDE; + } + else if (md > dd) + { + t = (nd - mn) / nn; // Intersect segment against 'q' endcap + return RR_ISECT_RAY_CYL_Q_SIDE; + } + else + { + // 'a' lies inside cylinder + t = 0.0f; + return RR_ISECT_RAY_CYL_SA_INSIDE; + } + } + float b = dd * mn - nd * md; + float discr = b * b - a * c; + if (discr < 0.0f) + { + return RR_ISECT_RAY_CYL_NONE; // No real roots; no intersection + } + t = (-b - Sqrt(discr)) / a; + CylinderIsectTypes result = RR_ISECT_RAY_CYL_PQ; // default along the PQ segment + + if (md + t * nd < 0.0f) + { + // Intersection outside cylinder on 'p' side + if (nd <= 0.0f) + { + return RR_ISECT_RAY_CYL_NONE; // Segment pointing away from endcap + } + float t0 = -md / nd; + // Keep intersection if Dot(S(t) - p, S(t) - p) <= r^2 + if (k + t0 * (2.0f * mn + t0 * nn) <= 0.0f) + { + // if( t0 < 0.0f ) t0 = 0.0f; // it's inside the cylinder + t = t0; + result = RR_ISECT_RAY_CYL_P_SIDE; + } + else + { + return RR_ISECT_RAY_CYL_NONE; + } + } + else if (md + t * nd > dd) + { + // Intersection outside cylinder on 'q' side + if (nd >= 0.0f) + { + return RR_ISECT_RAY_CYL_NONE; // Segment pointing away from endcap + } + float t0 = (dd - md) / nd; + // Keep intersection if Dot(S(t) - q, S(t) - q) <= r^2 + if (k + dd - 2.0f * md + t0 * (2.0f * (mn - nd) + t0 * nn) <= 0.0f) + { + // if( t0 < 0.0f ) t0 = 0.0f; // it's inside the cylinder + t = t0; + result = RR_ISECT_RAY_CYL_Q_SIDE; + } + else + { + return RR_ISECT_RAY_CYL_NONE; + } + } + + // Segment intersects cylinder between the end-caps; t is correct + if (t > 1.0f) { return RR_ISECT_RAY_CYL_NONE; // Intersection lies outside segment } - } - else - { - return result; - } -} -//========================================================================= -// IntersectSegmentCapsule -// [10/21/2009] -//========================================================================= -CapsuleIsectTypes -AZ::Intersect::IntersectSegmentCapsule(const Vector3& sa, const Vector3& dir, const Vector3& p, const Vector3& q, const float r, float& t) -{ - int result = IntersectSegmentCylinder(sa, dir, p, q, r, t); - - if (result == RR_ISECT_RAY_CYL_SA_INSIDE) - { - return ISECT_RAY_CAPSULE_SA_INSIDE; - } - - if (result == RR_ISECT_RAY_CYL_PQ) - { - return ISECT_RAY_CAPSULE_PQ; - } - - Vector3 dirNorm = dir; - float len = dirNorm.NormalizeWithLength(); - - // check spheres - float timeLenTop, timeLenBottom; - int resultTop = IntersectRaySphere(sa, dirNorm, p, r, timeLenTop); - if (resultTop == ISECT_RAY_SPHERE_SA_INSIDE) - { - return ISECT_RAY_CAPSULE_SA_INSIDE; - } - int resultBottom = IntersectRaySphere(sa, dirNorm, q, r, timeLenBottom); - if (resultBottom == ISECT_RAY_SPHERE_SA_INSIDE) - { - return ISECT_RAY_CAPSULE_SA_INSIDE; - } - - if (resultTop == ISECT_RAY_SPHERE_ISECT) - { - if (resultBottom == ISECT_RAY_SPHERE_ISECT) + else if (t < 0.0f) { - // if we intersect both spheres pick the closest one - if (timeLenTop < timeLenBottom) + if (c <= 0.0f) + { + t = 0.0f; + return RR_ISECT_RAY_CYL_SA_INSIDE; // Segment starts inside + } + else + { + return RR_ISECT_RAY_CYL_NONE; // Intersection lies outside segment + } + } + else + { + return result; + } + } + //========================================================================= + // IntersectSegmentCapsule + // [10/21/2009] + //========================================================================= + Intersect::CapsuleIsectTypes Intersect::IntersectSegmentCapsule( + const Vector3& sa, const Vector3& dir, const Vector3& p, const Vector3& q, const float r, float& t) + { + int result = IntersectSegmentCylinder(sa, dir, p, q, r, t); + + if (result == RR_ISECT_RAY_CYL_SA_INSIDE) + { + return ISECT_RAY_CAPSULE_SA_INSIDE; + } + + if (result == RR_ISECT_RAY_CYL_PQ) + { + return ISECT_RAY_CAPSULE_PQ; + } + + Vector3 dirNorm = dir; + float len = dirNorm.NormalizeWithLength(); + + // check spheres + float timeLenTop, timeLenBottom; + int resultTop = IntersectRaySphere(sa, dirNorm, p, r, timeLenTop); + if (resultTop == ISECT_RAY_SPHERE_SA_INSIDE) + { + return ISECT_RAY_CAPSULE_SA_INSIDE; + } + int resultBottom = IntersectRaySphere(sa, dirNorm, q, r, timeLenBottom); + if (resultBottom == ISECT_RAY_SPHERE_SA_INSIDE) + { + return ISECT_RAY_CAPSULE_SA_INSIDE; + } + + if (resultTop == ISECT_RAY_SPHERE_ISECT) + { + if (resultBottom == ISECT_RAY_SPHERE_ISECT) + { + // if we intersect both spheres pick the closest one + if (timeLenTop < timeLenBottom) + { + t = timeLenTop / len; + return ISECT_RAY_CAPSULE_P_SIDE; + } + else + { + t = timeLenBottom / len; + return ISECT_RAY_CAPSULE_Q_SIDE; + } + } + else { t = timeLenTop / len; return ISECT_RAY_CAPSULE_P_SIDE; } - else - { - t = timeLenBottom / len; - return ISECT_RAY_CAPSULE_Q_SIDE; - } } - else + + if (resultBottom == ISECT_RAY_SPHERE_ISECT) { - t = timeLenTop / len; - return ISECT_RAY_CAPSULE_P_SIDE; + t = timeLenBottom / len; + return ISECT_RAY_CAPSULE_Q_SIDE; } + + return ISECT_RAY_CAPSULE_NONE; } - if (resultBottom == ISECT_RAY_SPHERE_ISECT) + //========================================================================= + // IntersectSegmentPolyhedron + // [10/21/2009] + //========================================================================= + bool Intersect::IntersectSegmentPolyhedron( + const Vector3& sa, + const Vector3& dir, + const Plane p[], + int numPlanes, + float& tfirst, + float& tlast, + int& iFirstPlane, + int& iLastPlane) { - t = timeLenBottom / len; - return ISECT_RAY_CAPSULE_Q_SIDE; - } - - return ISECT_RAY_CAPSULE_NONE; -} - -//========================================================================= -// IntersectSegmentPolyhedron -// [10/21/2009] -//========================================================================= -bool -AZ::Intersect::IntersectSegmentPolyhedron( - const Vector3& sa, const Vector3& dir, const Plane p[], int numPlanes, - float& tfirst, float& tlast, int& iFirstPlane, int& iLastPlane) -{ - // Compute direction vector for the segment - Vector3 d = /*b - a*/ dir; - // Set initial interval to being the whole segment. For a ray, tlast should be - // set to +RR_FLT_MAX. For a line, additionally tfirst should be set to -RR_FLT_MAX - tfirst = 0.0f; - tlast = 1.0f; - iFirstPlane = -1; - iLastPlane = -1; - // Intersect segment against each plane - for (int i = 0; i < numPlanes; i++) - { - const Vector4& plane = p[i].GetPlaneEquationCoefficients(); - - float denom = plane.Dot3(d); - // don't forget we store -D in the plane - float dist = (-plane.GetW()) - plane.Dot3(sa); - // Test if segment runs parallel to the plane - if (denom == 0.0f) + // Compute direction vector for the segment + Vector3 d = /*b - a*/ dir; + // Set initial interval to being the whole segment. For a ray, tlast should be + // set to +RR_FLT_MAX. For a line, additionally tfirst should be set to -RR_FLT_MAX + tfirst = 0.0f; + tlast = 1.0f; + iFirstPlane = -1; + iLastPlane = -1; + // Intersect segment against each plane + for (int i = 0; i < numPlanes; i++) { - // If so, return "no intersection" if segment lies outside plane - if (dist < 0.0f) + const Vector4& plane = p[i].GetPlaneEquationCoefficients(); + + float denom = plane.Dot3(d); + // don't forget we store -D in the plane + float dist = (-plane.GetW()) - plane.Dot3(sa); + // Test if segment runs parallel to the plane + if (denom == 0.0f) { - return false; - } - } - else - { - // Compute parameterized t value for intersection with current plane - float t = dist / denom; - if (denom < 0.0f) - { - // When entering half space, update tfirst if t is larger - if (t > tfirst) + // If so, return "no intersection" if segment lies outside plane + if (dist < 0.0f) { - tfirst = t; - iFirstPlane = i; + return false; } } else { - // When exiting half space, update tlast if t is smaller - if (t < tlast) + // Compute parameterized t value for intersection with current plane + float t = dist / denom; + if (denom < 0.0f) { - tlast = t; - iLastPlane = i; + // When entering half space, update tfirst if t is larger + if (t > tfirst) + { + tfirst = t; + iFirstPlane = i; + } + } + else + { + // When exiting half space, update tlast if t is smaller + if (t < tlast) + { + tlast = t; + iLastPlane = i; + } + } + + // Exit with "no intersection" if intersection becomes empty + if (tfirst > tlast) + { + return false; } } - - // Exit with "no intersection" if intersection becomes empty - if (tfirst > tlast) - { - return false; - } } - } - //DBG_Assert(iFirstPlane!=-1&&iLastPlane!=-1,("We have some bad border case to have only one plane, fix this function!")); - if (iFirstPlane == -1 && iLastPlane == -1) - { - return false; - } - - // A nonzero logical intersection, so the segment intersects the polyhedron - return true; -} - -//========================================================================= -// ClosestSegmentSegment -// [10/21/2009] -//========================================================================= -void -AZ::Intersect::ClosestSegmentSegment( - const Vector3& segment1Start, const Vector3& segment1End, - const Vector3& segment2Start, const Vector3& segment2End, - float& segment1Proportion, float& segment2Proportion, - Vector3& closestPointSegment1, Vector3& closestPointSegment2, - float epsilon) -{ - const Vector3 segment1 = segment1End - segment1Start; - const Vector3 segment2 = segment2End - segment2Start; - const Vector3 segmentStartsVector = segment1Start - segment2Start; - const float segment1LengthSquared = segment1.Dot(segment1); - const float segment2LengthSquared = segment2.Dot(segment2); - - // Check if both segments degenerate into points - if (segment1LengthSquared <= epsilon && segment2LengthSquared <= epsilon) - { - segment1Proportion = 0.0f; - segment2Proportion = 0.0f; - closestPointSegment1 = segment1Start; - closestPointSegment2 = segment2Start; - return; - } - - float projSegment2SegmentStarts = segment2.Dot(segmentStartsVector); - - // Check if segment 1 degenerates into a point - if (segment1LengthSquared <= epsilon) - { - segment1Proportion = 0.0f; - segment2Proportion = AZ::GetClamp(projSegment2SegmentStarts / segment2LengthSquared, 0.0f, 1.0f); - } - else - { - float projSegment1SegmentStarts = segment1.Dot(segmentStartsVector); - // Check if segment 2 degenerates into a point - if (segment2LengthSquared <= epsilon) + // DBG_Assert(iFirstPlane!=-1&&iLastPlane!=-1,("We have some bad border case to have only one plane, fix this function!")); + if (iFirstPlane == -1 && iLastPlane == -1) { - segment1Proportion = AZ::GetClamp(-projSegment1SegmentStarts / segment1LengthSquared, 0.0f, 1.0f); + return false; + } + + // A nonzero logical intersection, so the segment intersects the polyhedron + return true; + } + + //========================================================================= + // ClosestSegmentSegment + // [10/21/2009] + //========================================================================= + void Intersect::ClosestSegmentSegment( + const Vector3& segment1Start, + const Vector3& segment1End, + const Vector3& segment2Start, + const Vector3& segment2End, + float& segment1Proportion, + float& segment2Proportion, + Vector3& closestPointSegment1, + Vector3& closestPointSegment2, + float epsilon) + { + const Vector3 segment1 = segment1End - segment1Start; + const Vector3 segment2 = segment2End - segment2Start; + const Vector3 segmentStartsVector = segment1Start - segment2Start; + const float segment1LengthSquared = segment1.Dot(segment1); + const float segment2LengthSquared = segment2.Dot(segment2); + + // Check if both segments degenerate into points + if (segment1LengthSquared <= epsilon && segment2LengthSquared <= epsilon) + { + segment1Proportion = 0.0f; segment2Proportion = 0.0f; + closestPointSegment1 = segment1Start; + closestPointSegment2 = segment2Start; + return; + } + + float projSegment2SegmentStarts = segment2.Dot(segmentStartsVector); + + // Check if segment 1 degenerates into a point + if (segment1LengthSquared <= epsilon) + { + segment1Proportion = 0.0f; + segment2Proportion = AZ::GetClamp(projSegment2SegmentStarts / segment2LengthSquared, 0.0f, 1.0f); } else { - // The general non-degenerate case starts here - float projSegment1Segment2 = segment1.Dot(segment2); - float denom = segment1LengthSquared * segment2LengthSquared - projSegment1Segment2 * projSegment1Segment2; // Always nonnegative - - // If segments not parallel, compute closest point on segment1 to segment2, and - // clamp to segment1. Else pick arbitrary segment1Proportion (here 0) - if (denom != 0.0f) + float projSegment1SegmentStarts = segment1.Dot(segmentStartsVector); + // Check if segment 2 degenerates into a point + if (segment2LengthSquared <= epsilon) { - segment1Proportion = AZ::GetClamp((projSegment1Segment2 * projSegment2SegmentStarts - projSegment1SegmentStarts * segment2LengthSquared) / denom, 0.0f, 1.0f); + segment1Proportion = AZ::GetClamp(-projSegment1SegmentStarts / segment1LengthSquared, 0.0f, 1.0f); + segment2Proportion = 0.0f; } else { - segment1Proportion = 0.0f; - } + // The general non-degenerate case starts here + float projSegment1Segment2 = segment1.Dot(segment2); + float denom = + segment1LengthSquared * segment2LengthSquared - projSegment1Segment2 * projSegment1Segment2; // Always nonnegative - // Compute point on segment2 closest to segment1 using - segment2Proportion = (projSegment1Segment2 * segment1Proportion + projSegment2SegmentStarts) / segment2LengthSquared; + // If segments not parallel, compute closest point on segment1 to segment2, and + // clamp to segment1. Else pick arbitrary segment1Proportion (here 0) + if (denom != 0.0f) + { + segment1Proportion = AZ::GetClamp( + (projSegment1Segment2 * projSegment2SegmentStarts - projSegment1SegmentStarts * segment2LengthSquared) / denom, + 0.0f, 1.0f); + } + else + { + segment1Proportion = 0.0f; + } - // If segment2Proportion in [0,1] done. Else clamp segment2Proportion, recompute segment1Proportion for the new value of segment2Proportion - // and clamp segment1Proportion to [0, 1] - if (segment2Proportion < 0.0f) - { - segment2Proportion = 0.0f; - segment1Proportion = AZ::GetClamp(-projSegment1SegmentStarts / segment1LengthSquared, 0.0f, 1.0f); - } - else if (segment2Proportion > 1.0f) - { - segment2Proportion = 1.0f; - segment1Proportion = AZ::GetClamp((projSegment1Segment2 - projSegment1SegmentStarts) / segment1LengthSquared, 0.0f, 1.0f); + // Compute point on segment2 closest to segment1 using + segment2Proportion = (projSegment1Segment2 * segment1Proportion + projSegment2SegmentStarts) / segment2LengthSquared; + + // If segment2Proportion in [0,1] done. Else clamp segment2Proportion, recompute segment1Proportion for the new value of + // segment2Proportion and clamp segment1Proportion to [0, 1] + if (segment2Proportion < 0.0f) + { + segment2Proportion = 0.0f; + segment1Proportion = AZ::GetClamp(-projSegment1SegmentStarts / segment1LengthSquared, 0.0f, 1.0f); + } + else if (segment2Proportion > 1.0f) + { + segment2Proportion = 1.0f; + segment1Proportion = + AZ::GetClamp((projSegment1Segment2 - projSegment1SegmentStarts) / segment1LengthSquared, 0.0f, 1.0f); + } } } + + closestPointSegment1 = segment1Start + segment1 * segment1Proportion; + closestPointSegment2 = segment2Start + segment2 * segment2Proportion; } - closestPointSegment1 = segment1Start + segment1 * segment1Proportion; - closestPointSegment2 = segment2Start + segment2 * segment2Proportion; -} - -void AZ::Intersect::ClosestSegmentSegment( - const Vector3& segment1Start, const Vector3& segment1End, - const Vector3& segment2Start, const Vector3& segment2End, - Vector3& closestPointSegment1, Vector3& closestPointSegment2, - float epsilon) -{ - float proportion1, proportion2; - AZ::Intersect::ClosestSegmentSegment( - segment1Start, segment1End, - segment2Start, segment2End, - proportion1, proportion2, - closestPointSegment1, closestPointSegment2, epsilon); -} - -void AZ::Intersect::ClosestPointSegment( - const Vector3& point, const Vector3& segmentStart, const Vector3& segmentEnd, - float& proportion, Vector3& closestPointOnSegment) -{ - Vector3 segment = segmentEnd - segmentStart; - // Project point onto segment, but deferring divide by segment.Dot(segment) - proportion = (point - segmentStart).Dot(segment); - if (proportion <= 0.0f) + void Intersect::ClosestSegmentSegment( + const Vector3& segment1Start, + const Vector3& segment1End, + const Vector3& segment2Start, + const Vector3& segment2End, + Vector3& closestPointSegment1, + Vector3& closestPointSegment2, + float epsilon) { - // Point projects outside the [segmentStart, segmentEnd] interval, on the segmentStart side, clamp to segmentStart - proportion = 0.0f; - closestPointOnSegment = segmentStart; + float proportion1, proportion2; + Intersect::ClosestSegmentSegment( + segment1Start, segment1End, segment2Start, segment2End, proportion1, proportion2, closestPointSegment1, closestPointSegment2, + epsilon); } - else + + void Intersect::ClosestPointSegment( + const Vector3& point, const Vector3& segmentStart, const Vector3& segmentEnd, float& proportion, Vector3& closestPointOnSegment) { - float segmentLengthSquared = segment.Dot(segment); - if (proportion >= segmentLengthSquared) + Vector3 segment = segmentEnd - segmentStart; + // Project point onto segment, but deferring divide by segment.Dot(segment) + proportion = (point - segmentStart).Dot(segment); + if (proportion <= 0.0f) { - // Point projects outside the [segmentStart, segmentEnd] interval, on the segmentEnd side, clamp to segmentEnd - proportion = 1.0f; - closestPointOnSegment = segmentEnd; + // Point projects outside the [segmentStart, segmentEnd] interval, on the segmentStart side, clamp to segmentStart + proportion = 0.0f; + closestPointOnSegment = segmentStart; } else { - // Point projects inside the [segmentStart, segmentEnd] interval, must do deferred divide now - proportion = proportion / segmentLengthSquared; - closestPointOnSegment = segmentStart + (proportion * segment); + float segmentLengthSquared = segment.Dot(segment); + if (proportion >= segmentLengthSquared) + { + // Point projects outside the [segmentStart, segmentEnd] interval, on the segmentEnd side, clamp to segmentEnd + proportion = 1.0f; + closestPointOnSegment = segmentEnd; + } + else + { + // Point projects inside the [segmentStart, segmentEnd] interval, must do deferred divide now + proportion = proportion / segmentLengthSquared; + closestPointOnSegment = segmentStart + (proportion * segment); + } } } -} #if 0 ////////////////////////////////////////////////////////////////////////// @@ -2121,3 +2171,5 @@ namespace test } ////////////////////////////////////////////////////////////////////////// #endif + +} // namespace AZ diff --git a/Code/Framework/AzCore/AzCore/Math/Sfmt.cpp b/Code/Framework/AzCore/AzCore/Math/Sfmt.cpp index 5f4dc9e5df..96c1a6a420 100644 --- a/Code/Framework/AzCore/AzCore/Math/Sfmt.cpp +++ b/Code/Framework/AzCore/AzCore/Math/Sfmt.cpp @@ -9,39 +9,38 @@ #include #include -#include #include +#include #include // for memset namespace AZ::SfmtInternal { - static const int N32 = N * 4; - static const int N64 = N * 2; - static const int POS1 = 122; - static const int SL1 = 18; - static const int SR1 = 11; - static const int SL2 = 1; - static const int SR2 = 1; - static const unsigned int MSK1 = 0xdfffffefU; - static const unsigned int MSK2 = 0xddfecb7fU; - static const unsigned int MSK3 = 0xbffaffffU; - static const unsigned int MSK4 = 0xbffffff6U; - static const unsigned int PARITY1 = 0x00000001U; - static const unsigned int PARITY2 = 0x00000000U; - static const unsigned int PARITY3 = 0x00000000U; - static const unsigned int PARITY4 = 0x13c9e684U; + static const int N32 = N * 4; + static const int N64 = N * 2; + static const int POS1 = 122; + static const int SL1 = 18; + static const int SR1 = 11; + static const int SL2 = 1; + static const int SR2 = 1; + static const unsigned int MSK1 = 0xdfffffefU; + static const unsigned int MSK2 = 0xddfecb7fU; + static const unsigned int MSK3 = 0xbffaffffU; + static const unsigned int MSK4 = 0xbffffff6U; + static const unsigned int PARITY1 = 0x00000001U; + static const unsigned int PARITY2 = 0x00000000U; + static const unsigned int PARITY3 = 0x00000000U; + static const unsigned int PARITY4 = 0x13c9e684U; /** a parity check vector which certificate the period of 2^{MEXP} */ - static unsigned int parity[4] = {PARITY1, PARITY2, PARITY3, PARITY4}; + static unsigned int parity[4] = { PARITY1, PARITY2, PARITY3, PARITY4 }; #ifdef ONLY64 -# define idxof(_i) (_i ^ 1) +#define idxof(_i) (_i ^ 1) #else -# define idxof(_i) _i +#define idxof(_i) _i #endif // ONLY64 - #if AZ_TRAIT_USE_PLATFORM_SIMD_SSE /** * This function represents the recursion formula. @@ -52,7 +51,8 @@ namespace AZ::SfmtInternal * @param mask 128-bit mask * @return output */ - AZ_FORCE_INLINE static Simd::Vec4::Int32Type simd_recursion(Simd::Vec4::Int32Type* a, Simd::Vec4::Int32Type* b, Simd::Vec4::Int32Type c, Simd::Vec4::Int32Type d, Simd::Vec4::Int32Type mask) + AZ_FORCE_INLINE static Simd::Vec4::Int32Type simd_recursion( + Simd::Vec4::Int32Type* a, Simd::Vec4::Int32Type* b, Simd::Vec4::Int32Type c, Simd::Vec4::Int32Type d, Simd::Vec4::Int32Type mask) { Simd::Vec4::Int32Type v, x, y, z; x = *a; @@ -151,7 +151,7 @@ namespace AZ::SfmtInternal inline void rshift128(w128_t* out, w128_t const* in, int shift) { AZ::u64 th, tl, oh, ol; - #ifdef ONLY64 +#ifdef ONLY64 th = ((AZ::u64)in->u[2] << 32) | ((AZ::u64)in->u[3]); tl = ((AZ::u64)in->u[0] << 32) | ((AZ::u64)in->u[1]); @@ -204,7 +204,7 @@ namespace AZ::SfmtInternal #endif } - inline void do_recursion(w128_t* r, w128_t* a, w128_t* b, w128_t* c, w128_t* d) + inline void do_recursion(w128_t* r, w128_t* a, w128_t* b, w128_t* c, w128_t* d) { w128_t x; w128_t y; @@ -229,7 +229,7 @@ namespace AZ::SfmtInternal inline void gen_rand_all(Sfmt& g) { int i; - w128_t* r1, * r2; + w128_t *r1, *r2; r1 = &g.m_sfmt[N - 2]; r2 = &g.m_sfmt[N - 1]; @@ -257,7 +257,7 @@ namespace AZ::SfmtInternal inline void gen_rand_array(Sfmt& g, w128_t* array, int size) { int i, j; - w128_t* r1, * r2; + w128_t *r1, *r2; r1 = &g.m_sfmt[N - 2]; r2 = &g.m_sfmt[N - 1]; @@ -295,82 +295,80 @@ namespace AZ::SfmtInternal #endif } // namespace AZ::SfmtInternal -using namespace AZ; - ////////////////////////////////////////////////////////////////////////// // Statics ////////////////////////////////////////////////////////////////////////// - -static EnvironmentVariable s_sfmt; -static const char* s_globalSfmtName = "GlobalSfmt"; - -Sfmt& Sfmt::GetInstance() +namespace AZ { - if (!s_sfmt) + static EnvironmentVariable s_sfmt; + static const char* s_globalSfmtName = "GlobalSfmt"; + + Sfmt& Sfmt::GetInstance() { - s_sfmt = AZ::Environment::FindVariable(s_globalSfmtName); if (!s_sfmt) { - Sfmt::Create(); + s_sfmt = AZ::Environment::FindVariable(s_globalSfmtName); + if (!s_sfmt) + { + Sfmt::Create(); + } + } + + return s_sfmt.Get(); + } + + void Sfmt::Create() + { + if (!s_sfmt) + { + s_sfmt = AZ::Environment::CreateVariable(s_globalSfmtName); } } - return s_sfmt.Get(); -} - -void Sfmt::Create() -{ - if (!s_sfmt) + void Sfmt::Destroy() { - s_sfmt = AZ::Environment::CreateVariable(s_globalSfmtName); + s_sfmt.Reset(); } -} -void Sfmt::Destroy() -{ - s_sfmt.Reset(); -} + //========================================================================= + // Sfmt + // [4/10/2012] + //========================================================================= + Sfmt::Sfmt() + { + m_psfmt32 = &m_sfmt[0].u[0]; + m_psfmt64 = reinterpret_cast(m_psfmt32); -//========================================================================= -// Sfmt -// [4/10/2012] -//========================================================================= -Sfmt::Sfmt() -{ - m_psfmt32 = &m_sfmt[0].u[0]; - m_psfmt64 = reinterpret_cast(m_psfmt32); + Seed(); + } - Seed(); -} + //========================================================================= + // Seed + // [4/10/2012] + //========================================================================= + Sfmt::Sfmt(AZ::u32* keys, int numKeys) + { + m_psfmt32 = &m_sfmt[0].u[0]; + m_psfmt64 = reinterpret_cast(m_psfmt32); -//========================================================================= -// Seed -// [4/10/2012] -//========================================================================= -Sfmt::Sfmt(AZ::u32* keys, int numKeys) -{ - m_psfmt32 = &m_sfmt[0].u[0]; - m_psfmt64 = reinterpret_cast(m_psfmt32); + Seed(keys, numKeys); + } - Seed(keys, numKeys); -} - -//========================================================================= -// Seed -// [4/10/2012] -//========================================================================= -void -Sfmt::Seed() -{ - // buffer with random values - AZ::u32 buffer[32]; - BetterPseudoRandom rnd; - bool result = rnd.GetRandom(buffer, sizeof(buffer)); - (void)result; - AZ_Warning("System", result, "Failed to seed properly the Smft generator!"); - Seed(buffer, AZ_ARRAY_SIZE(buffer)); -} + //========================================================================= + // Seed + // [4/10/2012] + //========================================================================= + void Sfmt::Seed() + { + // buffer with random values + AZ::u32 buffer[32]; + BetterPseudoRandom rnd; + bool result = rnd.GetRandom(buffer, sizeof(buffer)); + (void)result; + AZ_Warning("System", result, "Failed to seed properly the Smft generator!"); + Seed(buffer, AZ_ARRAY_SIZE(buffer)); + } /** * This function represents a function used in the initialization @@ -388,226 +386,222 @@ Sfmt::Seed() */ #define azsfmt_func2(x) ((x ^ (x >> 27)) * (AZ::u32)1566083941UL) -//========================================================================= -// Seed -// [4/10/2012] -//========================================================================= -void -Sfmt::Seed(AZ::u32* keys, int numKeys) -{ - using SfmtInternal::N; - using SfmtInternal::N32; - int i, j, count; - AZ::u32 r; - int lag; - int mid; - int size = N * 4; + //========================================================================= + // Seed + // [4/10/2012] + //========================================================================= + void Sfmt::Seed(AZ::u32* keys, int numKeys) + { + using SfmtInternal::N; + using SfmtInternal::N32; + int i, j, count; + AZ::u32 r; + int lag; + int mid; + int size = N * 4; - if (size >= 623) - { - lag = 11; - } - else if (size >= 68) - { - lag = 7; - } - else if (size >= 39) - { - lag = 5; - } - else - { - lag = 3; - } - mid = (size - lag) / 2; + if (size >= 623) + { + lag = 11; + } + else if (size >= 68) + { + lag = 7; + } + else if (size >= 39) + { + lag = 5; + } + else + { + lag = 3; + } + mid = (size - lag) / 2; - memset(m_sfmt, 0x8b, sizeof(m_sfmt)); - if (numKeys + 1 > SfmtInternal::N32) - { - count = numKeys + 1; - } - else - { - count = N32; - } - r = azsfmt_func1((m_psfmt32[idxof(0)] ^ m_psfmt32[idxof(mid)] ^ m_psfmt32[idxof(N32 - 1)])); - m_psfmt32[idxof(mid)] += r; - r += numKeys; - m_psfmt32[idxof(mid + lag)] += r; - m_psfmt32[idxof(0)] = r; + memset(m_sfmt, 0x8b, sizeof(m_sfmt)); + if (numKeys + 1 > SfmtInternal::N32) + { + count = numKeys + 1; + } + else + { + count = N32; + } + r = azsfmt_func1((m_psfmt32[idxof(0)] ^ m_psfmt32[idxof(mid)] ^ m_psfmt32[idxof(N32 - 1)])); + m_psfmt32[idxof(mid)] += r; + r += numKeys; + m_psfmt32[idxof(mid + lag)] += r; + m_psfmt32[idxof(0)] = r; - count--; - for (i = 1, j = 0; (j < count) && (j < numKeys); j++) - { - r = azsfmt_func1((m_psfmt32[idxof(i)] ^ m_psfmt32[idxof((i + mid) % N32)] ^ m_psfmt32[idxof((i + N32 - 1) % N32)])); - m_psfmt32[idxof((i + mid) % N32)] += r; - r += keys[j] + i; - m_psfmt32[idxof((i + mid + lag) % N32)] += r; - m_psfmt32[idxof(i)] = r; - i = (i + 1) % N32; - } - for (; j < count; j++) - { - r = azsfmt_func1((m_psfmt32[idxof(i)] ^ m_psfmt32[idxof((i + mid) % N32)] ^ m_psfmt32[idxof((i + N32 - 1) % N32)])); - m_psfmt32[idxof((i + mid) % N32)] += r; - r += i; - m_psfmt32[idxof((i + mid + lag) % N32)] += r; - m_psfmt32[idxof(i)] = r; - i = (i + 1) % N32; - } - for (j = 0; j < N32; j++) - { - r = azsfmt_func2((m_psfmt32[idxof(i)] + m_psfmt32[idxof((i + mid) % N32)] + m_psfmt32[idxof((i + N32 - 1) % N32)])); - m_psfmt32[idxof((i + mid) % N32)] ^= r; - r -= i; - m_psfmt32[idxof((i + mid + lag) % N32)] ^= r; - m_psfmt32[idxof(i)] = r; - i = (i + 1) % N32; - } + count--; + for (i = 1, j = 0; (j < count) && (j < numKeys); j++) + { + r = azsfmt_func1((m_psfmt32[idxof(i)] ^ m_psfmt32[idxof((i + mid) % N32)] ^ m_psfmt32[idxof((i + N32 - 1) % N32)])); + m_psfmt32[idxof((i + mid) % N32)] += r; + r += keys[j] + i; + m_psfmt32[idxof((i + mid + lag) % N32)] += r; + m_psfmt32[idxof(i)] = r; + i = (i + 1) % N32; + } + for (; j < count; j++) + { + r = azsfmt_func1((m_psfmt32[idxof(i)] ^ m_psfmt32[idxof((i + mid) % N32)] ^ m_psfmt32[idxof((i + N32 - 1) % N32)])); + m_psfmt32[idxof((i + mid) % N32)] += r; + r += i; + m_psfmt32[idxof((i + mid + lag) % N32)] += r; + m_psfmt32[idxof(i)] = r; + i = (i + 1) % N32; + } + for (j = 0; j < N32; j++) + { + r = azsfmt_func2((m_psfmt32[idxof(i)] + m_psfmt32[idxof((i + mid) % N32)] + m_psfmt32[idxof((i + N32 - 1) % N32)])); + m_psfmt32[idxof((i + mid) % N32)] ^= r; + r -= i; + m_psfmt32[idxof((i + mid + lag) % N32)] ^= r; + m_psfmt32[idxof(i)] = r; + i = (i + 1) % N32; + } - m_index = N32; - PeriodCertification(); -} + m_index = N32; + PeriodCertification(); + } #undef azsfmt_func1 #undef azsfmt_func2 -//========================================================================= -// PeriodCertification -// [4/10/2012] -//========================================================================= -void -Sfmt::PeriodCertification() -{ - int inner = 0; - int i, j; - AZ::u32 work; + //========================================================================= + // PeriodCertification + // [4/10/2012] + //========================================================================= + void Sfmt::PeriodCertification() + { + int inner = 0; + int i, j; + AZ::u32 work; - for (i = 0; i < 4; i++) - { - inner ^= m_psfmt32[idxof(i)] & SfmtInternal::parity[i]; - } - for (i = 16; i > 0; i >>= 1) - { - inner ^= inner >> i; - } - inner &= 1; - /* check OK */ - if (inner == 1) - { - return; - } - /* check NG, and modification */ - for (i = 0; i < 4; i++) - { - work = 1; - for (j = 0; j < 32; j++) + for (i = 0; i < 4; i++) { - if ((work & SfmtInternal::parity[i]) != 0) + inner ^= m_psfmt32[idxof(i)] & SfmtInternal::parity[i]; + } + for (i = 16; i > 0; i >>= 1) + { + inner ^= inner >> i; + } + inner &= 1; + /* check OK */ + if (inner == 1) + { + return; + } + /* check NG, and modification */ + for (i = 0; i < 4; i++) + { + work = 1; + for (j = 0; j < 32; j++) { - m_psfmt32[idxof(i)] ^= work; - return; + if ((work & SfmtInternal::parity[i]) != 0) + { + m_psfmt32[idxof(i)] ^= work; + return; + } + work = work << 1; } - work = work << 1; } } -} -//========================================================================= -// Rand32 -// [4/10/2012] -//========================================================================= -AZ::u32 Sfmt::Rand32() -{ - int index = m_index.fetch_add(1); - if (index >= SfmtInternal::N32) + //========================================================================= + // Rand32 + // [4/10/2012] + //========================================================================= + AZ::u32 Sfmt::Rand32() { - AZStd::lock_guard lock(m_generationMutex); - // if this thread is the one that sets m_index to 0, then this thread - // does the generation - index += 1; // compare against the result of fetch_add(1) above - if (m_index.compare_exchange_strong(index, 0)) + int index = m_index.fetch_add(1); + if (index >= SfmtInternal::N32) { - SfmtInternal::gen_rand_all(*this); + AZStd::lock_guard lock(m_generationMutex); + // if this thread is the one that sets m_index to 0, then this thread + // does the generation + index += 1; // compare against the result of fetch_add(1) above + if (m_index.compare_exchange_strong(index, 0)) + { + SfmtInternal::gen_rand_all(*this); + } + // try again, with the new table + return Rand32(); } - // try again, with the new table - return Rand32(); + return m_psfmt32[index]; } - return m_psfmt32[index]; -} -//========================================================================= -// Rand64 -// [4/10/2012] -//========================================================================= -AZ::u64 Sfmt::Rand64() -{ - int index = m_index.fetch_add(2); - if (index >= (SfmtInternal::N32 - 1)) + //========================================================================= + // Rand64 + // [4/10/2012] + //========================================================================= + AZ::u64 Sfmt::Rand64() { - AZStd::lock_guard lock(m_generationMutex); - // if this thread is the one that sets m_index to 0, then this thread - // does the generation - index += 2; // compare against the result of fetch_add(2) above - if (m_index.compare_exchange_strong(index, 0)) + int index = m_index.fetch_add(2); + if (index >= (SfmtInternal::N32 - 1)) { - SfmtInternal::gen_rand_all(*this); + AZStd::lock_guard lock(m_generationMutex); + // if this thread is the one that sets m_index to 0, then this thread + // does the generation + index += 2; // compare against the result of fetch_add(2) above + if (m_index.compare_exchange_strong(index, 0)) + { + SfmtInternal::gen_rand_all(*this); + } + // try again, with the new table + return Rand64(); } - // try again, with the new table - return Rand64(); + + AZ::u64 r; + r = m_psfmt64[index / 2]; + return r; } - AZ::u64 r; - r = m_psfmt64[index / 2]; - return r; -} + //========================================================================= + // FillArray32 + // [4/10/2012] + //========================================================================= + void Sfmt::FillArray32(AZ::u32* array, int size) + { + AZ_MATH_ASSERT(m_index == SfmtInternal::N32, "Invalid m_index! Reinitialize!"); + AZ_MATH_ASSERT(size % 4 == 0, "Size must be multiple of 4!"); + AZ_MATH_ASSERT(size >= SfmtInternal::N32, "Size must be bigger than %d GetMinArray32Size()!", SfmtInternal::N32); -//========================================================================= -// FillArray32 -// [4/10/2012] -//========================================================================= -void -Sfmt::FillArray32(AZ::u32* array, int size) -{ - AZ_MATH_ASSERT(m_index == SfmtInternal::N32, "Invalid m_index! Reinitialize!"); - AZ_MATH_ASSERT(size % 4 == 0, "Size must be multiple of 4!"); - AZ_MATH_ASSERT(size >= SfmtInternal::N32, "Size must be bigger than %d GetMinArray32Size()!", SfmtInternal::N32); + SfmtInternal::gen_rand_array(*this, (SfmtInternal::w128_t*)array, size / 4); + m_index = SfmtInternal::N32; + } - SfmtInternal::gen_rand_array(*this, (SfmtInternal::w128_t*)array, size / 4); - m_index = SfmtInternal::N32; -} + //========================================================================= + // FillArray64 + // [4/10/2012] + //========================================================================= + void Sfmt::FillArray64(AZ::u64* array, int size) + { + AZ_MATH_ASSERT(m_index == SfmtInternal::N32, "Invalid m_index! Reinitialize!"); + AZ_MATH_ASSERT(size % 4 == 0, "Size must be multiple of 4!"); + AZ_MATH_ASSERT(size >= SfmtInternal::N64, "Size must be bigger than %d GetMinArray64Size()!", SfmtInternal::N64); -//========================================================================= -// FillArray64 -// [4/10/2012] -//========================================================================= -void -Sfmt::FillArray64(AZ::u64* array, int size) -{ - AZ_MATH_ASSERT(m_index == SfmtInternal::N32, "Invalid m_index! Reinitialize!"); - AZ_MATH_ASSERT(size % 4 == 0, "Size must be multiple of 4!"); - AZ_MATH_ASSERT(size >= SfmtInternal::N64, "Size must be bigger than %d GetMinArray64Size()!", SfmtInternal::N64); + SfmtInternal::gen_rand_array(*this, (SfmtInternal::w128_t*)array, size / 2); + m_index = SfmtInternal::N32; + } - SfmtInternal::gen_rand_array(*this, (SfmtInternal::w128_t*)array, size / 2); - m_index = SfmtInternal::N32; -} + //========================================================================= + // GetMinArray32Size + // [4/10/2012] + //========================================================================= + int Sfmt::GetMinArray32Size() const + { + return SfmtInternal::N32; + } -//========================================================================= -// GetMinArray32Size -// [4/10/2012] -//========================================================================= -int -Sfmt::GetMinArray32Size() const -{ - return SfmtInternal::N32; -} + //========================================================================= + // GetMinArray64Size + // [4/10/2012] + //========================================================================= + int Sfmt::GetMinArray64Size() const + { + return SfmtInternal::N64; + } -//========================================================================= -// GetMinArray64Size -// [4/10/2012] -//========================================================================= -int -Sfmt::GetMinArray64Size() const -{ - return SfmtInternal::N64; -} +} // namespace AZ