From f56bb35301836e56582a575a75864392a0177875 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B8rgen=20P=2E=20Tjern=C3=B8?= Date: Mon, 2 Dec 2013 19:31:46 -0800 Subject: Fix line endings. WHAMMY. --- mp/src/public/collisionutils.cpp | 6524 +++++++++++++++++++------------------- 1 file changed, 3262 insertions(+), 3262 deletions(-) (limited to 'mp/src/public/collisionutils.cpp') diff --git a/mp/src/public/collisionutils.cpp b/mp/src/public/collisionutils.cpp index 99a40508..d26db4ab 100644 --- a/mp/src/public/collisionutils.cpp +++ b/mp/src/public/collisionutils.cpp @@ -1,3262 +1,3262 @@ -//========= Copyright Valve Corporation, All rights reserved. ============// -// -// Purpose: Common collision utility methods -// -// $Header: $ -// $NoKeywords: $ -//=============================================================================// - -#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB) - -#include "collisionutils.h" -#include "cmodel.h" -#include "mathlib/mathlib.h" -#include "mathlib/vector.h" -#include "tier0/dbg.h" -#include -#include "mathlib/vector4d.h" -#include "trace.h" - -// memdbgon must be the last include file in a .cpp file!!! -#include "tier0/memdbgon.h" - -#define UNINIT -99999.0 - -//----------------------------------------------------------------------------- -// Clears the trace -//----------------------------------------------------------------------------- -static void Collision_ClearTrace( const Vector &vecRayStart, const Vector &vecRayDelta, CBaseTrace *pTrace ) -{ - pTrace->startpos = vecRayStart; - pTrace->endpos = vecRayStart; - pTrace->endpos += vecRayDelta; - pTrace->startsolid = false; - pTrace->allsolid = false; - pTrace->fraction = 1.0f; - pTrace->contents = 0; -} - - -//----------------------------------------------------------------------------- -// Compute the offset in t along the ray that we'll use for the collision -//----------------------------------------------------------------------------- -static float ComputeBoxOffset( const Ray_t& ray ) -{ - if (ray.m_IsRay) - return 1e-3f; - - // Find the projection of the box diagonal along the ray... - float offset = FloatMakePositive(ray.m_Extents[0] * ray.m_Delta[0]) + - FloatMakePositive(ray.m_Extents[1] * ray.m_Delta[1]) + - FloatMakePositive(ray.m_Extents[2] * ray.m_Delta[2]); - - // We need to divide twice: Once to normalize the computation above - // so we get something in units of extents, and the second to normalize - // that with respect to the entire raycast. - offset *= InvRSquared( ray.m_Delta ); - - // 1e-3 is an epsilon - return offset + 1e-3; -} - - -//----------------------------------------------------------------------------- -// Intersects a swept box against a triangle -//----------------------------------------------------------------------------- -float IntersectRayWithTriangle( const Ray_t& ray, - const Vector& v1, const Vector& v2, const Vector& v3, bool oneSided ) -{ - // This is cute: Use barycentric coordinates to represent the triangle - // Vo(1-u-v) + V1u + V2v and intersect that with a line Po + Dt - // This gives us 3 equations + 3 unknowns, which we can solve with - // Cramer's rule... - // E1x u + E2x v - Dx t = Pox - Vox - // There's a couple of other optimizations, Cramer's rule involves - // computing the determinant of a matrix which has been constructed - // by three vectors. It turns out that - // det | A B C | = -( A x C ) dot B or -(C x B) dot A - // which we'll use below.. - - Vector edge1, edge2, org; - VectorSubtract( v2, v1, edge1 ); - VectorSubtract( v3, v1, edge2 ); - - // Cull out one-sided stuff - if (oneSided) - { - Vector normal; - CrossProduct( edge1, edge2, normal ); - if (DotProduct( normal, ray.m_Delta ) >= 0.0f) - return -1.0f; - } - - // FIXME: This is inaccurate, but fast for boxes - // We want to do a fast separating axis implementation here - // with a swept triangle along the reverse direction of the ray. - - // Compute some intermediary terms - Vector dirCrossEdge2, orgCrossEdge1; - CrossProduct( ray.m_Delta, edge2, dirCrossEdge2 ); - - // Compute the denominator of Cramer's rule: - // | -Dx E1x E2x | - // det | -Dy E1y E2y | = (D x E2) dot E1 - // | -Dz E1z E2z | - float denom = DotProduct( dirCrossEdge2, edge1 ); - if( FloatMakePositive( denom ) < 1e-6 ) - return -1.0f; - denom = 1.0f / denom; - - // Compute u. It's gotta lie in the range of 0 to 1. - // | -Dx orgx E2x | - // u = denom * det | -Dy orgy E2y | = (D x E2) dot org - // | -Dz orgz E2z | - VectorSubtract( ray.m_Start, v1, org ); - float u = DotProduct( dirCrossEdge2, org ) * denom; - if ((u < 0.0f) || (u > 1.0f)) - return -1.0f; - - // Compute t and v the same way... - // In barycentric coords, u + v < 1 - CrossProduct( org, edge1, orgCrossEdge1 ); - float v = DotProduct( orgCrossEdge1, ray.m_Delta ) * denom; - if ((v < 0.0f) || (v + u > 1.0f)) - return -1.0f; - - // Compute the distance along the ray direction that we need to fudge - // when using swept boxes - float boxt = ComputeBoxOffset( ray ); - float t = DotProduct( orgCrossEdge1, edge2 ) * denom; - if ((t < -boxt) || (t > 1.0f + boxt)) - return -1.0f; - - return clamp( t, 0.f, 1.f ); -} - -//----------------------------------------------------------------------------- -// computes the barycentric coordinates of an intersection -//----------------------------------------------------------------------------- - -bool ComputeIntersectionBarycentricCoordinates( const Ray_t& ray, - const Vector& v1, const Vector& v2, const Vector& v3, float& u, float& v, - float *t ) -{ - Vector edge1, edge2, org; - VectorSubtract( v2, v1, edge1 ); - VectorSubtract( v3, v1, edge2 ); - - // Compute some intermediary terms - Vector dirCrossEdge2, orgCrossEdge1; - CrossProduct( ray.m_Delta, edge2, dirCrossEdge2 ); - - // Compute the denominator of Cramer's rule: - // | -Dx E1x E2x | - // det | -Dy E1y E2y | = (D x E2) dot E1 - // | -Dz E1z E2z | - float denom = DotProduct( dirCrossEdge2, edge1 ); - if( FloatMakePositive( denom ) < 1e-6 ) - return false; - denom = 1.0f / denom; - - // Compute u. It's gotta lie in the range of 0 to 1. - // | -Dx orgx E2x | - // u = denom * det | -Dy orgy E2y | = (D x E2) dot org - // | -Dz orgz E2z | - VectorSubtract( ray.m_Start, v1, org ); - u = DotProduct( dirCrossEdge2, org ) * denom; - - // Compute t and v the same way... - // In barycentric coords, u + v < 1 - CrossProduct( org, edge1, orgCrossEdge1 ); - v = DotProduct( orgCrossEdge1, ray.m_Delta ) * denom; - - // Compute the distance along the ray direction that we need to fudge - // when using swept boxes - if( t ) - { - float boxt = ComputeBoxOffset( ray ); - *t = DotProduct( orgCrossEdge1, edge2 ) * denom; - if( ( *t < -boxt ) || ( *t > 1.0f + boxt ) ) - return false; - } - - return true; -} - -//----------------------------------------------------------------------------- -// Intersects a plane with a triangle (requires barycentric definition) -//----------------------------------------------------------------------------- - -int IntersectTriangleWithPlaneBarycentric( const Vector& org, const Vector& edgeU, - const Vector& edgeV, const Vector4D& plane, Vector2D* pIntersection ) -{ - // This uses a barycentric method, since we need that to determine - // interpolated points, alphas, and normals - // Given the plane equation P dot N + d = 0 - // and the barycentric coodinate equation P = Org + EdgeU * u + EdgeV * v - // Plug em in. Intersection occurs at u = 0 or v = 0 or u + v = 1 - - float orgDotNormal = DotProduct( org, plane.AsVector3D() ); - float edgeUDotNormal = DotProduct( edgeU, plane.AsVector3D() ); - float edgeVDotNormal = DotProduct( edgeV, plane.AsVector3D() ); - - int ptIdx = 0; - - // u = 0 - if ( edgeVDotNormal != 0.0f ) - { - pIntersection[ptIdx].x = 0.0f; - pIntersection[ptIdx].y = - ( orgDotNormal - plane.w ) / edgeVDotNormal; - if ((pIntersection[ptIdx].y >= 0.0f) && (pIntersection[ptIdx].y <= 1.0f)) - ++ptIdx; - } - - // v = 0 - if ( edgeUDotNormal != 0.0f ) - { - pIntersection[ptIdx].x = - ( orgDotNormal - plane.w ) / edgeUDotNormal; - pIntersection[ptIdx].y = 0.0f; - if ((pIntersection[ptIdx].x >= 0.0f) && (pIntersection[ptIdx].x <= 1.0f)) - ++ptIdx; - } - - // u + v = 1 - if (ptIdx == 2) - return ptIdx; - - if ( edgeVDotNormal != edgeUDotNormal ) - { - pIntersection[ptIdx].x = - ( orgDotNormal - plane.w + edgeVDotNormal) / - ( edgeUDotNormal - edgeVDotNormal); - pIntersection[ptIdx].y = 1.0f - pIntersection[ptIdx].x;; - if ((pIntersection[ptIdx].x >= 0.0f) && (pIntersection[ptIdx].x <= 1.0f) && - (pIntersection[ptIdx].y >= 0.0f) && (pIntersection[ptIdx].y <= 1.0f)) - ++ptIdx; - } - - Assert( ptIdx < 3 ); - return ptIdx; -} - - -//----------------------------------------------------------------------------- -// Returns true if a box intersects with a sphere -//----------------------------------------------------------------------------- -bool IsSphereIntersectingSphere( const Vector& center1, float radius1, - const Vector& center2, float radius2 ) -{ - Vector delta; - VectorSubtract( center2, center1, delta ); - float distSq = delta.LengthSqr(); - float radiusSum = radius1 + radius2; - return (distSq <= (radiusSum * radiusSum)); -} - - -//----------------------------------------------------------------------------- -// Returns true if a box intersects with a sphere -//----------------------------------------------------------------------------- -bool IsBoxIntersectingSphere( const Vector& boxMin, const Vector& boxMax, - const Vector& center, float radius ) -{ - // See Graphics Gems, box-sphere intersection - float dmin = 0.0f; - float flDelta; - - // Unrolled the loop.. this is a big cycle stealer... - if (center[0] < boxMin[0]) - { - flDelta = center[0] - boxMin[0]; - dmin += flDelta * flDelta; - } - else if (center[0] > boxMax[0]) - { - flDelta = boxMax[0] - center[0]; - dmin += flDelta * flDelta; - } - - if (center[1] < boxMin[1]) - { - flDelta = center[1] - boxMin[1]; - dmin += flDelta * flDelta; - } - else if (center[1] > boxMax[1]) - { - flDelta = boxMax[1] - center[1]; - dmin += flDelta * flDelta; - } - - if (center[2] < boxMin[2]) - { - flDelta = center[2] - boxMin[2]; - dmin += flDelta * flDelta; - } - else if (center[2] > boxMax[2]) - { - flDelta = boxMax[2] - center[2]; - dmin += flDelta * flDelta; - } - - return dmin < radius * radius; -} - -bool IsBoxIntersectingSphereExtents( const Vector& boxCenter, const Vector& boxHalfDiag, - const Vector& center, float radius ) -{ - // See Graphics Gems, box-sphere intersection - float dmin = 0.0f; - float flDelta, flDiff; - - // Unrolled the loop.. this is a big cycle stealer... - flDiff = FloatMakePositive( center.x - boxCenter.x ); - if (flDiff > boxHalfDiag.x) - { - flDelta = flDiff - boxHalfDiag.x; - dmin += flDelta * flDelta; - } - - flDiff = FloatMakePositive( center.y - boxCenter.y ); - if (flDiff > boxHalfDiag.y) - { - flDelta = flDiff - boxHalfDiag.y; - dmin += flDelta * flDelta; - } - - flDiff = FloatMakePositive( center.z - boxCenter.z ); - if (flDiff > boxHalfDiag.z) - { - flDelta = flDiff - boxHalfDiag.z; - dmin += flDelta * flDelta; - } - - return dmin < radius * radius; -} - - -//----------------------------------------------------------------------------- -// Returns true if a rectangle intersects with a circle -//----------------------------------------------------------------------------- -bool IsCircleIntersectingRectangle( const Vector2D& boxMin, const Vector2D& boxMax, - const Vector2D& center, float radius ) -{ - // See Graphics Gems, box-sphere intersection - float dmin = 0.0f; - float flDelta; - - if (center[0] < boxMin[0]) - { - flDelta = center[0] - boxMin[0]; - dmin += flDelta * flDelta; - } - else if (center[0] > boxMax[0]) - { - flDelta = boxMax[0] - center[0]; - dmin += flDelta * flDelta; - } - - if (center[1] < boxMin[1]) - { - flDelta = center[1] - boxMin[1]; - dmin += flDelta * flDelta; - } - else if (center[1] > boxMax[1]) - { - flDelta = boxMax[1] - center[1]; - dmin += flDelta * flDelta; - } - - return dmin < radius * radius; -} - - -//----------------------------------------------------------------------------- -// returns true if there's an intersection between ray and sphere -//----------------------------------------------------------------------------- -bool IsRayIntersectingSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, - const Vector& vecCenter, float flRadius, float flTolerance ) -{ - // For this algorithm, find a point on the ray which is closest to the sphere origin - // Do this by making a plane passing through the sphere origin - // whose normal is parallel to the ray. Intersect that plane with the ray. - // Plane: N dot P = I, N = D (ray direction), I = C dot N = C dot D - // Ray: P = O + D * t - // D dot ( O + D * t ) = C dot D - // D dot O + D dot D * t = C dot D - // t = (C - O) dot D / D dot D - // Clamp t to (0,1) - // Find distance of the point on the ray to the sphere center. - Assert( flTolerance >= 0.0f ); - flRadius += flTolerance; - - Vector vecRayToSphere; - VectorSubtract( vecCenter, vecRayOrigin, vecRayToSphere ); - float flNumerator = DotProduct( vecRayToSphere, vecRayDelta ); - - float t; - if (flNumerator <= 0.0f) - { - t = 0.0f; - } - else - { - float flDenominator = DotProduct( vecRayDelta, vecRayDelta ); - if ( flNumerator > flDenominator ) - t = 1.0f; - else - t = flNumerator / flDenominator; - } - - Vector vecClosestPoint; - VectorMA( vecRayOrigin, t, vecRayDelta, vecClosestPoint ); - return ( vecClosestPoint.DistToSqr( vecCenter ) <= flRadius * flRadius ); - - // NOTE: This in an alternate algorithm which I didn't use because I'd have to use a sqrt - // So it's probably faster to do this other algorithm. I'll leave the comments here - // for how to go back if we want to - - // Solve using the ray equation + the sphere equation - // P = o + dt - // (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 - // (ox + dx * t - xc)^2 + (oy + dy * t - yc)^2 + (oz + dz * t - zc)^2 = r^2 - // (ox - xc)^2 + 2 * (ox-xc) * dx * t + dx^2 * t^2 + - // (oy - yc)^2 + 2 * (oy-yc) * dy * t + dy^2 * t^2 + - // (oz - zc)^2 + 2 * (oz-zc) * dz * t + dz^2 * t^2 = r^2 - // (dx^2 + dy^2 + dz^2) * t^2 + 2 * ((ox-xc)dx + (oy-yc)dy + (oz-zc)dz) t + - // (ox-xc)^2 + (oy-yc)^2 + (oz-zc)^2 - r^2 = 0 - // or, t = (-b +/- sqrt( b^2 - 4ac)) / 2a - // a = DotProduct( vecRayDelta, vecRayDelta ); - // b = 2 * DotProduct( vecRayOrigin - vecCenter, vecRayDelta ) - // c = DotProduct(vecRayOrigin - vecCenter, vecRayOrigin - vecCenter) - flRadius * flRadius; - // Valid solutions are possible only if b^2 - 4ac >= 0 - // Therefore, compute that value + see if we got it -} - - -//----------------------------------------------------------------------------- -// -// IntersectInfiniteRayWithSphere -// -// Returns whether or not there was an intersection. -// Returns the two intersection points -// -//----------------------------------------------------------------------------- -bool IntersectInfiniteRayWithSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, - const Vector &vecSphereCenter, float flRadius, float *pT1, float *pT2 ) -{ - // Solve using the ray equation + the sphere equation - // P = o + dt - // (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 - // (ox + dx * t - xc)^2 + (oy + dy * t - yc)^2 + (oz + dz * t - zc)^2 = r^2 - // (ox - xc)^2 + 2 * (ox-xc) * dx * t + dx^2 * t^2 + - // (oy - yc)^2 + 2 * (oy-yc) * dy * t + dy^2 * t^2 + - // (oz - zc)^2 + 2 * (oz-zc) * dz * t + dz^2 * t^2 = r^2 - // (dx^2 + dy^2 + dz^2) * t^2 + 2 * ((ox-xc)dx + (oy-yc)dy + (oz-zc)dz) t + - // (ox-xc)^2 + (oy-yc)^2 + (oz-zc)^2 - r^2 = 0 - // or, t = (-b +/- sqrt( b^2 - 4ac)) / 2a - // a = DotProduct( vecRayDelta, vecRayDelta ); - // b = 2 * DotProduct( vecRayOrigin - vecCenter, vecRayDelta ) - // c = DotProduct(vecRayOrigin - vecCenter, vecRayOrigin - vecCenter) - flRadius * flRadius; - - Vector vecSphereToRay; - VectorSubtract( vecRayOrigin, vecSphereCenter, vecSphereToRay ); - - float a = DotProduct( vecRayDelta, vecRayDelta ); - - // This would occur in the case of a zero-length ray - if ( a == 0.0f ) - { - *pT1 = *pT2 = 0.0f; - return vecSphereToRay.LengthSqr() <= flRadius * flRadius; - } - - float b = 2 * DotProduct( vecSphereToRay, vecRayDelta ); - float c = DotProduct( vecSphereToRay, vecSphereToRay ) - flRadius * flRadius; - float flDiscrim = b * b - 4 * a * c; - if ( flDiscrim < 0.0f ) - return false; - - flDiscrim = sqrt( flDiscrim ); - float oo2a = 0.5f / a; - *pT1 = ( - b - flDiscrim ) * oo2a; - *pT2 = ( - b + flDiscrim ) * oo2a; - return true; -} - - - -//----------------------------------------------------------------------------- -// -// IntersectRayWithSphere -// -// Returns whether or not there was an intersection. -// Returns the two intersection points, clamped to (0,1) -// -//----------------------------------------------------------------------------- -bool IntersectRayWithSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, - const Vector &vecSphereCenter, float flRadius, float *pT1, float *pT2 ) -{ - if ( !IntersectInfiniteRayWithSphere( vecRayOrigin, vecRayDelta, vecSphereCenter, flRadius, pT1, pT2 ) ) - return false; - - if (( *pT1 > 1.0f ) || ( *pT2 < 0.0f )) - return false; - - // Clamp it! - if ( *pT1 < 0.0f ) - *pT1 = 0.0f; - if ( *pT2 > 1.0f ) - *pT2 = 1.0f; - - return true; -} - - -// returns true if the sphere and cone intersect -// NOTE: cone sine/cosine are the half angle of the cone -bool IsSphereIntersectingCone( const Vector &sphereCenter, float sphereRadius, const Vector &coneOrigin, const Vector &coneNormal, float coneSine, float coneCosine ) -{ - Vector backCenter = coneOrigin - (sphereRadius / coneSine) * coneNormal; - Vector delta = sphereCenter - backCenter; - float deltaLen = delta.Length(); - if ( DotProduct(coneNormal, delta) >= deltaLen*coneCosine ) - { - delta = sphereCenter - coneOrigin; - deltaLen = delta.Length(); - if ( -DotProduct(coneNormal, delta) >= deltaLen * coneSine ) - { - return ( deltaLen <= sphereRadius ) ? true : false; - } - return true; - } - return false; -} - - - -//----------------------------------------------------------------------------- -// returns true if the point is in the box -//----------------------------------------------------------------------------- -bool IsPointInBox( const Vector& pt, const Vector& boxMin, const Vector& boxMax ) -{ - Assert( boxMin[0] <= boxMax[0] ); - Assert( boxMin[1] <= boxMax[1] ); - Assert( boxMin[2] <= boxMax[2] ); - - // on x360, force use of SIMD version. - if (IsX360()) - { - return IsPointInBox( LoadUnaligned3SIMD(pt.Base()), LoadUnaligned3SIMD(boxMin.Base()), LoadUnaligned3SIMD(boxMax.Base()) ) ; - } - - if ( (pt[0] > boxMax[0]) || (pt[0] < boxMin[0]) ) - return false; - if ( (pt[1] > boxMax[1]) || (pt[1] < boxMin[1]) ) - return false; - if ( (pt[2] > boxMax[2]) || (pt[2] < boxMin[2]) ) - return false; - return true; -} - - -bool IsPointInCone( const Vector &pt, const Vector &origin, const Vector &axis, float cosAngle, float length ) -{ - Vector delta = pt - origin; - float dist = VectorNormalize( delta ); - float dot = DotProduct( delta, axis ); - if ( dot < cosAngle ) - return false; - if ( dist * dot > length ) - return false; - - return true; -} - - -//----------------------------------------------------------------------------- -// returns true if there's an intersection between two boxes -//----------------------------------------------------------------------------- -bool IsBoxIntersectingBox( const Vector& boxMin1, const Vector& boxMax1, - const Vector& boxMin2, const Vector& boxMax2 ) -{ - Assert( boxMin1[0] <= boxMax1[0] ); - Assert( boxMin1[1] <= boxMax1[1] ); - Assert( boxMin1[2] <= boxMax1[2] ); - Assert( boxMin2[0] <= boxMax2[0] ); - Assert( boxMin2[1] <= boxMax2[1] ); - Assert( boxMin2[2] <= boxMax2[2] ); - - if ( (boxMin1[0] > boxMax2[0]) || (boxMax1[0] < boxMin2[0]) ) - return false; - if ( (boxMin1[1] > boxMax2[1]) || (boxMax1[1] < boxMin2[1]) ) - return false; - if ( (boxMin1[2] > boxMax2[2]) || (boxMax1[2] < boxMin2[2]) ) - return false; - return true; -} - -bool IsBoxIntersectingBoxExtents( const Vector& boxCenter1, const Vector& boxHalfDiagonal1, - const Vector& boxCenter2, const Vector& boxHalfDiagonal2 ) -{ - Vector vecDelta, vecSize; - VectorSubtract( boxCenter1, boxCenter2, vecDelta ); - VectorAdd( boxHalfDiagonal1, boxHalfDiagonal2, vecSize ); - return ( FloatMakePositive( vecDelta.x ) <= vecSize.x ) && - ( FloatMakePositive( vecDelta.y ) <= vecSize.y ) && - ( FloatMakePositive( vecDelta.z ) <= vecSize.z ); -} - - -//----------------------------------------------------------------------------- -// -// IsOBBIntersectingOBB -// -// returns true if there's an intersection between two OBBs -// -//----------------------------------------------------------------------------- -bool IsOBBIntersectingOBB( const Vector &vecOrigin1, const QAngle &vecAngles1, const Vector& boxMin1, const Vector& boxMax1, - const Vector &vecOrigin2, const QAngle &vecAngles2, const Vector& boxMin2, const Vector& boxMax2, float flTolerance ) -{ - // FIXME: Simple case AABB check doesn't work because the min and max extents are not oriented based on the angle - // this fast check would only be good for cubes. - /*if ( vecAngles1 == vecAngles2 ) - { - const Vector &vecDelta = vecOrigin2 - vecOrigin1; - Vector vecOtherMins, vecOtherMaxs; - VectorAdd( boxMin2, vecDelta, vecOtherMins ); - VectorAdd( boxMax2, vecDelta, vecOtherMaxs ); - return IsBoxIntersectingBox( boxMin1, boxMax1, vecOtherMins, vecOtherMaxs ); - }*/ - - // OBB test... - cplane_t plane; - bool bFoundPlane = ComputeSeparatingPlane( vecOrigin1, vecAngles1, boxMin1, boxMax1, - vecOrigin2, vecAngles2, boxMin2, boxMax2, flTolerance, &plane ); - return (bFoundPlane == false); -} - -// NOTE: This is only very slightly faster on high end PCs and x360 -#define USE_SIMD_RAY_CHECKS 1 -//----------------------------------------------------------------------------- -// returns true if there's an intersection between box and ray -//----------------------------------------------------------------------------- -bool FASTCALL IsBoxIntersectingRay( const Vector& boxMin, const Vector& boxMax, - const Vector& origin, const Vector& vecDelta, float flTolerance ) -{ - -#if USE_SIMD_RAY_CHECKS - // Load the unaligned ray/box parameters into SIMD registers - fltx4 start = LoadUnaligned3SIMD(origin.Base()); - fltx4 delta = LoadUnaligned3SIMD(vecDelta.Base()); - fltx4 boxMins = LoadUnaligned3SIMD( boxMin.Base() ); - fltx4 boxMaxs = LoadUnaligned3SIMD( boxMax.Base() ); - fltx4 epsilon = ReplicateX4(flTolerance); - // compute the mins/maxs of the box expanded by the ray extents - // relocate the problem so that the ray start is at the origin. - fltx4 offsetMins = SubSIMD(boxMins, start); - fltx4 offsetMaxs = SubSIMD(boxMaxs, start); - fltx4 offsetMinsExpanded = SubSIMD(offsetMins, epsilon); - fltx4 offsetMaxsExpanded = AddSIMD(offsetMaxs, epsilon); - - // Check to see if both the origin (start point) and the end point (delta) are on the front side - // of any of the box sides - if so there can be no intersection - fltx4 startOutMins = CmpLtSIMD(Four_Zeros, offsetMinsExpanded); - fltx4 endOutMins = CmpLtSIMD(delta,offsetMinsExpanded); - fltx4 minsMask = AndSIMD( startOutMins, endOutMins ); - fltx4 startOutMaxs = CmpGtSIMD(Four_Zeros, offsetMaxsExpanded); - fltx4 endOutMaxs = CmpGtSIMD(delta,offsetMaxsExpanded); - fltx4 maxsMask = AndSIMD( startOutMaxs, endOutMaxs ); - if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(minsMask,maxsMask)))) - return false; - - // now build the per-axis interval of t for intersections - fltx4 invDelta = ReciprocalSaturateSIMD(delta); - fltx4 tmins = MulSIMD( offsetMinsExpanded, invDelta ); - fltx4 tmaxs = MulSIMD( offsetMaxsExpanded, invDelta ); - fltx4 crossPlane = OrSIMD(XorSIMD(startOutMins,endOutMins), XorSIMD(startOutMaxs,endOutMaxs)); - - // only consider axes where we crossed a plane - tmins = MaskedAssign( crossPlane, tmins, Four_Negative_FLT_MAX ); - tmaxs = MaskedAssign( crossPlane, tmaxs, Four_FLT_MAX ); - - // now sort the interval per axis - fltx4 mint = MinSIMD( tmins, tmaxs ); - fltx4 maxt = MaxSIMD( tmins, tmaxs ); - - // now find the intersection of the intervals on all axes - fltx4 firstOut = FindLowestSIMD3(maxt); - fltx4 lastIn = FindHighestSIMD3(mint); - // NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut] - firstOut = MinSIMD(firstOut, Four_Ones); - lastIn = MaxSIMD(lastIn, Four_Zeros); - - // If the final interval is valid lastIn boxMax[i] + flTolerance) ) - return false; - - continue; - } - - // non-parallel case - // Find the t's corresponding to the entry and exit of - // the ray along x, y, and z. The find the furthest entry - // point, and the closest exit point. Once that is done, - // we know we don't collide if the closest exit point - // is behind the starting location. We also don't collide if - // the closest exit point is in front of the furthest entry point - - float invDelta = 1.0f / vecDelta[i]; - float t1 = (boxMin[i] - flTolerance - origin[i]) * invDelta; - float t2 = (boxMax[i] + flTolerance - origin[i]) * invDelta; - if (t1 > t2) - { - float temp = t1; - t1 = t2; - t2 = temp; - } - if (t1 > tmin) - tmin = t1; - if (t2 < tmax) - tmax = t2; - if (tmin > tmax) - return false; - if (tmax < 0) - return false; - if (tmin > 1) - return false; - } - - return true; -#endif -} - -//----------------------------------------------------------------------------- -// returns true if there's an intersection between box and ray -//----------------------------------------------------------------------------- -bool FASTCALL IsBoxIntersectingRay( const Vector& boxMin, const Vector& boxMax, - const Vector& origin, const Vector& vecDelta, - const Vector& vecInvDelta, float flTolerance ) -{ -#if USE_SIMD_RAY_CHECKS - // Load the unaligned ray/box parameters into SIMD registers - fltx4 start = LoadUnaligned3SIMD(origin.Base()); - fltx4 delta = LoadUnaligned3SIMD(vecDelta.Base()); - fltx4 boxMins = LoadUnaligned3SIMD( boxMin.Base() ); - fltx4 boxMaxs = LoadUnaligned3SIMD( boxMax.Base() ); - // compute the mins/maxs of the box expanded by the ray extents - // relocate the problem so that the ray start is at the origin. - boxMins = SubSIMD(boxMins, start); - boxMaxs = SubSIMD(boxMaxs, start); - - // Check to see if both the origin (start point) and the end point (delta) are on the front side - // of any of the box sides - if so there can be no intersection - fltx4 startOutMins = CmpLtSIMD(Four_Zeros, boxMins); - fltx4 endOutMins = CmpLtSIMD(delta,boxMins); - fltx4 minsMask = AndSIMD( startOutMins, endOutMins ); - fltx4 startOutMaxs = CmpGtSIMD(Four_Zeros, boxMaxs); - fltx4 endOutMaxs = CmpGtSIMD(delta,boxMaxs); - fltx4 maxsMask = AndSIMD( startOutMaxs, endOutMaxs ); - if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(minsMask,maxsMask)))) - return false; - - // now build the per-axis interval of t for intersections - fltx4 epsilon = ReplicateX4(flTolerance); - fltx4 invDelta = LoadUnaligned3SIMD(vecInvDelta.Base()); - boxMins = SubSIMD(boxMins, epsilon); - boxMaxs = AddSIMD(boxMaxs, epsilon); - - boxMins = MulSIMD( boxMins, invDelta ); - boxMaxs = MulSIMD( boxMaxs, invDelta ); - - fltx4 crossPlane = OrSIMD(XorSIMD(startOutMins,endOutMins), XorSIMD(startOutMaxs,endOutMaxs)); - // only consider axes where we crossed a plane - boxMins = MaskedAssign( crossPlane, boxMins, Four_Negative_FLT_MAX ); - boxMaxs = MaskedAssign( crossPlane, boxMaxs, Four_FLT_MAX ); - - // now sort the interval per axis - fltx4 mint = MinSIMD( boxMins, boxMaxs ); - fltx4 maxt = MaxSIMD( boxMins, boxMaxs ); - - // now find the intersection of the intervals on all axes - fltx4 firstOut = FindLowestSIMD3(maxt); - fltx4 lastIn = FindHighestSIMD3(mint); - // NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut] - firstOut = MinSIMD(firstOut, Four_Ones); - lastIn = MaxSIMD(lastIn, Four_Zeros); - - // If the final interval is valid lastIn boxMax[i] + flTolerance ) ) - return false; - - continue; - } - - // Non-parallel case - // Find the t's corresponding to the entry and exit of - // the ray along x, y, and z. The find the furthest entry - // point, and the closest exit point. Once that is done, - // we know we don't collide if the closest exit point - // is behind the starting location. We also don't collide if - // the closest exit point is in front of the furthest entry point - float t1 = ( boxMin[i] - flTolerance - origin[i] ) * vecInvDelta[i]; - float t2 = ( boxMax[i] + flTolerance - origin[i] ) * vecInvDelta[i]; - if ( t1 > t2 ) - { - float temp = t1; - t1 = t2; - t2 = temp; - } - - if (t1 > tmin) - tmin = t1; - - if (t2 < tmax) - tmax = t2; - - if (tmin > tmax) - return false; - - if (tmax < 0) - return false; - - if (tmin > 1) - return false; - } - - return true; -#endif -} - -//----------------------------------------------------------------------------- -// Intersects a ray with a aabb, return true if they intersect -//----------------------------------------------------------------------------- -bool FASTCALL IsBoxIntersectingRay( const Vector& vecBoxMin, const Vector& vecBoxMax, const Ray_t& ray, float flTolerance ) -{ - // On the x360, we force use of the SIMD functions. -#if defined(_X360) - if (IsX360()) - { - return IsBoxIntersectingRay( - LoadUnaligned3SIMD(vecBoxMin.Base()), LoadUnaligned3SIMD(vecBoxMax.Base()), - ray, flTolerance); - } -#endif - - if ( !ray.m_IsSwept ) - { - Vector rayMins, rayMaxs; - VectorSubtract( ray.m_Start, ray.m_Extents, rayMins ); - VectorAdd( ray.m_Start, ray.m_Extents, rayMaxs ); - if ( flTolerance != 0.0f ) - { - rayMins.x -= flTolerance; rayMins.y -= flTolerance; rayMins.z -= flTolerance; - rayMaxs.x += flTolerance; rayMaxs.y += flTolerance; rayMaxs.z += flTolerance; - } - return IsBoxIntersectingBox( vecBoxMin, vecBoxMax, rayMins, rayMaxs ); - } - - Vector vecExpandedBoxMin, vecExpandedBoxMax; - VectorSubtract( vecBoxMin, ray.m_Extents, vecExpandedBoxMin ); - VectorAdd( vecBoxMax, ray.m_Extents, vecExpandedBoxMax ); - return IsBoxIntersectingRay( vecExpandedBoxMin, vecExpandedBoxMax, ray.m_Start, ray.m_Delta, flTolerance ); -} - - -//----------------------------------------------------------------------------- -// returns true if there's an intersection between box and ray (SIMD version) -//----------------------------------------------------------------------------- - - -#ifdef _X360 -bool FASTCALL IsBoxIntersectingRay( fltx4 boxMin, fltx4 boxMax, - fltx4 origin, fltx4 delta, fltx4 invDelta, // ray parameters - fltx4 vTolerance ///< eg from ReplicateX4(flTolerance) - ) -#else -bool FASTCALL IsBoxIntersectingRay( const fltx4 &inBoxMin, const fltx4 & inBoxMax, - const fltx4 & origin, const fltx4 & delta, const fltx4 & invDelta, // ray parameters - const fltx4 & vTolerance ///< eg from ReplicateX4(flTolerance) - ) -#endif -{ - // Load the unaligned ray/box parameters into SIMD registers - // compute the mins/maxs of the box expanded by the ray extents - // relocate the problem so that the ray start is at the origin. - -#ifdef _X360 - boxMin = SubSIMD(boxMin, origin); - boxMax = SubSIMD(boxMax, origin); -#else - fltx4 boxMin = SubSIMD(inBoxMin, origin); - fltx4 boxMax = SubSIMD(inBoxMax, origin); -#endif - - // Check to see if the origin (start point) and the end point (delta) are on the same side - // of any of the box sides - if so there can be no intersection - fltx4 startOutMins = AndSIMD( CmpLtSIMD(Four_Zeros, boxMin), CmpLtSIMD(delta,boxMin) ); - fltx4 startOutMaxs = AndSIMD( CmpGtSIMD(Four_Zeros, boxMax), CmpGtSIMD(delta,boxMax) ); - if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(startOutMaxs,startOutMins)))) - return false; - - // now build the per-axis interval of t for intersections - boxMin = SubSIMD(boxMin, vTolerance); - boxMax = AddSIMD(boxMax, vTolerance); - - boxMin = MulSIMD( boxMin, invDelta ); - boxMax = MulSIMD( boxMax, invDelta ); - - // now sort the interval per axis - fltx4 mint = MinSIMD( boxMin, boxMax ); - fltx4 maxt = MaxSIMD( boxMin, boxMax ); - - // now find the intersection of the intervals on all axes - fltx4 firstOut = FindLowestSIMD3(maxt); - fltx4 lastIn = FindHighestSIMD3(mint); - // NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut] - firstOut = MinSIMD(firstOut, Four_Ones); - lastIn = MaxSIMD(lastIn, Four_Zeros); - - // If the final interval is valid lastInt1 = -1.0f; - pTrace->t2 = 1.0f; - pTrace->hitside = -1; - - // UNDONE: This makes this code a little messy - pTrace->startsolid = true; - - for ( i = 0; i < 6; ++i ) - { - if ( i >= 3 ) - { - d1 = vecRayStart[i-3] - boxMaxs[i-3]; - d2 = d1 + vecRayDelta[i-3]; - } - else - { - d1 = -vecRayStart[i] + boxMins[i]; - d2 = d1 - vecRayDelta[i]; - } - - // if completely in front of face, no intersection - if (d1 > 0 && d2 > 0) - { - // UNDONE: Have to revert this in case it's still set - // UNDONE: Refactor to have only 2 return points (true/false) from this function - pTrace->startsolid = false; - return false; - } - - // completely inside, check next face - if (d1 <= 0 && d2 <= 0) - continue; - - if (d1 > 0) - { - pTrace->startsolid = false; - } - - // crosses face - if (d1 > d2) - { - f = d1 - flTolerance; - if ( f < 0 ) - { - f = 0; - } - f = f / (d1-d2); - if (f > pTrace->t1) - { - pTrace->t1 = f; - pTrace->hitside = i; - } - } - else - { - // leave - f = (d1 + flTolerance) / (d1-d2); - if (f < pTrace->t2) - { - pTrace->t2 = f; - } - } - } - - return pTrace->startsolid || (pTrace->t1 < pTrace->t2 && pTrace->t1 >= 0.0f); -} - - -//----------------------------------------------------------------------------- -// Intersects a ray against a box -//----------------------------------------------------------------------------- -bool IntersectRayWithBox( const Vector &vecRayStart, const Vector &vecRayDelta, - const Vector &boxMins, const Vector &boxMaxs, float flTolerance, CBaseTrace *pTrace, float *pFractionLeftSolid ) -{ - Collision_ClearTrace( vecRayStart, vecRayDelta, pTrace ); - - BoxTraceInfo_t trace; - - if ( IntersectRayWithBox( vecRayStart, vecRayDelta, boxMins, boxMaxs, flTolerance, &trace ) ) - { - pTrace->startsolid = trace.startsolid; - if (trace.t1 < trace.t2 && trace.t1 >= 0.0f) - { - pTrace->fraction = trace.t1; - VectorMA( pTrace->startpos, trace.t1, vecRayDelta, pTrace->endpos ); - pTrace->contents = CONTENTS_SOLID; - pTrace->plane.normal = vec3_origin; - if ( trace.hitside >= 3 ) - { - trace.hitside -= 3; - pTrace->plane.dist = boxMaxs[trace.hitside]; - pTrace->plane.normal[trace.hitside] = 1.0f; - pTrace->plane.type = trace.hitside; - } - else - { - pTrace->plane.dist = -boxMins[trace.hitside]; - pTrace->plane.normal[trace.hitside] = -1.0f; - pTrace->plane.type = trace.hitside; - } - return true; - } - - if ( pTrace->startsolid ) - { - pTrace->allsolid = (trace.t2 <= 0.0f) || (trace.t2 >= 1.0f); - pTrace->fraction = 0; - if ( pFractionLeftSolid ) - { - *pFractionLeftSolid = trace.t2; - } - pTrace->endpos = pTrace->startpos; - pTrace->contents = CONTENTS_SOLID; - pTrace->plane.dist = pTrace->startpos[0]; - pTrace->plane.normal.Init( 1.0f, 0.0f, 0.0f ); - pTrace->plane.type = 0; - pTrace->startpos = vecRayStart + (trace.t2 * vecRayDelta); - return true; - } - } - - return false; -} - - -//----------------------------------------------------------------------------- -// Intersects a ray against a box -//----------------------------------------------------------------------------- -bool IntersectRayWithBox( const Ray_t &ray, const Vector &boxMins, const Vector &boxMaxs, - float flTolerance, CBaseTrace *pTrace, float *pFractionLeftSolid ) -{ - if ( !ray.m_IsRay ) - { - Vector vecExpandedMins = boxMins - ray.m_Extents; - Vector vecExpandedMaxs = boxMaxs + ray.m_Extents; - bool bIntersects = IntersectRayWithBox( ray.m_Start, ray.m_Delta, vecExpandedMins, vecExpandedMaxs, flTolerance, pTrace, pFractionLeftSolid ); - pTrace->startpos += ray.m_StartOffset; - pTrace->endpos += ray.m_StartOffset; - return bIntersects; - } - return IntersectRayWithBox( ray.m_Start, ray.m_Delta, boxMins, boxMaxs, flTolerance, pTrace, pFractionLeftSolid ); -} - - -//----------------------------------------------------------------------------- -// Intersects a ray against an OBB, returns t1 and t2 -//----------------------------------------------------------------------------- -bool IntersectRayWithOBB( const Vector &vecRayStart, const Vector &vecRayDelta, - const matrix3x4_t &matOBBToWorld, const Vector &vecOBBMins, const Vector &vecOBBMaxs, - float flTolerance, BoxTraceInfo_t *pTrace ) -{ - // FIXME: Two transforms is pretty expensive. Should we optimize this? - Vector start, delta; - VectorITransform( vecRayStart, matOBBToWorld, start ); - VectorIRotate( vecRayDelta, matOBBToWorld, delta ); - - return IntersectRayWithBox( start, delta, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); -} - - - -//----------------------------------------------------------------------------- -// Intersects a ray against an OBB -//----------------------------------------------------------------------------- -bool IntersectRayWithOBB( const Vector &vecRayStart, const Vector &vecRayDelta, - const matrix3x4_t &matOBBToWorld, const Vector &vecOBBMins, const Vector &vecOBBMaxs, - float flTolerance, CBaseTrace *pTrace ) -{ - Collision_ClearTrace( vecRayStart, vecRayDelta, pTrace ); - - // FIXME: Make it work with tolerance - Assert( flTolerance == 0.0f ); - - // OPTIMIZE: Store this in the box instead of computing it here - // compute center in local space - Vector vecBoxExtents = (vecOBBMins + vecOBBMaxs) * 0.5; - Vector vecBoxCenter; - - // transform to world space - VectorTransform( vecBoxExtents, matOBBToWorld, vecBoxCenter ); - - // calc extents from local center - vecBoxExtents = vecOBBMaxs - vecBoxExtents; - - // OPTIMIZE: This is optimized for world space. If the transform is fast enough, it may make more - // sense to just xform and call UTIL_ClipToBox() instead. MEASURE THIS. - - // save the extents of the ray along - Vector extent, uextent; - Vector segmentCenter = vecRayStart + vecRayDelta - vecBoxCenter; - - extent.Init(); - - // check box axes for separation - for ( int j = 0; j < 3; j++ ) - { - extent[j] = vecRayDelta.x * matOBBToWorld[0][j] + vecRayDelta.y * matOBBToWorld[1][j] + vecRayDelta.z * matOBBToWorld[2][j]; - uextent[j] = fabsf(extent[j]); - float coord = segmentCenter.x * matOBBToWorld[0][j] + segmentCenter.y * matOBBToWorld[1][j] + segmentCenter.z * matOBBToWorld[2][j]; - coord = fabsf(coord); - - if ( coord > (vecBoxExtents[j] + uextent[j]) ) - return false; - } - - // now check cross axes for separation - float tmp, cextent; - Vector cross = vecRayDelta.Cross( segmentCenter ); - cextent = cross.x * matOBBToWorld[0][0] + cross.y * matOBBToWorld[1][0] + cross.z * matOBBToWorld[2][0]; - cextent = fabsf(cextent); - tmp = vecBoxExtents[1]*uextent[2] + vecBoxExtents[2]*uextent[1]; - if ( cextent > tmp ) - return false; - - cextent = cross.x * matOBBToWorld[0][1] + cross.y * matOBBToWorld[1][1] + cross.z * matOBBToWorld[2][1]; - cextent = fabsf(cextent); - tmp = vecBoxExtents[0]*uextent[2] + vecBoxExtents[2]*uextent[0]; - if ( cextent > tmp ) - return false; - - cextent = cross.x * matOBBToWorld[0][2] + cross.y * matOBBToWorld[1][2] + cross.z * matOBBToWorld[2][2]; - cextent = fabsf(cextent); - tmp = vecBoxExtents[0]*uextent[1] + vecBoxExtents[1]*uextent[0]; - if ( cextent > tmp ) - return false; - - // !!! We hit this box !!! compute intersection point and return - // Compute ray start in bone space - Vector start; - VectorITransform( vecRayStart, matOBBToWorld, start ); - - // extent is ray.m_Delta in bone space, recompute delta in bone space - extent *= 2.0f; - - // delta was prescaled by the current t, so no need to see if this intersection - // is closer - trace_t boxTrace; - if ( !IntersectRayWithBox( start, extent, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ) ) - return false; - - // Fix up the start/end pos and fraction - Vector vecTemp; - VectorTransform( pTrace->endpos, matOBBToWorld, vecTemp ); - pTrace->endpos = vecTemp; - - pTrace->startpos = vecRayStart; - pTrace->fraction *= 2.0f; - - // Fix up the plane information - float flSign = pTrace->plane.normal[ pTrace->plane.type ]; - pTrace->plane.normal[0] = flSign * matOBBToWorld[0][pTrace->plane.type]; - pTrace->plane.normal[1] = flSign * matOBBToWorld[1][pTrace->plane.type]; - pTrace->plane.normal[2] = flSign * matOBBToWorld[2][pTrace->plane.type]; - pTrace->plane.dist = DotProduct( pTrace->endpos, pTrace->plane.normal ); - pTrace->plane.type = 3; - - return true; -} - - -//----------------------------------------------------------------------------- -// Intersects a ray against an OBB -//----------------------------------------------------------------------------- -bool IntersectRayWithOBB( const Vector &vecRayOrigin, const Vector &vecRayDelta, - const Vector &vecBoxOrigin, const QAngle &angBoxRotation, - const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace ) -{ - if (angBoxRotation == vec3_angle) - { - Vector vecAbsMins, vecAbsMaxs; - VectorAdd( vecBoxOrigin, vecOBBMins, vecAbsMins ); - VectorAdd( vecBoxOrigin, vecOBBMaxs, vecAbsMaxs ); - return IntersectRayWithBox( vecRayOrigin, vecRayDelta, vecAbsMins, vecAbsMaxs, flTolerance, pTrace ); - } - - matrix3x4_t obbToWorld; - AngleMatrix( angBoxRotation, vecBoxOrigin, obbToWorld ); - return IntersectRayWithOBB( vecRayOrigin, vecRayDelta, obbToWorld, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); -} - - -//----------------------------------------------------------------------------- -// Box support map -//----------------------------------------------------------------------------- -inline void ComputeSupportMap( const Vector &vecDirection, const Vector &vecBoxMins, - const Vector &vecBoxMaxs, float pDist[2] ) -{ - int nIndex = (vecDirection.x > 0.0f); - pDist[nIndex] = vecBoxMaxs.x * vecDirection.x; - pDist[1 - nIndex] = vecBoxMins.x * vecDirection.x; - - nIndex = (vecDirection.y > 0.0f); - pDist[nIndex] += vecBoxMaxs.y * vecDirection.y; - pDist[1 - nIndex] += vecBoxMins.y * vecDirection.y; - - nIndex = (vecDirection.z > 0.0f); - pDist[nIndex] += vecBoxMaxs.z * vecDirection.z; - pDist[1 - nIndex] += vecBoxMins.z * vecDirection.z; -} - -inline void ComputeSupportMap( const Vector &vecDirection, int i1, int i2, - const Vector &vecBoxMins, const Vector &vecBoxMaxs, float pDist[2] ) -{ - int nIndex = (vecDirection[i1] > 0.0f); - pDist[nIndex] = vecBoxMaxs[i1] * vecDirection[i1]; - pDist[1 - nIndex] = vecBoxMins[i1] * vecDirection[i1]; - - nIndex = (vecDirection[i2] > 0.0f); - pDist[nIndex] += vecBoxMaxs[i2] * vecDirection[i2]; - pDist[1 - nIndex] += vecBoxMins[i2] * vecDirection[i2]; -} - -//----------------------------------------------------------------------------- -// Intersects a ray against an OBB -//----------------------------------------------------------------------------- -static int s_ExtIndices[3][2] = -{ - { 2, 1 }, - { 0, 2 }, - { 0, 1 }, -}; - -static int s_MatIndices[3][2] = -{ - { 1, 2 }, - { 2, 0 }, - { 1, 0 }, -}; - -bool IntersectRayWithOBB( const Ray_t &ray, const matrix3x4_t &matOBBToWorld, - const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace ) -{ - if ( ray.m_IsRay ) - { - return IntersectRayWithOBB( ray.m_Start, ray.m_Delta, matOBBToWorld, - vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); - } - - Collision_ClearTrace( ray.m_Start + ray.m_StartOffset, ray.m_Delta, pTrace ); - - // Compute a bounding sphere around the bloated OBB - Vector vecOBBCenter; - VectorAdd( vecOBBMins, vecOBBMaxs, vecOBBCenter ); - vecOBBCenter *= 0.5f; - vecOBBCenter.x += matOBBToWorld[0][3]; - vecOBBCenter.y += matOBBToWorld[1][3]; - vecOBBCenter.z += matOBBToWorld[2][3]; - - Vector vecOBBHalfDiagonal; - VectorSubtract( vecOBBMaxs, vecOBBMins, vecOBBHalfDiagonal ); - vecOBBHalfDiagonal *= 0.5f; - - float flRadius = vecOBBHalfDiagonal.Length() + ray.m_Extents.Length(); - if ( !IsRayIntersectingSphere( ray.m_Start, ray.m_Delta, vecOBBCenter, flRadius, flTolerance ) ) - return false; - - // Ok, we passed the trivial reject, so lets do the dirty deed. - // Basically we're going to do the GJK thing explicitly. We'll shrink the ray down - // to a point, and bloat the OBB by the ray's extents. This will generate facet - // planes which are perpendicular to all of the separating axes typically seen in - // a standard seperating axis implementation. - - // We're going to create a number of planes through various vertices in the OBB - // which represent all of the separating planes. Then we're going to bloat the planes - // by the ray extents. - - // We're going to do all work in OBB-space because it's easier to do the - // support-map in this case - - // First, transform the ray into the space of the OBB - Vector vecLocalRayOrigin, vecLocalRayDirection; - VectorITransform( ray.m_Start, matOBBToWorld, vecLocalRayOrigin ); - VectorIRotate( ray.m_Delta, matOBBToWorld, vecLocalRayDirection ); - - // Next compute all separating planes - Vector pPlaneNormal[15]; - float ppPlaneDist[15][2]; - - int i; - for ( i = 0; i < 3; ++i ) - { - // Each plane needs to be bloated an amount = to the abs dot product of - // the ray extents with the plane normal - // For the OBB planes, do it in world space; - // and use the direction of the OBB (the ith column of matOBBToWorld) in world space vs extents - pPlaneNormal[i].Init( ); - pPlaneNormal[i][i] = 1.0f; - - float flExtentDotNormal = - FloatMakePositive( matOBBToWorld[0][i] * ray.m_Extents.x ) + - FloatMakePositive( matOBBToWorld[1][i] * ray.m_Extents.y ) + - FloatMakePositive( matOBBToWorld[2][i] * ray.m_Extents.z ); - - ppPlaneDist[i][0] = vecOBBMins[i] - flExtentDotNormal; - ppPlaneDist[i][1] = vecOBBMaxs[i] + flExtentDotNormal; - - // For the ray-extents planes, they are bloated by the extents - // Use the support map to determine which - VectorCopy( matOBBToWorld[i], pPlaneNormal[i+3].Base() ); - ComputeSupportMap( pPlaneNormal[i+3], vecOBBMins, vecOBBMaxs, ppPlaneDist[i+3] ); - ppPlaneDist[i+3][0] -= ray.m_Extents[i]; - ppPlaneDist[i+3][1] += ray.m_Extents[i]; - - // Now the edge cases... (take the cross product of x,y,z axis w/ ray extent axes - // given by the rows of the obb to world matrix. - // Compute the ray extent bloat in world space because it's easier... - - // These are necessary to compute the world-space versions of - // the edges so we can compute the extent dot products - float flRayExtent0 = ray.m_Extents[s_ExtIndices[i][0]]; - float flRayExtent1 = ray.m_Extents[s_ExtIndices[i][1]]; - const float *pMatRow0 = matOBBToWorld[s_MatIndices[i][0]]; - const float *pMatRow1 = matOBBToWorld[s_MatIndices[i][1]]; - - // x axis of the OBB + world ith axis - pPlaneNormal[i+6].Init( 0.0f, -matOBBToWorld[i][2], matOBBToWorld[i][1] ); - ComputeSupportMap( pPlaneNormal[i+6], 1, 2, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+6] ); - flExtentDotNormal = - FloatMakePositive( pMatRow0[0] ) * flRayExtent0 + - FloatMakePositive( pMatRow1[0] ) * flRayExtent1; - ppPlaneDist[i+6][0] -= flExtentDotNormal; - ppPlaneDist[i+6][1] += flExtentDotNormal; - - // y axis of the OBB + world ith axis - pPlaneNormal[i+9].Init( matOBBToWorld[i][2], 0.0f, -matOBBToWorld[i][0] ); - ComputeSupportMap( pPlaneNormal[i+9], 0, 2, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+9] ); - flExtentDotNormal = - FloatMakePositive( pMatRow0[1] ) * flRayExtent0 + - FloatMakePositive( pMatRow1[1] ) * flRayExtent1; - ppPlaneDist[i+9][0] -= flExtentDotNormal; - ppPlaneDist[i+9][1] += flExtentDotNormal; - - // z axis of the OBB + world ith axis - pPlaneNormal[i+12].Init( -matOBBToWorld[i][1], matOBBToWorld[i][0], 0.0f ); - ComputeSupportMap( pPlaneNormal[i+12], 0, 1, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+12] ); - flExtentDotNormal = - FloatMakePositive( pMatRow0[2] ) * flRayExtent0 + - FloatMakePositive( pMatRow1[2] ) * flRayExtent1; - ppPlaneDist[i+12][0] -= flExtentDotNormal; - ppPlaneDist[i+12][1] += flExtentDotNormal; - } - - float enterfrac, leavefrac; - float d1[2], d2[2]; - float f; - - int hitplane = -1; - int hitside = -1; - enterfrac = -1.0f; - leavefrac = 1.0f; - - pTrace->startsolid = true; - - Vector vecLocalRayEnd; - VectorAdd( vecLocalRayOrigin, vecLocalRayDirection, vecLocalRayEnd ); - - for ( i = 0; i < 15; ++i ) - { - // FIXME: Not particularly optimal since there's a lot of 0's in the plane normals - float flStartDot = DotProduct( pPlaneNormal[i], vecLocalRayOrigin ); - float flEndDot = DotProduct( pPlaneNormal[i], vecLocalRayEnd ); - - // NOTE: Negative here is because the plane normal + dist - // are defined in negative terms for the far plane (plane dist index 0) - d1[0] = -(flStartDot - ppPlaneDist[i][0]); - d2[0] = -(flEndDot - ppPlaneDist[i][0]); - - d1[1] = flStartDot - ppPlaneDist[i][1]; - d2[1] = flEndDot - ppPlaneDist[i][1]; - - int j; - for ( j = 0; j < 2; ++j ) - { - // if completely in front near plane or behind far plane no intersection - if (d1[j] > 0 && d2[j] > 0) - return false; - - // completely inside, check next plane set - if (d1[j] <= 0 && d2[j] <= 0) - continue; - - if (d1[j] > 0) - { - pTrace->startsolid = false; - } - - // crosses face - float flDenom = 1.0f / (d1[j] - d2[j]); - if (d1[j] > d2[j]) - { - f = d1[j] - flTolerance; - if ( f < 0 ) - { - f = 0; - } - f *= flDenom; - if (f > enterfrac) - { - enterfrac = f; - hitplane = i; - hitside = j; - } - } - else - { - // leave - f = (d1[j] + flTolerance) * flDenom; - if (f < leavefrac) - { - leavefrac = f; - } - } - } - } - - if (enterfrac < leavefrac && enterfrac >= 0.0f) - { - pTrace->fraction = enterfrac; - VectorMA( pTrace->startpos, enterfrac, ray.m_Delta, pTrace->endpos ); - pTrace->contents = CONTENTS_SOLID; - - // Need to transform the plane into world space... - cplane_t temp; - temp.normal = pPlaneNormal[hitplane]; - temp.dist = ppPlaneDist[hitplane][hitside]; - if (hitside == 0) - { - temp.normal *= -1.0f; - temp.dist *= -1.0f; - } - temp.type = 3; - - MatrixITransformPlane( matOBBToWorld, temp, pTrace->plane ); - return true; - } - - if ( pTrace->startsolid ) - { - pTrace->allsolid = (leavefrac <= 0.0f) || (leavefrac >= 1.0f); - pTrace->fraction = 0; - pTrace->endpos = pTrace->startpos; - pTrace->contents = CONTENTS_SOLID; - pTrace->plane.dist = pTrace->startpos[0]; - pTrace->plane.normal.Init( 1.0f, 0.0f, 0.0f ); - pTrace->plane.type = 0; - return true; - } - - return false; -} - - -//----------------------------------------------------------------------------- -// Intersects a ray against an OBB -//----------------------------------------------------------------------------- -bool IntersectRayWithOBB( const Ray_t &ray, const Vector &vecBoxOrigin, const QAngle &angBoxRotation, - const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace ) -{ - if ( angBoxRotation == vec3_angle ) - { - Vector vecWorldMins, vecWorldMaxs; - VectorAdd( vecBoxOrigin, vecOBBMins, vecWorldMins ); - VectorAdd( vecBoxOrigin, vecOBBMaxs, vecWorldMaxs ); - return IntersectRayWithBox( ray, vecWorldMins, vecWorldMaxs, flTolerance, pTrace ); - } - - if ( ray.m_IsRay ) - { - return IntersectRayWithOBB( ray.m_Start, ray.m_Delta, vecBoxOrigin, angBoxRotation, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); - } - - matrix3x4_t matOBBToWorld; - AngleMatrix( angBoxRotation, vecBoxOrigin, matOBBToWorld ); - return IntersectRayWithOBB( ray, matOBBToWorld, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); -} - - -//----------------------------------------------------------------------------- -// -//----------------------------------------------------------------------------- -void GetNonMajorAxes( const Vector &vNormal, Vector2D &axes ) -{ - axes[0] = 0; - axes[1] = 1; - - if( FloatMakePositive( vNormal.x ) > FloatMakePositive( vNormal.y ) ) - { - if( FloatMakePositive( vNormal.x ) > FloatMakePositive( vNormal.z ) ) - { - axes[0] = 1; - axes[1] = 2; - } - } - else - { - if( FloatMakePositive( vNormal.y ) > FloatMakePositive( vNormal.z ) ) - { - axes[0] = 0; - axes[1] = 2; - } - } -} - - -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -QuadBarycentricRetval_t QuadWithParallelEdges( const Vector &vecOrigin, - const Vector &vecU, float lengthU, const Vector &vecV, float lengthV, - const Vector &pt, Vector2D &vecUV ) -{ - Ray_t rayAxis; - Ray_t rayPt; - - // - // handle the u axis - // - rayAxis.m_Start = vecOrigin; - rayAxis.m_Delta = vecU; - rayAxis.m_IsRay = true; - - rayPt.m_Start = pt; - rayPt.m_Delta = vecV * -( lengthV * 10.0f ); - rayPt.m_IsRay = true; - - float s, t; - IntersectRayWithRay( rayAxis, rayPt, t, s ); - vecUV[0] = t / lengthU; - - // - // handle the v axis - // - rayAxis.m_Delta = vecV; - - rayPt.m_Delta = vecU * -( lengthU * 10.0f ); - - IntersectRayWithRay( rayAxis, rayPt, t, s ); - vecUV[1] = t / lengthV; - - // inside of the quad?? - if( ( vecUV[0] < 0.0f ) || ( vecUV[0] > 1.0f ) || - ( vecUV[1] < 0.0f ) || ( vecUV[1] > 1.0f ) ) - return BARY_QUADRATIC_FALSE; - - return BARY_QUADRATIC_TRUE; -} - - -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -void ResolveQuadratic( double tPlus, double tMinus, - const Vector axisU0, const Vector axisU1, - const Vector axisV0, const Vector axisV1, - const Vector axisOrigin, const Vector pt, - int projU, double &s, double &t ) -{ - // calculate the sPlus, sMinus pair(s) - double sDenomPlus = ( axisU0[projU] * ( 1 - tPlus ) ) + ( axisU1[projU] * tPlus ); - double sDenomMinus = ( axisU0[projU] * ( 1 - tMinus ) ) + ( axisU1[projU] * tMinus ); - - double sPlus = UNINIT, sMinus = UNINIT; - if( FloatMakePositive( sDenomPlus ) >= 1e-5 ) - { - sPlus = ( pt[projU] - axisOrigin[projU] - ( axisV0[projU] * tPlus ) ) / sDenomPlus; - } - - if( FloatMakePositive( sDenomMinus ) >= 1e-5 ) - { - sMinus = ( pt[projU] - axisOrigin[projU] - ( axisV0[projU] * tMinus ) ) / sDenomMinus; - } - - if( ( tPlus >= 0.0 ) && ( tPlus <= 1.0 ) && ( sPlus >= 0.0 ) && ( sPlus <= 1.0 ) ) - { - s = sPlus; - t = tPlus; - return; - } - - if( ( tMinus >= 0.0 ) && ( tMinus <= 1.0 ) && ( sMinus >= 0.0 ) && ( sMinus <= 1.0 ) ) - { - s = sMinus; - t = tMinus; - return; - } - - double s0, t0, s1, t1; - - s0 = sPlus; - t0 = tPlus; - if( s0 >= 1.0 ) { s0 -= 1.0; } - if( t0 >= 1.0 ) { t0 -= 1.0; } - - s1 = sMinus; - t1 = tMinus; - if( s1 >= 1.0 ) { s1 -= 1.0; } - if( t1 >= 1.0 ) { t1 -= 1.0; } - - s0 = FloatMakePositive( s0 ); - t0 = FloatMakePositive( t0 ); - s1 = FloatMakePositive( s1 ); - t1 = FloatMakePositive( t1 ); - - double max0, max1; - max0 = s0; - if( t0 > max0 ) { max0 = t0; } - max1 = s1; - if( t1 > max1 ) { max1 = t1; } - - if( max0 > max1 ) - { - s = sMinus; - t = tMinus; - } - else - { - s = sPlus; - t = tPlus; - } -} - - -//----------------------------------------------------------------------------- -// -//----------------------------------------------------------------------------- - -QuadBarycentricRetval_t PointInQuadToBarycentric( const Vector &v1, const Vector &v2, - const Vector &v3, const Vector &v4, const Vector &point, Vector2D &uv ) -{ -#define PIQ_TEXTURE_EPSILON 0.001 -#define PIQ_PLANE_EPSILON 0.1 -#define PIQ_DOT_EPSILON 0.99f - - // - // Think of a quad with points v1, v2, v3, v4 and u, v line segments - // u0 = v2 - v1 - // u1 = v3 - v4 - // v0 = v4 - v1 - // v1 = v3 - v2 - // - Vector axisU[2], axisV[2]; - Vector axisUNorm[2], axisVNorm[2]; - axisU[0] = axisUNorm[0] = v2 - v1; - axisU[1] = axisUNorm[1] = v3 - v4; - axisV[0] = axisVNorm[0] = v4 - v1; - axisV[1] = axisVNorm[1] = v3 - v2; - - float lengthU[2], lengthV[2]; - lengthU[0] = VectorNormalize( axisUNorm[0] ); - lengthU[1] = VectorNormalize( axisUNorm[1] ); - lengthV[0] = VectorNormalize( axisVNorm[0] ); - lengthV[1] = VectorNormalize( axisVNorm[1] ); - - // - // check for an early out - parallel opposite edges! - // NOTE: quad property if 1 set of opposite edges is parallel and equal - // in length, then the other set of edges is as well - // - if( axisUNorm[0].Dot( axisUNorm[1] ) > PIQ_DOT_EPSILON ) - { - if( FloatMakePositive( lengthU[0] - lengthU[1] ) < PIQ_PLANE_EPSILON ) - { - return QuadWithParallelEdges( v1, axisUNorm[0], lengthU[0], axisVNorm[0], lengthV[0], point, uv ); - } - } - - // - // since we are solving for s in our equations below we need to ensure that - // the v axes are non-parallel - // - bool bFlipped = false; - if( axisVNorm[0].Dot( axisVNorm[1] ) > PIQ_DOT_EPSILON ) - { - Vector tmp[2]; - tmp[0] = axisV[0]; - tmp[1] = axisV[1]; - axisV[0] = axisU[0]; - axisV[1] = axisU[1]; - axisU[0] = tmp[0]; - axisU[1] = tmp[1]; - bFlipped = true; - } - - // - // get the "projection" axes - // - Vector2D projAxes; - Vector vNormal = axisU[0].Cross( axisV[0] ); - GetNonMajorAxes( vNormal, projAxes ); - - // - // NOTE: axisU[0][projAxes[0]] < axisU[0][projAxes[1]], - // this is done to decrease error when dividing later - // - if( FloatMakePositive( axisU[0][projAxes[0]] ) < FloatMakePositive( axisU[0][projAxes[1]] ) ) - { - int tmp = projAxes[0]; - projAxes[0] = projAxes[1]; - projAxes[1] = tmp; - } - - // Here's how we got these equations: - // - // Given the points and u,v line segments above... - // - // Then: - // - // (1.0) PT = P0 + U0 * s + V * t - // - // where - // - // (1.1) V = V0 + s * (V1 - V0) - // (1.2) U = U0 + t * (U1 - U0) - // - // Therefore (from 1.1 + 1.0): - // PT - P0 = U0 * s + (V0 + s * (V1-V0)) * t - // Group s's: - // PT - P0 - t * V0 = s * (U0 + t * (V1-V0)) - // Two equations and two unknowns in x and y get you the following quadratic: - // - // solve the quadratic - // - double s = 0.0, t = 0.0; - double A, negB, C; - - A = ( axisU[0][projAxes[1]] * axisV[0][projAxes[0]] ) - - ( axisU[0][projAxes[0]] * axisV[0][projAxes[1]] ) - - ( axisU[1][projAxes[1]] * axisV[0][projAxes[0]] ) + - ( axisU[1][projAxes[0]] * axisV[0][projAxes[1]] ); - C = ( v1[projAxes[1]] * axisU[0][projAxes[0]] ) - - ( point[projAxes[1]] * axisU[0][projAxes[0]] ) - - ( v1[projAxes[0]] * axisU[0][projAxes[1]] ) + - ( point[projAxes[0]] * axisU[0][projAxes[1]] ); - negB = C - - ( v1[projAxes[1]] * axisU[1][projAxes[0]] ) + - ( point[projAxes[1]] * axisU[1][projAxes[0]] ) + - ( v1[projAxes[0]] * axisU[1][projAxes[1]] ) - - ( point[projAxes[0]] * axisU[1][projAxes[1]] ) + - ( axisU[0][projAxes[1]] * axisV[0][projAxes[0]] ) - - ( axisU[0][projAxes[0]] * axisV[0][projAxes[1]] ); - - if( ( A > -PIQ_PLANE_EPSILON ) && ( A < PIQ_PLANE_EPSILON ) ) - { - // shouldn't be here -- this should have been take care of in the "early out" -// Assert( 0 ); - - Vector vecUAvg, vecVAvg; - vecUAvg = ( axisUNorm[0] + axisUNorm[1] ) * 0.5f; - vecVAvg = ( axisVNorm[0] + axisVNorm[1] ) * 0.5f; - - float fLengthUAvg = ( lengthU[0] + lengthU[1] ) * 0.5f; - float fLengthVAvg = ( lengthV[0] + lengthV[1] ) * 0.5f; - - return QuadWithParallelEdges( v1, vecUAvg, fLengthUAvg, vecVAvg, fLengthVAvg, point, uv ); - -#if 0 - // legacy code -- kept here for completeness! - - // not a quadratic -- solve linearly - t = C / negB; - - // See (1.2) above - float ui = axisU[0][projAxes[0]] + t * ( axisU[1][projAxes[0]] - axisU[0][projAxes[0]] ); - if( FloatMakePositive( ui ) >= 1e-5 ) - { - // See (1.0) above - s = ( point[projAxes[0]] - v1[projAxes[0]] - axisV[0][projAxes[0]] * t ) / ui; - } -#endif - } - else - { - // (-b +/- sqrt( b^2 - 4ac )) / 2a - double discriminant = (negB*negB) - (4.0f * A * C); - if( discriminant < 0.0f ) - { - uv[0] = -99999.0f; - uv[1] = -99999.0f; - return BARY_QUADRATIC_NEGATIVE_DISCRIMINANT; - } - - double quad = sqrt( discriminant ); - double QPlus = ( negB + quad ) / ( 2.0f * A ); - double QMinus = ( negB - quad ) / ( 2.0f * A ); - - ResolveQuadratic( QPlus, QMinus, axisU[0], axisU[1], axisV[0], axisV[1], v1, point, projAxes[0], s, t ); - } - - if( !bFlipped ) - { - uv[0] = ( float )s; - uv[1] = ( float )t; - } - else - { - uv[0] = ( float )t; - uv[1] = ( float )s; - } - - // inside of the quad?? - if( ( uv[0] < 0.0f ) || ( uv[0] > 1.0f ) || ( uv[1] < 0.0f ) || ( uv[1] > 1.0f ) ) - return BARY_QUADRATIC_FALSE; - - return BARY_QUADRATIC_TRUE; - -#undef PIQ_TEXTURE_EPSILON -#undef PIQ_PLANE_EPSILON -} - - -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -void PointInQuadFromBarycentric( const Vector &v1, const Vector &v2, const Vector &v3, const Vector &v4, - const Vector2D &uv, Vector &point ) -{ - // - // Think of a quad with points v1, v2, v3, v4 and u, v line segments - // find the ray from v0 edge to v1 edge at v - // - Vector vPts[2]; - VectorLerp( v1, v4, uv[1], vPts[0] ); - VectorLerp( v2, v3, uv[1], vPts[1] ); - VectorLerp( vPts[0], vPts[1], uv[0], point ); -} - - -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -void TexCoordInQuadFromBarycentric( const Vector2D &v1, const Vector2D &v2, const Vector2D &v3, const Vector2D &v4, - const Vector2D &uv, Vector2D &texCoord ) -{ - // - // Think of a quad with points v1, v2, v3, v4 and u, v line segments - // find the ray from v0 edge to v1 edge at v - // - Vector2D vCoords[2]; - Vector2DLerp( v1, v4, uv[1], vCoords[0] ); - Vector2DLerp( v2, v3, uv[1], vCoords[1] ); - Vector2DLerp( vCoords[0], vCoords[1], uv[0], texCoord ); -} - - -//----------------------------------------------------------------------------- -// Compute point from barycentric specification -// Edge u goes from v0 to v1, edge v goes from v0 to v2 -//----------------------------------------------------------------------------- -void ComputePointFromBarycentric( const Vector& v0, const Vector& v1, const Vector& v2, - float u, float v, Vector& pt ) -{ - Vector edgeU, edgeV; - VectorSubtract( v1, v0, edgeU ); - VectorSubtract( v2, v0, edgeV ); - VectorMA( v0, u, edgeU, pt ); - VectorMA( pt, v, edgeV, pt ); -} - -void ComputePointFromBarycentric( const Vector2D& v0, const Vector2D& v1, const Vector2D& v2, - float u, float v, Vector2D& pt ) -{ - Vector2D edgeU, edgeV; - Vector2DSubtract( v1, v0, edgeU ); - Vector2DSubtract( v2, v0, edgeV ); - Vector2DMA( v0, u, edgeU, pt ); - Vector2DMA( pt, v, edgeV, pt ); -} - - -//----------------------------------------------------------------------------- -// Compute a matrix that has the correct orientation but which has an origin at -// the center of the bounds -//----------------------------------------------------------------------------- -static void ComputeCenterMatrix( const Vector& origin, const QAngle& angles, - const Vector& mins, const Vector& maxs, matrix3x4_t& matrix ) -{ - Vector centroid; - VectorAdd( mins, maxs, centroid ); - centroid *= 0.5f; - AngleMatrix( angles, matrix ); - - Vector worldCentroid; - VectorRotate( centroid, matrix, worldCentroid ); - worldCentroid += origin; - MatrixSetColumn( worldCentroid, 3, matrix ); -} - -static void ComputeCenterIMatrix( const Vector& origin, const QAngle& angles, - const Vector& mins, const Vector& maxs, matrix3x4_t& matrix ) -{ - Vector centroid; - VectorAdd( mins, maxs, centroid ); - centroid *= -0.5f; - AngleIMatrix( angles, matrix ); - - // For the translational component here, note that the origin in world space - // is T = R * C + O, (R = rotation matrix, C = centroid in local space, O = origin in world space) - // The IMatrix translation = - transpose(R) * T = -C - transpose(R) * 0 - Vector localOrigin; - VectorRotate( origin, matrix, localOrigin ); - centroid -= localOrigin; - MatrixSetColumn( centroid, 3, matrix ); -} - - -//----------------------------------------------------------------------------- -// Compute a matrix which is the absolute value of another -//----------------------------------------------------------------------------- -static inline void ComputeAbsMatrix( const matrix3x4_t& in, matrix3x4_t& out ) -{ - FloatBits(out[0][0]) = FloatAbsBits(in[0][0]); - FloatBits(out[0][1]) = FloatAbsBits(in[0][1]); - FloatBits(out[0][2]) = FloatAbsBits(in[0][2]); - FloatBits(out[1][0]) = FloatAbsBits(in[1][0]); - FloatBits(out[1][1]) = FloatAbsBits(in[1][1]); - FloatBits(out[1][2]) = FloatAbsBits(in[1][2]); - FloatBits(out[2][0]) = FloatAbsBits(in[2][0]); - FloatBits(out[2][1]) = FloatAbsBits(in[2][1]); - FloatBits(out[2][2]) = FloatAbsBits(in[2][2]); -} - - -//----------------------------------------------------------------------------- -// Compute a separating plane between two boxes (expensive!) -// Returns false if no separating plane exists -//----------------------------------------------------------------------------- -static bool ComputeSeparatingPlane( const matrix3x4_t &worldToBox1, const matrix3x4_t &box2ToWorld, - const Vector& box1Size, const Vector& box2Size, float tolerance, cplane_t* pPlane ) -{ - // The various separating planes can be either - // 1) A plane parallel to one of the box face planes - // 2) A plane parallel to the cross-product of an edge from each box - - // First, compute the basis of second box in the space of the first box - // NOTE: These basis place the origin at the centroid of each box! - matrix3x4_t box2ToBox1; - ConcatTransforms( worldToBox1, box2ToWorld, box2ToBox1 ); - - // We're going to be using the origin of box2 in the space of box1 alot, - // lets extract it from the matrix.... - Vector box2Origin; - MatrixGetColumn( box2ToBox1, 3, box2Origin ); - - // Next get the absolute values of these entries and store in absbox2ToBox1. - matrix3x4_t absBox2ToBox1; - ComputeAbsMatrix( box2ToBox1, absBox2ToBox1 ); - - // There are 15 tests to make. The first 3 involve trying planes parallel - // to the faces of the first box. - - // NOTE: The algorithm here involves finding the projections of the two boxes - // onto a particular line. If the projections on the line do not overlap, - // that means that there's a plane perpendicular to the line which separates - // the two boxes; and we've therefore found a separating plane. - - // The way we check for overlay is we find the projections of the two boxes - // onto the line, and add them up. We compare the sum with the projection - // of the relative center of box2 onto the same line. - - Vector tmp; - float boxProjectionSum; - float originProjection; - - // NOTE: For these guys, we're taking advantage of the fact that the ith - // row of the box2ToBox1 is the direction of the box1 (x,y,z)-axis - // transformed into the space of box2. - - // First side of box 1 - boxProjectionSum = box1Size.x + MatrixRowDotProduct( absBox2ToBox1, 0, box2Size ); - originProjection = FloatMakePositive( box2Origin.x ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - VectorCopy( worldToBox1[0], pPlane->normal.Base() ); - return true; - } - - // Second side of box 1 - boxProjectionSum = box1Size.y + MatrixRowDotProduct( absBox2ToBox1, 1, box2Size ); - originProjection = FloatMakePositive( box2Origin.y ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - VectorCopy( worldToBox1[1], pPlane->normal.Base() ); - return true; - } - - // Third side of box 1 - boxProjectionSum = box1Size.z + MatrixRowDotProduct( absBox2ToBox1, 2, box2Size ); - originProjection = FloatMakePositive( box2Origin.z ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - VectorCopy( worldToBox1[2], pPlane->normal.Base() ); - return true; - } - - // The next three involve checking splitting planes parallel to the - // faces of the second box. - - // NOTE: For these guys, we're taking advantage of the fact that the 0th - // column of the box2ToBox1 is the direction of the box2 x-axis - // transformed into the space of box1. - // Here, we're determining the distance of box2's center from box1's center - // by projecting it onto a line parallel to box2's axis - - // First side of box 2 - boxProjectionSum = box2Size.x + MatrixColumnDotProduct( absBox2ToBox1, 0, box1Size ); - originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 0, box2Origin ) ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 0, pPlane->normal ); - return true; - } - - // Second side of box 2 - boxProjectionSum = box2Size.y + MatrixColumnDotProduct( absBox2ToBox1, 1, box1Size ); - originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 1, box2Origin ) ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 1, pPlane->normal ); - return true; - } - - // Third side of box 2 - boxProjectionSum = box2Size.z + MatrixColumnDotProduct( absBox2ToBox1, 2, box1Size ); - originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 2, box2Origin ) ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 2, pPlane->normal ); - return true; - } - - // Next check the splitting planes which are orthogonal to the pairs - // of edges, one from box1 and one from box2. As only direction matters, - // there are 9 pairs since each box has 3 distinct edge directions. - - // Here, we take advantage of the fact that the edges from box 1 are all - // axis aligned; therefore the crossproducts are simplified. Let's walk through - // the example of b1e1 x b2e1: - - // In this example, the line to check is perpendicular to b1e1 + b2e2 - // we can compute this line by taking the cross-product: - // - // [ i j k ] - // [ 1 0 0 ] = - ez j + ey k = l1 - // [ ex ey ez ] - - // Where ex, ey, ez is the components of box2's x axis in the space of box 1, - // which is == to the 0th column of of box2toBox1 - - // The projection of box1 onto this line = the absolute dot product of the box size - // against the line, which = - // AbsDot( box1Size, l1 ) = abs( -ez * box1.y ) + abs( ey * box1.z ) - - // To compute the projection of box2 onto this line, we'll do it in the space of box 2 - // - // [ i j k ] - // [ fx fy fz ] = fz j - fy k = l2 - // [ 1 0 0 ] - - // Where fx, fy, fz is the components of box1's x axis in the space of box 2, - // which is == to the 0th row of of box2toBox1 - - // The projection of box2 onto this line = the absolute dot product of the box size - // against the line, which = - // AbsDot( box2Size, l2 ) = abs( fz * box2.y ) + abs ( fy * box2.z ) - - // The projection of the relative origin position on this line is done in the - // space of box 1: - // - // originProjection = DotProduct( <-ez j + ey k>, box2Origin ) = - // -ez * box2Origin.y + ey * box2Origin.z - - // NOTE: These checks can be bogus if both edges are parallel. The if - // checks at the beginning of each block are designed to catch that case - - // b1e1 x b2e1 - if ( absBox2ToBox1[0][0] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.y * absBox2ToBox1[2][0] + box1Size.z * absBox2ToBox1[1][0] + - box2Size.y * absBox2ToBox1[0][2] + box2Size.z * absBox2ToBox1[0][1]; - originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][0] + box2Origin.z * box2ToBox1[1][0] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 0, tmp ); - CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e1 x b2e2 - if ( absBox2ToBox1[0][1] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.y * absBox2ToBox1[2][1] + box1Size.z * absBox2ToBox1[1][1] + - box2Size.x * absBox2ToBox1[0][2] + box2Size.z * absBox2ToBox1[0][0]; - originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][1] + box2Origin.z * box2ToBox1[1][1] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 1, tmp ); - CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e1 x b2e3 - if ( absBox2ToBox1[0][2] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.y * absBox2ToBox1[2][2] + box1Size.z * absBox2ToBox1[1][2] + - box2Size.x * absBox2ToBox1[0][1] + box2Size.y * absBox2ToBox1[0][0]; - originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][2] + box2Origin.z * box2ToBox1[1][2] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 2, tmp ); - CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e2 x b2e1 - if ( absBox2ToBox1[1][0] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.x * absBox2ToBox1[2][0] + box1Size.z * absBox2ToBox1[0][0] + - box2Size.y * absBox2ToBox1[1][2] + box2Size.z * absBox2ToBox1[1][1]; - originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][0] - box2Origin.z * box2ToBox1[0][0] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 0, tmp ); - CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e2 x b2e2 - if ( absBox2ToBox1[1][1] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.x * absBox2ToBox1[2][1] + box1Size.z * absBox2ToBox1[0][1] + - box2Size.x * absBox2ToBox1[1][2] + box2Size.z * absBox2ToBox1[1][0]; - originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][1] - box2Origin.z * box2ToBox1[0][1] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 1, tmp ); - CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e2 x b2e3 - if ( absBox2ToBox1[1][2] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.x * absBox2ToBox1[2][2] + box1Size.z * absBox2ToBox1[0][2] + - box2Size.x * absBox2ToBox1[1][1] + box2Size.y * absBox2ToBox1[1][0]; - originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][2] - box2Origin.z * box2ToBox1[0][2] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 2, tmp ); - CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e3 x b2e1 - if ( absBox2ToBox1[2][0] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.x * absBox2ToBox1[1][0] + box1Size.y * absBox2ToBox1[0][0] + - box2Size.y * absBox2ToBox1[2][2] + box2Size.z * absBox2ToBox1[2][1]; - originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][0] + box2Origin.y * box2ToBox1[0][0] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 0, tmp ); - CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e3 x b2e2 - if ( absBox2ToBox1[2][1] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.x * absBox2ToBox1[1][1] + box1Size.y * absBox2ToBox1[0][1] + - box2Size.x * absBox2ToBox1[2][2] + box2Size.z * absBox2ToBox1[2][0]; - originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][1] + box2Origin.y * box2ToBox1[0][1] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 1, tmp ); - CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - - // b1e3 x b2e3 - if ( absBox2ToBox1[2][2] < 1.0f - 1e-3f ) - { - boxProjectionSum = - box1Size.x * absBox2ToBox1[1][2] + box1Size.y * absBox2ToBox1[0][2] + - box2Size.x * absBox2ToBox1[2][1] + box2Size.y * absBox2ToBox1[2][0]; - originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][2] + box2Origin.y * box2ToBox1[0][2] ) + tolerance; - if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) - { - MatrixGetColumn( box2ToWorld, 2, tmp ); - CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); - return true; - } - } - return false; -} - - -//----------------------------------------------------------------------------- -// Compute a separating plane between two boxes (expensive!) -// Returns false if no separating plane exists -//----------------------------------------------------------------------------- -bool ComputeSeparatingPlane( const Vector& org1, const QAngle& angles1, const Vector& min1, const Vector& max1, - const Vector& org2, const QAngle& angles2, const Vector& min2, const Vector& max2, - float tolerance, cplane_t* pPlane ) -{ - matrix3x4_t worldToBox1, box2ToWorld; - ComputeCenterIMatrix( org1, angles1, min1, max1, worldToBox1 ); - ComputeCenterMatrix( org2, angles2, min2, max2, box2ToWorld ); - - // Then compute the size of the two boxes - Vector box1Size, box2Size; - VectorSubtract( max1, min1, box1Size ); - VectorSubtract( max2, min2, box2Size ); - box1Size *= 0.5f; - box2Size *= 0.5f; - - return ComputeSeparatingPlane( worldToBox1, box2ToWorld, box1Size, box2Size, tolerance, pPlane ); -} - - -//----------------------------------------------------------------------------- -// Swept OBB test -//----------------------------------------------------------------------------- -bool IsRayIntersectingOBB( const Ray_t &ray, const Vector& org, const QAngle& angles, - const Vector& mins, const Vector& maxs ) -{ - if ( angles == vec3_angle ) - { - Vector vecWorldMins, vecWorldMaxs; - VectorAdd( org, mins, vecWorldMins ); - VectorAdd( org, maxs, vecWorldMaxs ); - return IsBoxIntersectingRay( vecWorldMins, vecWorldMaxs, ray ); - } - - if ( ray.m_IsRay ) - { - matrix3x4_t worldToBox; - AngleIMatrix( angles, org, worldToBox ); - - Ray_t rotatedRay; - VectorTransform( ray.m_Start, worldToBox, rotatedRay.m_Start ); - VectorRotate( ray.m_Delta, worldToBox, rotatedRay.m_Delta ); - rotatedRay.m_StartOffset = vec3_origin; - rotatedRay.m_Extents = vec3_origin; - rotatedRay.m_IsRay = ray.m_IsRay; - rotatedRay.m_IsSwept = ray.m_IsSwept; - - return IsBoxIntersectingRay( mins, maxs, rotatedRay ); - } - - if ( !ray.m_IsSwept ) - { - cplane_t plane; - return ComputeSeparatingPlane( ray.m_Start, vec3_angle, -ray.m_Extents, ray.m_Extents, - org, angles, mins, maxs, 0.0f, &plane ) == false; - } - - // NOTE: See the comments in ComputeSeparatingPlane to understand this math - - // First, compute the basis of box in the space of the ray - // NOTE: These basis place the origin at the centroid of each box! - matrix3x4_t worldToBox1, box2ToWorld; - ComputeCenterMatrix( org, angles, mins, maxs, box2ToWorld ); - - // Find the center + extents of an AABB surrounding the ray - Vector vecRayCenter; - VectorMA( ray.m_Start, 0.5, ray.m_Delta, vecRayCenter ); - vecRayCenter *= -1.0f; - SetIdentityMatrix( worldToBox1 ); - MatrixSetColumn( vecRayCenter, 3, worldToBox1 ); - - Vector box1Size; - box1Size.x = ray.m_Extents.x + FloatMakePositive( ray.m_Delta.x ) * 0.5f; - box1Size.y = ray.m_Extents.y + FloatMakePositive( ray.m_Delta.y ) * 0.5f; - box1Size.z = ray.m_Extents.z + FloatMakePositive( ray.m_Delta.z ) * 0.5f; - - // Then compute the size of the box - Vector box2Size; - VectorSubtract( maxs, mins, box2Size ); - box2Size *= 0.5f; - - // Do an OBB test of the box with the AABB surrounding the ray - cplane_t plane; - if ( ComputeSeparatingPlane( worldToBox1, box2ToWorld, box1Size, box2Size, 0.0f, &plane ) ) - return false; - - // Now deal with the planes which are the cross products of the ray sweep direction vs box edges - Vector vecRayDirection = ray.m_Delta; - VectorNormalize( vecRayDirection ); - - // Need a vector between ray center vs box center measured in the space of the ray (world) - Vector vecCenterDelta; - vecCenterDelta.x = box2ToWorld[0][3] - ray.m_Start.x; - vecCenterDelta.y = box2ToWorld[1][3] - ray.m_Start.y; - vecCenterDelta.z = box2ToWorld[2][3] - ray.m_Start.z; - - // Rotate the ray direction into the space of the OBB - Vector vecAbsRayDirBox2; - VectorIRotate( vecRayDirection, box2ToWorld, vecAbsRayDirBox2 ); - - // Make abs versions of the ray in world space + ray in box2 space - VectorAbs( vecAbsRayDirBox2, vecAbsRayDirBox2 ); - - // Now do the work for the planes which are perpendicular to the edges of the AABB - // and the sweep direction edges... - - // In this example, the line to check is perpendicular to box edge x + ray delta - // we can compute this line by taking the cross-product: - // - // [ i j k ] - // [ 1 0 0 ] = - dz j + dy k = l1 - // [ dx dy dz ] - - // Where dx, dy, dz is the ray delta (normalized) - - // The projection of the box onto this line = the absolute dot product of the box size - // against the line, which = - // AbsDot( vecBoxHalfDiagonal, l1 ) = abs( -dz * vecBoxHalfDiagonal.y ) + abs( dy * vecBoxHalfDiagonal.z ) - - // Because the plane contains the sweep direction, the sweep will produce - // no extra projection onto the line normal to the plane. - // Therefore all we need to do is project the ray extents onto this line also: - // AbsDot( ray.m_Extents, l1 ) = abs( -dz * ray.m_Extents.y ) + abs( dy * ray.m_Extents.z ) - - Vector vecPlaneNormal; - - // box x x ray delta - CrossProduct( vecRayDirection, Vector( box2ToWorld[0][0], box2ToWorld[1][0], box2ToWorld[2][0] ), vecPlaneNormal ); - float flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) ); - float flBoxProjectionSum = - vecAbsRayDirBox2.z * box2Size.y + vecAbsRayDirBox2.y * box2Size.z + - DotProductAbs( vecPlaneNormal, ray.m_Extents ); - if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) ) - return false; - - // box y x ray delta - CrossProduct( vecRayDirection, Vector( box2ToWorld[0][1], box2ToWorld[1][1], box2ToWorld[2][1] ), vecPlaneNormal ); - flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) ); - flBoxProjectionSum = - vecAbsRayDirBox2.z * box2Size.x + vecAbsRayDirBox2.x * box2Size.z + - DotProductAbs( vecPlaneNormal, ray.m_Extents ); - if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) ) - return false; - - // box z x ray delta - CrossProduct( vecRayDirection, Vector( box2ToWorld[0][2], box2ToWorld[1][2], box2ToWorld[2][2] ), vecPlaneNormal ); - flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) ); - flBoxProjectionSum = - vecAbsRayDirBox2.y * box2Size.x + vecAbsRayDirBox2.x * box2Size.y + - DotProductAbs( vecPlaneNormal, ray.m_Extents ); - if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) ) - return false; - - return true; -} - -//-------------------------------------------------------------------------- -// Purpose: -// -// NOTE: -// triangle points are given in clockwise order (aabb-triangle test) -// -// 1 edge0 = 1 - 0 -// | \ edge1 = 2 - 1 -// | \ edge2 = 0 - 2 -// | \ . -// | \ . -// 0-----2 . -// -//-------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- -// Purpose: find the minima and maxima of the 3 given values -//----------------------------------------------------------------------------- -inline void FindMinMax( float v1, float v2, float v3, float &min, float &max ) -{ - min = max = v1; - if ( v2 < min ) { min = v2; } - if ( v2 > max ) { max = v2; } - if ( v3 < min ) { min = v3; } - if ( v3 > max ) { max = v3; } -} - -//----------------------------------------------------------------------------- -// Purpose: -//----------------------------------------------------------------------------- -inline bool AxisTestEdgeCrossX2( float flEdgeZ, float flEdgeY, float flAbsEdgeZ, float flAbsEdgeY, - const Vector &p1, const Vector &p3, const Vector &vecExtents, - float flTolerance ) -{ - // Cross Product( axialX(1,0,0) x edge ): x = 0.0f, y = edge.z, z = -edge.y - // Triangle Point Distances: dist(x) = normal.y * pt(x).y + normal.z * pt(x).z - float flDist1 = flEdgeZ * p1.y - flEdgeY * p1.z; - float flDist3 = flEdgeZ * p3.y - flEdgeY * p3.z; - - // Extents are symmetric: dist = abs( normal.y ) * extents.y + abs( normal.z ) * extents.z - float flDistBox = flAbsEdgeZ * vecExtents.y + flAbsEdgeY * vecExtents.z; - - // Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). - if ( flDist1 < flDist3 ) - { - if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) - return false; - } - else - { - if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) - return false; - } - - return true; -} - -//-------------------------------------------------------------------------- -// Purpose: -//-------------------------------------------------------------------------- -inline bool AxisTestEdgeCrossX3( float flEdgeZ, float flEdgeY, float flAbsEdgeZ, float flAbsEdgeY, - const Vector &p1, const Vector &p2, const Vector &vecExtents, - float flTolerance ) -{ - // Cross Product( axialX(1,0,0) x edge ): x = 0.0f, y = edge.z, z = -edge.y - // Triangle Point Distances: dist(x) = normal.y * pt(x).y + normal.z * pt(x).z - float flDist1 = flEdgeZ * p1.y - flEdgeY * p1.z; - float flDist2 = flEdgeZ * p2.y - flEdgeY * p2.z; - - // Extents are symmetric: dist = abs( normal.y ) * extents.y + abs( normal.z ) * extents.z - float flDistBox = flAbsEdgeZ * vecExtents.y + flAbsEdgeY * vecExtents.z; - - // Either dist1, dist2 is the closest point to the box, determine which and test of overlap with box(AABB). - if ( flDist1 < flDist2 ) - { - if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) ) - return false; - } - else - { - if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) - return false; - } - - return true; -} - -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -inline bool AxisTestEdgeCrossY2( float flEdgeZ, float flEdgeX, float flAbsEdgeZ, float flAbsEdgeX, - const Vector &p1, const Vector &p3, const Vector &vecExtents, - float flTolerance ) -{ - // Cross Product( axialY(0,1,0) x edge ): x = -edge.z, y = 0.0f, z = edge.x - // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.z * pt(x).z - float flDist1 = -flEdgeZ * p1.x + flEdgeX * p1.z; - float flDist3 = -flEdgeZ * p3.x + flEdgeX * p3.z; - - // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.z ) * extents.z - float flDistBox = flAbsEdgeZ * vecExtents.x + flAbsEdgeX * vecExtents.z; - - // Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). - if ( flDist1 < flDist3 ) - { - if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) - return false; - } - else - { - if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) - return false; - } - - return true; -} - -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -inline bool AxisTestEdgeCrossY3( float flEdgeZ, float flEdgeX, float flAbsEdgeZ, float flAbsEdgeX, - const Vector &p1, const Vector &p2, const Vector &vecExtents, - float flTolerance ) -{ - // Cross Product( axialY(0,1,0) x edge ): x = -edge.z, y = 0.0f, z = edge.x - // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.z * pt(x).z - float flDist1 = -flEdgeZ * p1.x + flEdgeX * p1.z; - float flDist2 = -flEdgeZ * p2.x + flEdgeX * p2.z; - - // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.z ) * extents.z - float flDistBox = flAbsEdgeZ * vecExtents.x + flAbsEdgeX * vecExtents.z; - - // Either dist1, dist2 is the closest point to the box, determine which and test of overlap with box(AABB). - if ( flDist1 < flDist2 ) - { - if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) ) - return false; - } - else - { - if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) - return false; - } - - return true; -} - -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -inline bool AxisTestEdgeCrossZ1( float flEdgeY, float flEdgeX, float flAbsEdgeY, float flAbsEdgeX, - const Vector &p2, const Vector &p3, const Vector &vecExtents, - float flTolerance ) -{ - // Cross Product( axialZ(0,0,1) x edge ): x = edge.y, y = -edge.x, z = 0.0f - // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.y * pt(x).y - float flDist2 = flEdgeY * p2.x - flEdgeX * p2.y; - float flDist3 = flEdgeY * p3.x - flEdgeX * p3.y; - - // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.y ) * extents.y - float flDistBox = flAbsEdgeY * vecExtents.x + flAbsEdgeX * vecExtents.y; - - // Either dist2, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). - if ( flDist3 < flDist2 ) - { - if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) ) - return false; - } - else - { - if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) - return false; - } - - return true; -} - -//-------------------------------------------------------------------------- -//-------------------------------------------------------------------------- -inline bool AxisTestEdgeCrossZ2( float flEdgeY, float flEdgeX, float flAbsEdgeY, float flAbsEdgeX, - const Vector &p1, const Vector &p3, const Vector &vecExtents, - float flTolerance ) -{ - // Cross Product( axialZ(0,0,1) x edge ): x = edge.y, y = -edge.x, z = 0.0f - // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.y * pt(x).y - float flDist1 = flEdgeY * p1.x - flEdgeX * p1.y; - float flDist3 = flEdgeY * p3.x - flEdgeX * p3.y; - - // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.y ) * extents.y - float flDistBox = flAbsEdgeY * vecExtents.x + flAbsEdgeX * vecExtents.y; - - // Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). - if ( flDist1 < flDist3 ) - { - if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) - return false; - } - else - { - if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) - return false; - } - - return true; -} - -//----------------------------------------------------------------------------- -// Purpose: Test for an intersection (overlap) between an axial-aligned bounding -// box (AABB) and a triangle. -// -// Using the "Separating-Axis Theorem" to test for intersections between -// a triangle and an axial-aligned bounding box (AABB). -// 1. 3 Axis Planes - x, y, z -// 2. 9 Edge Planes Tests - the 3 edges of the triangle crossed with all 3 axial -// planes (x, y, z) -// 3. 1 Face Plane - the triangle plane (cplane_t plane below) -// Output: false = separating axis (no intersection) -// true = intersection -//----------------------------------------------------------------------------- -bool IsBoxIntersectingTriangle( const Vector &vecBoxCenter, const Vector &vecBoxExtents, - const Vector &v1, const Vector &v2, const Vector &v3, - const cplane_t &plane, float flTolerance ) -{ - // Test the axial planes (x,y,z) against the min, max of the triangle. - float flMin, flMax; - Vector p1, p2, p3; - - // x plane - p1.x = v1.x - vecBoxCenter.x; - p2.x = v2.x - vecBoxCenter.x; - p3.x = v3.x - vecBoxCenter.x; - FindMinMax( p1.x, p2.x, p3.x, flMin, flMax ); - if ( ( flMin > ( vecBoxExtents.x + flTolerance ) ) || ( flMax < -( vecBoxExtents.x + flTolerance ) ) ) - return false; - - // y plane - p1.y = v1.y - vecBoxCenter.y; - p2.y = v2.y - vecBoxCenter.y; - p3.y = v3.y - vecBoxCenter.y; - FindMinMax( p1.y, p2.y, p3.y, flMin, flMax ); - if ( ( flMin > ( vecBoxExtents.y + flTolerance ) ) || ( flMax < -( vecBoxExtents.y + flTolerance ) ) ) - return false; - - // z plane - p1.z = v1.z - vecBoxCenter.z; - p2.z = v2.z - vecBoxCenter.z; - p3.z = v3.z - vecBoxCenter.z; - FindMinMax( p1.z, p2.z, p3.z, flMin, flMax ); - if ( ( flMin > ( vecBoxExtents.z + flTolerance ) ) || ( flMax < -( vecBoxExtents.z + flTolerance ) ) ) - return false; - - // Test the 9 edge cases. - Vector vecEdge, vecAbsEdge; - - // edge 0 (cross x,y,z) - vecEdge = p2 - p1; - vecAbsEdge.y = FloatMakePositive( vecEdge.y ); - vecAbsEdge.z = FloatMakePositive( vecEdge.z ); - if ( !AxisTestEdgeCrossX2( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p3, vecBoxExtents, flTolerance ) ) - return false; - - vecAbsEdge.x = FloatMakePositive( vecEdge.x ); - if ( !AxisTestEdgeCrossY2( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p3, vecBoxExtents, flTolerance ) ) - return false; - - if ( !AxisTestEdgeCrossZ1( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p2, p3, vecBoxExtents, flTolerance ) ) - return false; - - // edge 1 (cross x,y,z) - vecEdge = p3 - p2; - vecAbsEdge.y = FloatMakePositive( vecEdge.y ); - vecAbsEdge.z = FloatMakePositive( vecEdge.z ); - if ( !AxisTestEdgeCrossX2( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p2, vecBoxExtents, flTolerance ) ) - return false; - - vecAbsEdge.x = FloatMakePositive( vecEdge.x ); - if ( !AxisTestEdgeCrossY2( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p2, vecBoxExtents, flTolerance ) ) - return false; - - if ( !AxisTestEdgeCrossZ2( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p1, p3, vecBoxExtents, flTolerance ) ) - return false; - - // edge 2 (cross x,y,z) - vecEdge = p1 - p3; - vecAbsEdge.y = FloatMakePositive( vecEdge.y ); - vecAbsEdge.z = FloatMakePositive( vecEdge.z ); - if ( !AxisTestEdgeCrossX3( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p2, vecBoxExtents, flTolerance ) ) - return false; - - vecAbsEdge.x = FloatMakePositive( vecEdge.x ); - if ( !AxisTestEdgeCrossY3( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p2, vecBoxExtents, flTolerance ) ) - return false; - - if ( !AxisTestEdgeCrossZ1( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p2, p3, vecBoxExtents, flTolerance ) ) - return false; - - // Test against the triangle face plane. - Vector vecMin, vecMax; - VectorSubtract( vecBoxCenter, vecBoxExtents, vecMin ); - VectorAdd( vecBoxCenter, vecBoxExtents, vecMax ); - if ( BoxOnPlaneSide( vecMin, vecMax, &plane ) != 3 ) - return false; - - return true; -} - -// NOTE: JAY: This is untested code based on Real-time Collision Detection by Ericson -#if 0 -Vector CalcClosestPointOnTriangle( const Vector &P, const Vector &v0, const Vector &v1, const Vector &v2 ) -{ - Vector e0 = v1 - v0; - Vector e1 = v2 - v0; - Vector p0 = P - v0; - - // voronoi region of v0 - float d1 = DotProduct( e0, p0 ); - float d2 = DotProduct( e1, p0 ); - if (d1 <= 0.0f && d2 <= 0.0f) - return v0; - - // voronoi region of v1 - Vector p1 = P - v1; - float d3 = DotProduct( e0, p1 ); - float d4 = DotProduct( e1, p1 ); - if (d3 >=0.0f && d4 <= d3) - return v1; - - // voronoi region of e0 (v0-v1) - float ve2 = d1*d4 - d3*d2; - if ( ve2 <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) - { - float v = d1 / (d1-d3); - return v0 + v * e0; - } - // voronoi region of v2 - Vector p2 = P - v2; - float d5 = DotProduct( e0, p2 ); - float d6 = DotProduct( e1, p2 ); - if (d6 >= 0.0f && d5 <= d6) - return v2; - // voronoi region of e1 - float ve1 = d5*d2 - d1*d6; - if (ve1 <= 0.0f && d2 >= 0.0f && d6 >= 0.0f) - { - float w = d2 / (d2-d6); - return v0 + w * e1; - } - // voronoi region on e2 - float ve0 = d3*d6 - d5*d4; - if ( ve0 <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) - { - float w = (d4-d3)/((d4-d3) + (d5-d6)); - return v1 + w * (v2-v1); - } - // voronoi region of v0v1v2 triangle - float denom = 1.0f / (ve0+ve1+ve2); - float v = ve1*denom; - float w = ve2 * denom; - return v0 + e0 * v + e1 * w; -} -#endif - - -bool OBBHasFullyContainedIntersectionWithQuad( const Vector &vOBBExtent1_Scaled, const Vector &vOBBExtent2_Scaled, const Vector &vOBBExtent3_Scaled, const Vector &ptOBBCenter, - const Vector &vQuadNormal, float fQuadPlaneDist, const Vector &ptQuadCenter, - const Vector &vQuadExtent1_Normalized, float fQuadExtent1Length, - const Vector &vQuadExtent2_Normalized, float fQuadExtent2Length ) -{ - Vector ptOBB[8]; //this specific ordering helps us web out from a point to its 3 connecting points with some bit math (most importantly, no if's) - ptOBB[0] = ptOBBCenter - vOBBExtent1_Scaled - vOBBExtent2_Scaled - vOBBExtent3_Scaled; - ptOBB[1] = ptOBBCenter - vOBBExtent1_Scaled - vOBBExtent2_Scaled + vOBBExtent3_Scaled; - ptOBB[2] = ptOBBCenter - vOBBExtent1_Scaled + vOBBExtent2_Scaled + vOBBExtent3_Scaled; - ptOBB[3] = ptOBBCenter - vOBBExtent1_Scaled + vOBBExtent2_Scaled - vOBBExtent3_Scaled; - ptOBB[4] = ptOBBCenter + vOBBExtent1_Scaled - vOBBExtent2_Scaled - vOBBExtent3_Scaled; - ptOBB[5] = ptOBBCenter + vOBBExtent1_Scaled - vOBBExtent2_Scaled + vOBBExtent3_Scaled; - ptOBB[6] = ptOBBCenter + vOBBExtent1_Scaled + vOBBExtent2_Scaled + vOBBExtent3_Scaled; - ptOBB[7] = ptOBBCenter + vOBBExtent1_Scaled + vOBBExtent2_Scaled - vOBBExtent3_Scaled; - - float fDists[8]; - for( int i = 0; i != 8; ++i ) - fDists[i] = vQuadNormal.Dot( ptOBB[i] ) - fQuadPlaneDist; - - int iSides[8]; - int iSideMask = 0; - for( int i = 0; i != 8; ++i ) - { - if( fDists[i] > 0.0f ) - { - iSides[i] = 1; - iSideMask |= 1; - } - else - { - iSides[i] = 2; - iSideMask |= 2; - } - } - - if( iSideMask != 3 ) //points reside entirely on one side of the quad's plane - return false; - - Vector ptPlaneIntersections[12]; //only have 12 lines, can only possibly generate 12 split points - int iPlaneIntersectionsCount = 0; - - for( int i = 0; i != 8; ++i ) - { - if( iSides[i] == 2 ) //point behind the plane - { - int iAxisCrossings[3]; - iAxisCrossings[0] = i ^ 4; //upper 4 vs lower 4 crosses vOBBExtent1 axis - iAxisCrossings[1] = ((i + 1) & 3) + (i & 4); //cycle to the next element while staying within the upper 4 or lower 4, this will cross either vOBBExtent2 or vOBBExtent3 axis, we don't care which - iAxisCrossings[2] = ((i - 1) & 3) + (i & 4); //cylce to the previous element while staying within the upper 4 or lower 4, this will cross the axis iAxisCrossings[1] didn't cross - - for( int j = 0; j != 3; ++j ) - { - if( iSides[iAxisCrossings[j]] == 1 ) //point in front of the plane - { - //line between ptOBB[i] and ptOBB[iAxisCrossings[j]] intersects the plane, generate a point at the intersection for further testing - float fTotalDist = fDists[iAxisCrossings[j]] - fDists[i]; //remember that fDists[i] is a negative value - ptPlaneIntersections[iPlaneIntersectionsCount] = (ptOBB[iAxisCrossings[j]] * (-fDists[i]/fTotalDist)) + (ptOBB[i] * (fDists[iAxisCrossings[j]]/fTotalDist)); - - Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane - - ++iPlaneIntersectionsCount; - } - } - } - } - - Assert( iPlaneIntersectionsCount != 0 ); - - for( int i = 0; i != iPlaneIntersectionsCount; ++i ) - { - //these points are guaranteed to be on the plane, now just check to see if they're within the quad's extents - Vector vToPointFromQuadCenter = ptPlaneIntersections[i] - ptQuadCenter; - - float fExt1Dist = vQuadExtent1_Normalized.Dot( vToPointFromQuadCenter ); - if( fabs( fExt1Dist ) > fQuadExtent1Length ) - return false; //point is outside boundaries - - //vToPointFromQuadCenter -= vQuadExtent1_Normalized * fExt1Dist; //to handle diamond shaped quads - - float fExt2Dist = vQuadExtent2_Normalized.Dot( vToPointFromQuadCenter ); - if( fabs( fExt2Dist ) > fQuadExtent2Length ) - return false; //point is outside boundaries - } - - return true; //there were lines crossing the quad plane, and every line crossing that plane had its intersection with the plane within the quad's boundaries -} - -//----------------------------------------------------------------------------- -// Compute if the Ray intersects the quad plane, and whether the entire -// Ray/Quad intersection is contained within the quad itself -// -// False if no intersection exists, or if part of the intersection is -// outside the quad's extents -//----------------------------------------------------------------------------- -bool RayHasFullyContainedIntersectionWithQuad( const Ray_t &ray, - const Vector &vQuadNormal, float fQuadPlaneDist, const Vector &ptQuadCenter, - const Vector &vQuadExtent1_Normalized, float fQuadExtent1Length, - const Vector &vQuadExtent2_Normalized, float fQuadExtent2Length ) -{ - Vector ptPlaneIntersections[(12 + 12 + 8)]; //absolute max possible: 12 lines to connect the start box, 12 more to connect the end box, 8 to connect the boxes to eachother - - //8 points to make an AABB, 8 lines to connect each point from it's start to end point along the ray, 8 possible intersections - int iPlaneIntersectionsCount = 0; - - if( ray.m_IsRay ) - { - //just 1 line - if( ray.m_IsSwept ) - { - Vector ptEndPoints[2]; - ptEndPoints[0] = ray.m_Start; - ptEndPoints[1] = ptEndPoints[0] + ray.m_Delta; - - int i; - float fDists[2]; - for( i = 0; i != 2; ++i ) - fDists[i] = vQuadNormal.Dot( ptEndPoints[i] ) - fQuadPlaneDist; - - for( i = 0; i != 2; ++i ) - { - if( fDists[i] <= 0.0f ) - { - int j = 1-i; - if( fDists[j] >= 0.0f ) - { - float fInvTotalDist = 1.0f / (fDists[j] - fDists[i]); //fDists[i] <= 0, ray is swept so no chance that the denom was 0 - ptPlaneIntersections[0] = (ptEndPoints[i] * (fDists[j] * fInvTotalDist)) - (ptEndPoints[j] * (fDists[i] * fInvTotalDist)); //fDists[i] <= 0 - Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane - iPlaneIntersectionsCount = 1; - } - else - { - return false; - } - break; - } - } - - if( i == 2 ) - return false; - } - else //not swept, so this is actually a point on quad question - { - if( fabs( vQuadNormal.Dot( ray.m_Start ) - fQuadPlaneDist ) < 1e-6 ) - { - ptPlaneIntersections[0] = ray.m_Start; - iPlaneIntersectionsCount = 1; - } - else - { - return false; - } - } - } - else - { - Vector ptEndPoints[2][8]; - //this specific ordering helps us web out from a point to its 3 connecting points with some bit math (most importantly, no if's) - ptEndPoints[0][0] = ray.m_Start; ptEndPoints[0][0].x -= ray.m_Extents.x; ptEndPoints[0][0].y -= ray.m_Extents.y; ptEndPoints[0][0].z -= ray.m_Extents.z; - ptEndPoints[0][1] = ray.m_Start; ptEndPoints[0][1].x -= ray.m_Extents.x; ptEndPoints[0][1].y -= ray.m_Extents.y; ptEndPoints[0][1].z += ray.m_Extents.z; - ptEndPoints[0][2] = ray.m_Start; ptEndPoints[0][2].x -= ray.m_Extents.x; ptEndPoints[0][2].y += ray.m_Extents.y; ptEndPoints[0][2].z += ray.m_Extents.z; - ptEndPoints[0][3] = ray.m_Start; ptEndPoints[0][3].x -= ray.m_Extents.x; ptEndPoints[0][3].y += ray.m_Extents.y; ptEndPoints[0][3].z -= ray.m_Extents.z; - ptEndPoints[0][4] = ray.m_Start; ptEndPoints[0][4].x += ray.m_Extents.x; ptEndPoints[0][4].y -= ray.m_Extents.y; ptEndPoints[0][4].z -= ray.m_Extents.z; - ptEndPoints[0][5] = ray.m_Start; ptEndPoints[0][5].x += ray.m_Extents.x; ptEndPoints[0][5].y -= ray.m_Extents.y; ptEndPoints[0][5].z += ray.m_Extents.z; - ptEndPoints[0][6] = ray.m_Start; ptEndPoints[0][6].x += ray.m_Extents.x; ptEndPoints[0][6].y += ray.m_Extents.y; ptEndPoints[0][6].z += ray.m_Extents.z; - ptEndPoints[0][7] = ray.m_Start; ptEndPoints[0][7].x += ray.m_Extents.x; ptEndPoints[0][7].y += ray.m_Extents.y; ptEndPoints[0][7].z -= ray.m_Extents.z; - - float fDists[2][8]; - int iSides[2][8]; - int iSideMask[2] = { 0, 0 }; - for( int i = 0; i != 8; ++i ) - { - fDists[0][i] = vQuadNormal.Dot( ptEndPoints[0][i] ) - fQuadPlaneDist; - if( fDists[0][i] > 0.0f ) - { - iSides[0][i] = 1; - iSideMask[0] |= 1; - } - else - { - iSides[0][i] = 2; - iSideMask[0] |= 2; - } - } - - if( ray.m_IsSwept ) - { - for( int i = 0; i != 8; ++i ) - ptEndPoints[1][i] = ptEndPoints[0][i] + ray.m_Delta; - - for( int i = 0; i != 8; ++i ) - { - fDists[1][i] = vQuadNormal.Dot( ptEndPoints[1][i] ) - fQuadPlaneDist; - if( fDists[1][i] > 0.0f ) - { - iSides[1][i] = 1; - iSideMask[1] |= 1; - } - else - { - iSides[1][i] = 2; - iSideMask[1] |= 2; - } - } - } - - if( (iSideMask[0] | iSideMask[1]) != 3 ) - { - //Assert( (iSideMask[0] | iSideMask[1]) != 2 ); - return false; //all points resides entirely on one side of the quad - } - - - //generate intersections for boxes split by the plane at either end of the ray - for( int k = 0; k != 2; ++k ) - { - if( iSideMask[k] == 3 ) //box is split by the plane - { - for( int i = 0; i != 8; ++i ) - { - if( iSides[k][i] == 2 ) //point behind the plane - { - int iAxisCrossings[3]; - iAxisCrossings[0] = i ^ 4; //upper 4 vs lower 4 crosses X axis - iAxisCrossings[1] = ((i + 1) & 3) + (i & 4); //cycle to the next element while staying within the upper 4 or lower 4, this will cross either Y or Z axis, we don't care which - iAxisCrossings[2] = ((i - 1) & 3) + (i & 4); //cylce to the previous element while staying within the upper 4 or lower 4, this will cross the axis iAxisCrossings[1] didn't cross - - for( int j = 0; j != 3; ++j ) - { - if( iSides[k][iAxisCrossings[j]] == 1 ) //point in front of the plane - { - //line between ptEndPoints[i] and ptEndPoints[iAxisCrossings[j]] intersects the plane, generate a point at the intersection for further testing - float fInvTotalDist = 1.0f / (fDists[k][iAxisCrossings[j]] - fDists[k][i]); //remember that fDists[k][i] is a negative value - ptPlaneIntersections[iPlaneIntersectionsCount] = (ptEndPoints[k][iAxisCrossings[j]] * (-fDists[k][i] * fInvTotalDist)) + (ptEndPoints[k][i] * (fDists[k][iAxisCrossings[j]] * fInvTotalDist)); - - Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane - - ++iPlaneIntersectionsCount; - } - } - } - } - } - } - - if( ray.m_IsSwept ) - { - for( int i = 0; i != 8; ++i ) - { - if( iSides[0][i] != iSides[1][i] ) - { - int iPosSide, iNegSide; - if( iSides[0][i] == 1 ) - { - iPosSide = 0; - iNegSide = 1; - } - else - { - iPosSide = 1; - iNegSide = 0; - } - - Assert( (fDists[iPosSide][i] >= 0.0f) && (fDists[iNegSide][i] <= 0.0f) ); - - float fInvTotalDist = 1.0f / (fDists[iPosSide][i] - fDists[iNegSide][i]); //remember that fDists[iNegSide][i] is a negative value - ptPlaneIntersections[iPlaneIntersectionsCount] = (ptEndPoints[iPosSide][i] * (-fDists[iNegSide][i] * fInvTotalDist)) + (ptEndPoints[iNegSide][i] * (fDists[iPosSide][i] * fInvTotalDist)); - - Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane - - ++iPlaneIntersectionsCount; - } - } - } - } - - //down here, we should simply have a collection of plane intersections, now we see if they reside within the quad - Assert( iPlaneIntersectionsCount != 0 ); - - for( int i = 0; i != iPlaneIntersectionsCount; ++i ) - { - //these points are guaranteed to be on the plane, now just check to see if they're within the quad's extents - Vector vToPointFromQuadCenter = ptPlaneIntersections[i] - ptQuadCenter; - - float fExt1Dist = vQuadExtent1_Normalized.Dot( vToPointFromQuadCenter ); - if( fabs( fExt1Dist ) > fQuadExtent1Length ) - return false; //point is outside boundaries - - //vToPointFromQuadCenter -= vQuadExtent1_Normalized * fExt1Dist; //to handle diamond shaped quads - - float fExt2Dist = vQuadExtent2_Normalized.Dot( vToPointFromQuadCenter ); - if( fabs( fExt2Dist ) > fQuadExtent2Length ) - return false; //point is outside boundaries - } - - return true; //there were lines crossing the quad plane, and every line crossing that plane had its intersection with the plane within the quad's boundaries -} - -#endif // !_STATIC_LINKED || _SHARED_LIB +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: Common collision utility methods +// +// $Header: $ +// $NoKeywords: $ +//=============================================================================// + +#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB) + +#include "collisionutils.h" +#include "cmodel.h" +#include "mathlib/mathlib.h" +#include "mathlib/vector.h" +#include "tier0/dbg.h" +#include +#include "mathlib/vector4d.h" +#include "trace.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +#define UNINIT -99999.0 + +//----------------------------------------------------------------------------- +// Clears the trace +//----------------------------------------------------------------------------- +static void Collision_ClearTrace( const Vector &vecRayStart, const Vector &vecRayDelta, CBaseTrace *pTrace ) +{ + pTrace->startpos = vecRayStart; + pTrace->endpos = vecRayStart; + pTrace->endpos += vecRayDelta; + pTrace->startsolid = false; + pTrace->allsolid = false; + pTrace->fraction = 1.0f; + pTrace->contents = 0; +} + + +//----------------------------------------------------------------------------- +// Compute the offset in t along the ray that we'll use for the collision +//----------------------------------------------------------------------------- +static float ComputeBoxOffset( const Ray_t& ray ) +{ + if (ray.m_IsRay) + return 1e-3f; + + // Find the projection of the box diagonal along the ray... + float offset = FloatMakePositive(ray.m_Extents[0] * ray.m_Delta[0]) + + FloatMakePositive(ray.m_Extents[1] * ray.m_Delta[1]) + + FloatMakePositive(ray.m_Extents[2] * ray.m_Delta[2]); + + // We need to divide twice: Once to normalize the computation above + // so we get something in units of extents, and the second to normalize + // that with respect to the entire raycast. + offset *= InvRSquared( ray.m_Delta ); + + // 1e-3 is an epsilon + return offset + 1e-3; +} + + +//----------------------------------------------------------------------------- +// Intersects a swept box against a triangle +//----------------------------------------------------------------------------- +float IntersectRayWithTriangle( const Ray_t& ray, + const Vector& v1, const Vector& v2, const Vector& v3, bool oneSided ) +{ + // This is cute: Use barycentric coordinates to represent the triangle + // Vo(1-u-v) + V1u + V2v and intersect that with a line Po + Dt + // This gives us 3 equations + 3 unknowns, which we can solve with + // Cramer's rule... + // E1x u + E2x v - Dx t = Pox - Vox + // There's a couple of other optimizations, Cramer's rule involves + // computing the determinant of a matrix which has been constructed + // by three vectors. It turns out that + // det | A B C | = -( A x C ) dot B or -(C x B) dot A + // which we'll use below.. + + Vector edge1, edge2, org; + VectorSubtract( v2, v1, edge1 ); + VectorSubtract( v3, v1, edge2 ); + + // Cull out one-sided stuff + if (oneSided) + { + Vector normal; + CrossProduct( edge1, edge2, normal ); + if (DotProduct( normal, ray.m_Delta ) >= 0.0f) + return -1.0f; + } + + // FIXME: This is inaccurate, but fast for boxes + // We want to do a fast separating axis implementation here + // with a swept triangle along the reverse direction of the ray. + + // Compute some intermediary terms + Vector dirCrossEdge2, orgCrossEdge1; + CrossProduct( ray.m_Delta, edge2, dirCrossEdge2 ); + + // Compute the denominator of Cramer's rule: + // | -Dx E1x E2x | + // det | -Dy E1y E2y | = (D x E2) dot E1 + // | -Dz E1z E2z | + float denom = DotProduct( dirCrossEdge2, edge1 ); + if( FloatMakePositive( denom ) < 1e-6 ) + return -1.0f; + denom = 1.0f / denom; + + // Compute u. It's gotta lie in the range of 0 to 1. + // | -Dx orgx E2x | + // u = denom * det | -Dy orgy E2y | = (D x E2) dot org + // | -Dz orgz E2z | + VectorSubtract( ray.m_Start, v1, org ); + float u = DotProduct( dirCrossEdge2, org ) * denom; + if ((u < 0.0f) || (u > 1.0f)) + return -1.0f; + + // Compute t and v the same way... + // In barycentric coords, u + v < 1 + CrossProduct( org, edge1, orgCrossEdge1 ); + float v = DotProduct( orgCrossEdge1, ray.m_Delta ) * denom; + if ((v < 0.0f) || (v + u > 1.0f)) + return -1.0f; + + // Compute the distance along the ray direction that we need to fudge + // when using swept boxes + float boxt = ComputeBoxOffset( ray ); + float t = DotProduct( orgCrossEdge1, edge2 ) * denom; + if ((t < -boxt) || (t > 1.0f + boxt)) + return -1.0f; + + return clamp( t, 0.f, 1.f ); +} + +//----------------------------------------------------------------------------- +// computes the barycentric coordinates of an intersection +//----------------------------------------------------------------------------- + +bool ComputeIntersectionBarycentricCoordinates( const Ray_t& ray, + const Vector& v1, const Vector& v2, const Vector& v3, float& u, float& v, + float *t ) +{ + Vector edge1, edge2, org; + VectorSubtract( v2, v1, edge1 ); + VectorSubtract( v3, v1, edge2 ); + + // Compute some intermediary terms + Vector dirCrossEdge2, orgCrossEdge1; + CrossProduct( ray.m_Delta, edge2, dirCrossEdge2 ); + + // Compute the denominator of Cramer's rule: + // | -Dx E1x E2x | + // det | -Dy E1y E2y | = (D x E2) dot E1 + // | -Dz E1z E2z | + float denom = DotProduct( dirCrossEdge2, edge1 ); + if( FloatMakePositive( denom ) < 1e-6 ) + return false; + denom = 1.0f / denom; + + // Compute u. It's gotta lie in the range of 0 to 1. + // | -Dx orgx E2x | + // u = denom * det | -Dy orgy E2y | = (D x E2) dot org + // | -Dz orgz E2z | + VectorSubtract( ray.m_Start, v1, org ); + u = DotProduct( dirCrossEdge2, org ) * denom; + + // Compute t and v the same way... + // In barycentric coords, u + v < 1 + CrossProduct( org, edge1, orgCrossEdge1 ); + v = DotProduct( orgCrossEdge1, ray.m_Delta ) * denom; + + // Compute the distance along the ray direction that we need to fudge + // when using swept boxes + if( t ) + { + float boxt = ComputeBoxOffset( ray ); + *t = DotProduct( orgCrossEdge1, edge2 ) * denom; + if( ( *t < -boxt ) || ( *t > 1.0f + boxt ) ) + return false; + } + + return true; +} + +//----------------------------------------------------------------------------- +// Intersects a plane with a triangle (requires barycentric definition) +//----------------------------------------------------------------------------- + +int IntersectTriangleWithPlaneBarycentric( const Vector& org, const Vector& edgeU, + const Vector& edgeV, const Vector4D& plane, Vector2D* pIntersection ) +{ + // This uses a barycentric method, since we need that to determine + // interpolated points, alphas, and normals + // Given the plane equation P dot N + d = 0 + // and the barycentric coodinate equation P = Org + EdgeU * u + EdgeV * v + // Plug em in. Intersection occurs at u = 0 or v = 0 or u + v = 1 + + float orgDotNormal = DotProduct( org, plane.AsVector3D() ); + float edgeUDotNormal = DotProduct( edgeU, plane.AsVector3D() ); + float edgeVDotNormal = DotProduct( edgeV, plane.AsVector3D() ); + + int ptIdx = 0; + + // u = 0 + if ( edgeVDotNormal != 0.0f ) + { + pIntersection[ptIdx].x = 0.0f; + pIntersection[ptIdx].y = - ( orgDotNormal - plane.w ) / edgeVDotNormal; + if ((pIntersection[ptIdx].y >= 0.0f) && (pIntersection[ptIdx].y <= 1.0f)) + ++ptIdx; + } + + // v = 0 + if ( edgeUDotNormal != 0.0f ) + { + pIntersection[ptIdx].x = - ( orgDotNormal - plane.w ) / edgeUDotNormal; + pIntersection[ptIdx].y = 0.0f; + if ((pIntersection[ptIdx].x >= 0.0f) && (pIntersection[ptIdx].x <= 1.0f)) + ++ptIdx; + } + + // u + v = 1 + if (ptIdx == 2) + return ptIdx; + + if ( edgeVDotNormal != edgeUDotNormal ) + { + pIntersection[ptIdx].x = - ( orgDotNormal - plane.w + edgeVDotNormal) / + ( edgeUDotNormal - edgeVDotNormal); + pIntersection[ptIdx].y = 1.0f - pIntersection[ptIdx].x;; + if ((pIntersection[ptIdx].x >= 0.0f) && (pIntersection[ptIdx].x <= 1.0f) && + (pIntersection[ptIdx].y >= 0.0f) && (pIntersection[ptIdx].y <= 1.0f)) + ++ptIdx; + } + + Assert( ptIdx < 3 ); + return ptIdx; +} + + +//----------------------------------------------------------------------------- +// Returns true if a box intersects with a sphere +//----------------------------------------------------------------------------- +bool IsSphereIntersectingSphere( const Vector& center1, float radius1, + const Vector& center2, float radius2 ) +{ + Vector delta; + VectorSubtract( center2, center1, delta ); + float distSq = delta.LengthSqr(); + float radiusSum = radius1 + radius2; + return (distSq <= (radiusSum * radiusSum)); +} + + +//----------------------------------------------------------------------------- +// Returns true if a box intersects with a sphere +//----------------------------------------------------------------------------- +bool IsBoxIntersectingSphere( const Vector& boxMin, const Vector& boxMax, + const Vector& center, float radius ) +{ + // See Graphics Gems, box-sphere intersection + float dmin = 0.0f; + float flDelta; + + // Unrolled the loop.. this is a big cycle stealer... + if (center[0] < boxMin[0]) + { + flDelta = center[0] - boxMin[0]; + dmin += flDelta * flDelta; + } + else if (center[0] > boxMax[0]) + { + flDelta = boxMax[0] - center[0]; + dmin += flDelta * flDelta; + } + + if (center[1] < boxMin[1]) + { + flDelta = center[1] - boxMin[1]; + dmin += flDelta * flDelta; + } + else if (center[1] > boxMax[1]) + { + flDelta = boxMax[1] - center[1]; + dmin += flDelta * flDelta; + } + + if (center[2] < boxMin[2]) + { + flDelta = center[2] - boxMin[2]; + dmin += flDelta * flDelta; + } + else if (center[2] > boxMax[2]) + { + flDelta = boxMax[2] - center[2]; + dmin += flDelta * flDelta; + } + + return dmin < radius * radius; +} + +bool IsBoxIntersectingSphereExtents( const Vector& boxCenter, const Vector& boxHalfDiag, + const Vector& center, float radius ) +{ + // See Graphics Gems, box-sphere intersection + float dmin = 0.0f; + float flDelta, flDiff; + + // Unrolled the loop.. this is a big cycle stealer... + flDiff = FloatMakePositive( center.x - boxCenter.x ); + if (flDiff > boxHalfDiag.x) + { + flDelta = flDiff - boxHalfDiag.x; + dmin += flDelta * flDelta; + } + + flDiff = FloatMakePositive( center.y - boxCenter.y ); + if (flDiff > boxHalfDiag.y) + { + flDelta = flDiff - boxHalfDiag.y; + dmin += flDelta * flDelta; + } + + flDiff = FloatMakePositive( center.z - boxCenter.z ); + if (flDiff > boxHalfDiag.z) + { + flDelta = flDiff - boxHalfDiag.z; + dmin += flDelta * flDelta; + } + + return dmin < radius * radius; +} + + +//----------------------------------------------------------------------------- +// Returns true if a rectangle intersects with a circle +//----------------------------------------------------------------------------- +bool IsCircleIntersectingRectangle( const Vector2D& boxMin, const Vector2D& boxMax, + const Vector2D& center, float radius ) +{ + // See Graphics Gems, box-sphere intersection + float dmin = 0.0f; + float flDelta; + + if (center[0] < boxMin[0]) + { + flDelta = center[0] - boxMin[0]; + dmin += flDelta * flDelta; + } + else if (center[0] > boxMax[0]) + { + flDelta = boxMax[0] - center[0]; + dmin += flDelta * flDelta; + } + + if (center[1] < boxMin[1]) + { + flDelta = center[1] - boxMin[1]; + dmin += flDelta * flDelta; + } + else if (center[1] > boxMax[1]) + { + flDelta = boxMax[1] - center[1]; + dmin += flDelta * flDelta; + } + + return dmin < radius * radius; +} + + +//----------------------------------------------------------------------------- +// returns true if there's an intersection between ray and sphere +//----------------------------------------------------------------------------- +bool IsRayIntersectingSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, + const Vector& vecCenter, float flRadius, float flTolerance ) +{ + // For this algorithm, find a point on the ray which is closest to the sphere origin + // Do this by making a plane passing through the sphere origin + // whose normal is parallel to the ray. Intersect that plane with the ray. + // Plane: N dot P = I, N = D (ray direction), I = C dot N = C dot D + // Ray: P = O + D * t + // D dot ( O + D * t ) = C dot D + // D dot O + D dot D * t = C dot D + // t = (C - O) dot D / D dot D + // Clamp t to (0,1) + // Find distance of the point on the ray to the sphere center. + Assert( flTolerance >= 0.0f ); + flRadius += flTolerance; + + Vector vecRayToSphere; + VectorSubtract( vecCenter, vecRayOrigin, vecRayToSphere ); + float flNumerator = DotProduct( vecRayToSphere, vecRayDelta ); + + float t; + if (flNumerator <= 0.0f) + { + t = 0.0f; + } + else + { + float flDenominator = DotProduct( vecRayDelta, vecRayDelta ); + if ( flNumerator > flDenominator ) + t = 1.0f; + else + t = flNumerator / flDenominator; + } + + Vector vecClosestPoint; + VectorMA( vecRayOrigin, t, vecRayDelta, vecClosestPoint ); + return ( vecClosestPoint.DistToSqr( vecCenter ) <= flRadius * flRadius ); + + // NOTE: This in an alternate algorithm which I didn't use because I'd have to use a sqrt + // So it's probably faster to do this other algorithm. I'll leave the comments here + // for how to go back if we want to + + // Solve using the ray equation + the sphere equation + // P = o + dt + // (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 + // (ox + dx * t - xc)^2 + (oy + dy * t - yc)^2 + (oz + dz * t - zc)^2 = r^2 + // (ox - xc)^2 + 2 * (ox-xc) * dx * t + dx^2 * t^2 + + // (oy - yc)^2 + 2 * (oy-yc) * dy * t + dy^2 * t^2 + + // (oz - zc)^2 + 2 * (oz-zc) * dz * t + dz^2 * t^2 = r^2 + // (dx^2 + dy^2 + dz^2) * t^2 + 2 * ((ox-xc)dx + (oy-yc)dy + (oz-zc)dz) t + + // (ox-xc)^2 + (oy-yc)^2 + (oz-zc)^2 - r^2 = 0 + // or, t = (-b +/- sqrt( b^2 - 4ac)) / 2a + // a = DotProduct( vecRayDelta, vecRayDelta ); + // b = 2 * DotProduct( vecRayOrigin - vecCenter, vecRayDelta ) + // c = DotProduct(vecRayOrigin - vecCenter, vecRayOrigin - vecCenter) - flRadius * flRadius; + // Valid solutions are possible only if b^2 - 4ac >= 0 + // Therefore, compute that value + see if we got it +} + + +//----------------------------------------------------------------------------- +// +// IntersectInfiniteRayWithSphere +// +// Returns whether or not there was an intersection. +// Returns the two intersection points +// +//----------------------------------------------------------------------------- +bool IntersectInfiniteRayWithSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, + const Vector &vecSphereCenter, float flRadius, float *pT1, float *pT2 ) +{ + // Solve using the ray equation + the sphere equation + // P = o + dt + // (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 + // (ox + dx * t - xc)^2 + (oy + dy * t - yc)^2 + (oz + dz * t - zc)^2 = r^2 + // (ox - xc)^2 + 2 * (ox-xc) * dx * t + dx^2 * t^2 + + // (oy - yc)^2 + 2 * (oy-yc) * dy * t + dy^2 * t^2 + + // (oz - zc)^2 + 2 * (oz-zc) * dz * t + dz^2 * t^2 = r^2 + // (dx^2 + dy^2 + dz^2) * t^2 + 2 * ((ox-xc)dx + (oy-yc)dy + (oz-zc)dz) t + + // (ox-xc)^2 + (oy-yc)^2 + (oz-zc)^2 - r^2 = 0 + // or, t = (-b +/- sqrt( b^2 - 4ac)) / 2a + // a = DotProduct( vecRayDelta, vecRayDelta ); + // b = 2 * DotProduct( vecRayOrigin - vecCenter, vecRayDelta ) + // c = DotProduct(vecRayOrigin - vecCenter, vecRayOrigin - vecCenter) - flRadius * flRadius; + + Vector vecSphereToRay; + VectorSubtract( vecRayOrigin, vecSphereCenter, vecSphereToRay ); + + float a = DotProduct( vecRayDelta, vecRayDelta ); + + // This would occur in the case of a zero-length ray + if ( a == 0.0f ) + { + *pT1 = *pT2 = 0.0f; + return vecSphereToRay.LengthSqr() <= flRadius * flRadius; + } + + float b = 2 * DotProduct( vecSphereToRay, vecRayDelta ); + float c = DotProduct( vecSphereToRay, vecSphereToRay ) - flRadius * flRadius; + float flDiscrim = b * b - 4 * a * c; + if ( flDiscrim < 0.0f ) + return false; + + flDiscrim = sqrt( flDiscrim ); + float oo2a = 0.5f / a; + *pT1 = ( - b - flDiscrim ) * oo2a; + *pT2 = ( - b + flDiscrim ) * oo2a; + return true; +} + + + +//----------------------------------------------------------------------------- +// +// IntersectRayWithSphere +// +// Returns whether or not there was an intersection. +// Returns the two intersection points, clamped to (0,1) +// +//----------------------------------------------------------------------------- +bool IntersectRayWithSphere( const Vector &vecRayOrigin, const Vector &vecRayDelta, + const Vector &vecSphereCenter, float flRadius, float *pT1, float *pT2 ) +{ + if ( !IntersectInfiniteRayWithSphere( vecRayOrigin, vecRayDelta, vecSphereCenter, flRadius, pT1, pT2 ) ) + return false; + + if (( *pT1 > 1.0f ) || ( *pT2 < 0.0f )) + return false; + + // Clamp it! + if ( *pT1 < 0.0f ) + *pT1 = 0.0f; + if ( *pT2 > 1.0f ) + *pT2 = 1.0f; + + return true; +} + + +// returns true if the sphere and cone intersect +// NOTE: cone sine/cosine are the half angle of the cone +bool IsSphereIntersectingCone( const Vector &sphereCenter, float sphereRadius, const Vector &coneOrigin, const Vector &coneNormal, float coneSine, float coneCosine ) +{ + Vector backCenter = coneOrigin - (sphereRadius / coneSine) * coneNormal; + Vector delta = sphereCenter - backCenter; + float deltaLen = delta.Length(); + if ( DotProduct(coneNormal, delta) >= deltaLen*coneCosine ) + { + delta = sphereCenter - coneOrigin; + deltaLen = delta.Length(); + if ( -DotProduct(coneNormal, delta) >= deltaLen * coneSine ) + { + return ( deltaLen <= sphereRadius ) ? true : false; + } + return true; + } + return false; +} + + + +//----------------------------------------------------------------------------- +// returns true if the point is in the box +//----------------------------------------------------------------------------- +bool IsPointInBox( const Vector& pt, const Vector& boxMin, const Vector& boxMax ) +{ + Assert( boxMin[0] <= boxMax[0] ); + Assert( boxMin[1] <= boxMax[1] ); + Assert( boxMin[2] <= boxMax[2] ); + + // on x360, force use of SIMD version. + if (IsX360()) + { + return IsPointInBox( LoadUnaligned3SIMD(pt.Base()), LoadUnaligned3SIMD(boxMin.Base()), LoadUnaligned3SIMD(boxMax.Base()) ) ; + } + + if ( (pt[0] > boxMax[0]) || (pt[0] < boxMin[0]) ) + return false; + if ( (pt[1] > boxMax[1]) || (pt[1] < boxMin[1]) ) + return false; + if ( (pt[2] > boxMax[2]) || (pt[2] < boxMin[2]) ) + return false; + return true; +} + + +bool IsPointInCone( const Vector &pt, const Vector &origin, const Vector &axis, float cosAngle, float length ) +{ + Vector delta = pt - origin; + float dist = VectorNormalize( delta ); + float dot = DotProduct( delta, axis ); + if ( dot < cosAngle ) + return false; + if ( dist * dot > length ) + return false; + + return true; +} + + +//----------------------------------------------------------------------------- +// returns true if there's an intersection between two boxes +//----------------------------------------------------------------------------- +bool IsBoxIntersectingBox( const Vector& boxMin1, const Vector& boxMax1, + const Vector& boxMin2, const Vector& boxMax2 ) +{ + Assert( boxMin1[0] <= boxMax1[0] ); + Assert( boxMin1[1] <= boxMax1[1] ); + Assert( boxMin1[2] <= boxMax1[2] ); + Assert( boxMin2[0] <= boxMax2[0] ); + Assert( boxMin2[1] <= boxMax2[1] ); + Assert( boxMin2[2] <= boxMax2[2] ); + + if ( (boxMin1[0] > boxMax2[0]) || (boxMax1[0] < boxMin2[0]) ) + return false; + if ( (boxMin1[1] > boxMax2[1]) || (boxMax1[1] < boxMin2[1]) ) + return false; + if ( (boxMin1[2] > boxMax2[2]) || (boxMax1[2] < boxMin2[2]) ) + return false; + return true; +} + +bool IsBoxIntersectingBoxExtents( const Vector& boxCenter1, const Vector& boxHalfDiagonal1, + const Vector& boxCenter2, const Vector& boxHalfDiagonal2 ) +{ + Vector vecDelta, vecSize; + VectorSubtract( boxCenter1, boxCenter2, vecDelta ); + VectorAdd( boxHalfDiagonal1, boxHalfDiagonal2, vecSize ); + return ( FloatMakePositive( vecDelta.x ) <= vecSize.x ) && + ( FloatMakePositive( vecDelta.y ) <= vecSize.y ) && + ( FloatMakePositive( vecDelta.z ) <= vecSize.z ); +} + + +//----------------------------------------------------------------------------- +// +// IsOBBIntersectingOBB +// +// returns true if there's an intersection between two OBBs +// +//----------------------------------------------------------------------------- +bool IsOBBIntersectingOBB( const Vector &vecOrigin1, const QAngle &vecAngles1, const Vector& boxMin1, const Vector& boxMax1, + const Vector &vecOrigin2, const QAngle &vecAngles2, const Vector& boxMin2, const Vector& boxMax2, float flTolerance ) +{ + // FIXME: Simple case AABB check doesn't work because the min and max extents are not oriented based on the angle + // this fast check would only be good for cubes. + /*if ( vecAngles1 == vecAngles2 ) + { + const Vector &vecDelta = vecOrigin2 - vecOrigin1; + Vector vecOtherMins, vecOtherMaxs; + VectorAdd( boxMin2, vecDelta, vecOtherMins ); + VectorAdd( boxMax2, vecDelta, vecOtherMaxs ); + return IsBoxIntersectingBox( boxMin1, boxMax1, vecOtherMins, vecOtherMaxs ); + }*/ + + // OBB test... + cplane_t plane; + bool bFoundPlane = ComputeSeparatingPlane( vecOrigin1, vecAngles1, boxMin1, boxMax1, + vecOrigin2, vecAngles2, boxMin2, boxMax2, flTolerance, &plane ); + return (bFoundPlane == false); +} + +// NOTE: This is only very slightly faster on high end PCs and x360 +#define USE_SIMD_RAY_CHECKS 1 +//----------------------------------------------------------------------------- +// returns true if there's an intersection between box and ray +//----------------------------------------------------------------------------- +bool FASTCALL IsBoxIntersectingRay( const Vector& boxMin, const Vector& boxMax, + const Vector& origin, const Vector& vecDelta, float flTolerance ) +{ + +#if USE_SIMD_RAY_CHECKS + // Load the unaligned ray/box parameters into SIMD registers + fltx4 start = LoadUnaligned3SIMD(origin.Base()); + fltx4 delta = LoadUnaligned3SIMD(vecDelta.Base()); + fltx4 boxMins = LoadUnaligned3SIMD( boxMin.Base() ); + fltx4 boxMaxs = LoadUnaligned3SIMD( boxMax.Base() ); + fltx4 epsilon = ReplicateX4(flTolerance); + // compute the mins/maxs of the box expanded by the ray extents + // relocate the problem so that the ray start is at the origin. + fltx4 offsetMins = SubSIMD(boxMins, start); + fltx4 offsetMaxs = SubSIMD(boxMaxs, start); + fltx4 offsetMinsExpanded = SubSIMD(offsetMins, epsilon); + fltx4 offsetMaxsExpanded = AddSIMD(offsetMaxs, epsilon); + + // Check to see if both the origin (start point) and the end point (delta) are on the front side + // of any of the box sides - if so there can be no intersection + fltx4 startOutMins = CmpLtSIMD(Four_Zeros, offsetMinsExpanded); + fltx4 endOutMins = CmpLtSIMD(delta,offsetMinsExpanded); + fltx4 minsMask = AndSIMD( startOutMins, endOutMins ); + fltx4 startOutMaxs = CmpGtSIMD(Four_Zeros, offsetMaxsExpanded); + fltx4 endOutMaxs = CmpGtSIMD(delta,offsetMaxsExpanded); + fltx4 maxsMask = AndSIMD( startOutMaxs, endOutMaxs ); + if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(minsMask,maxsMask)))) + return false; + + // now build the per-axis interval of t for intersections + fltx4 invDelta = ReciprocalSaturateSIMD(delta); + fltx4 tmins = MulSIMD( offsetMinsExpanded, invDelta ); + fltx4 tmaxs = MulSIMD( offsetMaxsExpanded, invDelta ); + fltx4 crossPlane = OrSIMD(XorSIMD(startOutMins,endOutMins), XorSIMD(startOutMaxs,endOutMaxs)); + + // only consider axes where we crossed a plane + tmins = MaskedAssign( crossPlane, tmins, Four_Negative_FLT_MAX ); + tmaxs = MaskedAssign( crossPlane, tmaxs, Four_FLT_MAX ); + + // now sort the interval per axis + fltx4 mint = MinSIMD( tmins, tmaxs ); + fltx4 maxt = MaxSIMD( tmins, tmaxs ); + + // now find the intersection of the intervals on all axes + fltx4 firstOut = FindLowestSIMD3(maxt); + fltx4 lastIn = FindHighestSIMD3(mint); + // NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut] + firstOut = MinSIMD(firstOut, Four_Ones); + lastIn = MaxSIMD(lastIn, Four_Zeros); + + // If the final interval is valid lastIn boxMax[i] + flTolerance) ) + return false; + + continue; + } + + // non-parallel case + // Find the t's corresponding to the entry and exit of + // the ray along x, y, and z. The find the furthest entry + // point, and the closest exit point. Once that is done, + // we know we don't collide if the closest exit point + // is behind the starting location. We also don't collide if + // the closest exit point is in front of the furthest entry point + + float invDelta = 1.0f / vecDelta[i]; + float t1 = (boxMin[i] - flTolerance - origin[i]) * invDelta; + float t2 = (boxMax[i] + flTolerance - origin[i]) * invDelta; + if (t1 > t2) + { + float temp = t1; + t1 = t2; + t2 = temp; + } + if (t1 > tmin) + tmin = t1; + if (t2 < tmax) + tmax = t2; + if (tmin > tmax) + return false; + if (tmax < 0) + return false; + if (tmin > 1) + return false; + } + + return true; +#endif +} + +//----------------------------------------------------------------------------- +// returns true if there's an intersection between box and ray +//----------------------------------------------------------------------------- +bool FASTCALL IsBoxIntersectingRay( const Vector& boxMin, const Vector& boxMax, + const Vector& origin, const Vector& vecDelta, + const Vector& vecInvDelta, float flTolerance ) +{ +#if USE_SIMD_RAY_CHECKS + // Load the unaligned ray/box parameters into SIMD registers + fltx4 start = LoadUnaligned3SIMD(origin.Base()); + fltx4 delta = LoadUnaligned3SIMD(vecDelta.Base()); + fltx4 boxMins = LoadUnaligned3SIMD( boxMin.Base() ); + fltx4 boxMaxs = LoadUnaligned3SIMD( boxMax.Base() ); + // compute the mins/maxs of the box expanded by the ray extents + // relocate the problem so that the ray start is at the origin. + boxMins = SubSIMD(boxMins, start); + boxMaxs = SubSIMD(boxMaxs, start); + + // Check to see if both the origin (start point) and the end point (delta) are on the front side + // of any of the box sides - if so there can be no intersection + fltx4 startOutMins = CmpLtSIMD(Four_Zeros, boxMins); + fltx4 endOutMins = CmpLtSIMD(delta,boxMins); + fltx4 minsMask = AndSIMD( startOutMins, endOutMins ); + fltx4 startOutMaxs = CmpGtSIMD(Four_Zeros, boxMaxs); + fltx4 endOutMaxs = CmpGtSIMD(delta,boxMaxs); + fltx4 maxsMask = AndSIMD( startOutMaxs, endOutMaxs ); + if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(minsMask,maxsMask)))) + return false; + + // now build the per-axis interval of t for intersections + fltx4 epsilon = ReplicateX4(flTolerance); + fltx4 invDelta = LoadUnaligned3SIMD(vecInvDelta.Base()); + boxMins = SubSIMD(boxMins, epsilon); + boxMaxs = AddSIMD(boxMaxs, epsilon); + + boxMins = MulSIMD( boxMins, invDelta ); + boxMaxs = MulSIMD( boxMaxs, invDelta ); + + fltx4 crossPlane = OrSIMD(XorSIMD(startOutMins,endOutMins), XorSIMD(startOutMaxs,endOutMaxs)); + // only consider axes where we crossed a plane + boxMins = MaskedAssign( crossPlane, boxMins, Four_Negative_FLT_MAX ); + boxMaxs = MaskedAssign( crossPlane, boxMaxs, Four_FLT_MAX ); + + // now sort the interval per axis + fltx4 mint = MinSIMD( boxMins, boxMaxs ); + fltx4 maxt = MaxSIMD( boxMins, boxMaxs ); + + // now find the intersection of the intervals on all axes + fltx4 firstOut = FindLowestSIMD3(maxt); + fltx4 lastIn = FindHighestSIMD3(mint); + // NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut] + firstOut = MinSIMD(firstOut, Four_Ones); + lastIn = MaxSIMD(lastIn, Four_Zeros); + + // If the final interval is valid lastIn boxMax[i] + flTolerance ) ) + return false; + + continue; + } + + // Non-parallel case + // Find the t's corresponding to the entry and exit of + // the ray along x, y, and z. The find the furthest entry + // point, and the closest exit point. Once that is done, + // we know we don't collide if the closest exit point + // is behind the starting location. We also don't collide if + // the closest exit point is in front of the furthest entry point + float t1 = ( boxMin[i] - flTolerance - origin[i] ) * vecInvDelta[i]; + float t2 = ( boxMax[i] + flTolerance - origin[i] ) * vecInvDelta[i]; + if ( t1 > t2 ) + { + float temp = t1; + t1 = t2; + t2 = temp; + } + + if (t1 > tmin) + tmin = t1; + + if (t2 < tmax) + tmax = t2; + + if (tmin > tmax) + return false; + + if (tmax < 0) + return false; + + if (tmin > 1) + return false; + } + + return true; +#endif +} + +//----------------------------------------------------------------------------- +// Intersects a ray with a aabb, return true if they intersect +//----------------------------------------------------------------------------- +bool FASTCALL IsBoxIntersectingRay( const Vector& vecBoxMin, const Vector& vecBoxMax, const Ray_t& ray, float flTolerance ) +{ + // On the x360, we force use of the SIMD functions. +#if defined(_X360) + if (IsX360()) + { + return IsBoxIntersectingRay( + LoadUnaligned3SIMD(vecBoxMin.Base()), LoadUnaligned3SIMD(vecBoxMax.Base()), + ray, flTolerance); + } +#endif + + if ( !ray.m_IsSwept ) + { + Vector rayMins, rayMaxs; + VectorSubtract( ray.m_Start, ray.m_Extents, rayMins ); + VectorAdd( ray.m_Start, ray.m_Extents, rayMaxs ); + if ( flTolerance != 0.0f ) + { + rayMins.x -= flTolerance; rayMins.y -= flTolerance; rayMins.z -= flTolerance; + rayMaxs.x += flTolerance; rayMaxs.y += flTolerance; rayMaxs.z += flTolerance; + } + return IsBoxIntersectingBox( vecBoxMin, vecBoxMax, rayMins, rayMaxs ); + } + + Vector vecExpandedBoxMin, vecExpandedBoxMax; + VectorSubtract( vecBoxMin, ray.m_Extents, vecExpandedBoxMin ); + VectorAdd( vecBoxMax, ray.m_Extents, vecExpandedBoxMax ); + return IsBoxIntersectingRay( vecExpandedBoxMin, vecExpandedBoxMax, ray.m_Start, ray.m_Delta, flTolerance ); +} + + +//----------------------------------------------------------------------------- +// returns true if there's an intersection between box and ray (SIMD version) +//----------------------------------------------------------------------------- + + +#ifdef _X360 +bool FASTCALL IsBoxIntersectingRay( fltx4 boxMin, fltx4 boxMax, + fltx4 origin, fltx4 delta, fltx4 invDelta, // ray parameters + fltx4 vTolerance ///< eg from ReplicateX4(flTolerance) + ) +#else +bool FASTCALL IsBoxIntersectingRay( const fltx4 &inBoxMin, const fltx4 & inBoxMax, + const fltx4 & origin, const fltx4 & delta, const fltx4 & invDelta, // ray parameters + const fltx4 & vTolerance ///< eg from ReplicateX4(flTolerance) + ) +#endif +{ + // Load the unaligned ray/box parameters into SIMD registers + // compute the mins/maxs of the box expanded by the ray extents + // relocate the problem so that the ray start is at the origin. + +#ifdef _X360 + boxMin = SubSIMD(boxMin, origin); + boxMax = SubSIMD(boxMax, origin); +#else + fltx4 boxMin = SubSIMD(inBoxMin, origin); + fltx4 boxMax = SubSIMD(inBoxMax, origin); +#endif + + // Check to see if the origin (start point) and the end point (delta) are on the same side + // of any of the box sides - if so there can be no intersection + fltx4 startOutMins = AndSIMD( CmpLtSIMD(Four_Zeros, boxMin), CmpLtSIMD(delta,boxMin) ); + fltx4 startOutMaxs = AndSIMD( CmpGtSIMD(Four_Zeros, boxMax), CmpGtSIMD(delta,boxMax) ); + if ( IsAnyNegative(SetWToZeroSIMD(OrSIMD(startOutMaxs,startOutMins)))) + return false; + + // now build the per-axis interval of t for intersections + boxMin = SubSIMD(boxMin, vTolerance); + boxMax = AddSIMD(boxMax, vTolerance); + + boxMin = MulSIMD( boxMin, invDelta ); + boxMax = MulSIMD( boxMax, invDelta ); + + // now sort the interval per axis + fltx4 mint = MinSIMD( boxMin, boxMax ); + fltx4 maxt = MaxSIMD( boxMin, boxMax ); + + // now find the intersection of the intervals on all axes + fltx4 firstOut = FindLowestSIMD3(maxt); + fltx4 lastIn = FindHighestSIMD3(mint); + // NOTE: This is really a scalar quantity now [t0,t1] == [lastIn,firstOut] + firstOut = MinSIMD(firstOut, Four_Ones); + lastIn = MaxSIMD(lastIn, Four_Zeros); + + // If the final interval is valid lastInt1 = -1.0f; + pTrace->t2 = 1.0f; + pTrace->hitside = -1; + + // UNDONE: This makes this code a little messy + pTrace->startsolid = true; + + for ( i = 0; i < 6; ++i ) + { + if ( i >= 3 ) + { + d1 = vecRayStart[i-3] - boxMaxs[i-3]; + d2 = d1 + vecRayDelta[i-3]; + } + else + { + d1 = -vecRayStart[i] + boxMins[i]; + d2 = d1 - vecRayDelta[i]; + } + + // if completely in front of face, no intersection + if (d1 > 0 && d2 > 0) + { + // UNDONE: Have to revert this in case it's still set + // UNDONE: Refactor to have only 2 return points (true/false) from this function + pTrace->startsolid = false; + return false; + } + + // completely inside, check next face + if (d1 <= 0 && d2 <= 0) + continue; + + if (d1 > 0) + { + pTrace->startsolid = false; + } + + // crosses face + if (d1 > d2) + { + f = d1 - flTolerance; + if ( f < 0 ) + { + f = 0; + } + f = f / (d1-d2); + if (f > pTrace->t1) + { + pTrace->t1 = f; + pTrace->hitside = i; + } + } + else + { + // leave + f = (d1 + flTolerance) / (d1-d2); + if (f < pTrace->t2) + { + pTrace->t2 = f; + } + } + } + + return pTrace->startsolid || (pTrace->t1 < pTrace->t2 && pTrace->t1 >= 0.0f); +} + + +//----------------------------------------------------------------------------- +// Intersects a ray against a box +//----------------------------------------------------------------------------- +bool IntersectRayWithBox( const Vector &vecRayStart, const Vector &vecRayDelta, + const Vector &boxMins, const Vector &boxMaxs, float flTolerance, CBaseTrace *pTrace, float *pFractionLeftSolid ) +{ + Collision_ClearTrace( vecRayStart, vecRayDelta, pTrace ); + + BoxTraceInfo_t trace; + + if ( IntersectRayWithBox( vecRayStart, vecRayDelta, boxMins, boxMaxs, flTolerance, &trace ) ) + { + pTrace->startsolid = trace.startsolid; + if (trace.t1 < trace.t2 && trace.t1 >= 0.0f) + { + pTrace->fraction = trace.t1; + VectorMA( pTrace->startpos, trace.t1, vecRayDelta, pTrace->endpos ); + pTrace->contents = CONTENTS_SOLID; + pTrace->plane.normal = vec3_origin; + if ( trace.hitside >= 3 ) + { + trace.hitside -= 3; + pTrace->plane.dist = boxMaxs[trace.hitside]; + pTrace->plane.normal[trace.hitside] = 1.0f; + pTrace->plane.type = trace.hitside; + } + else + { + pTrace->plane.dist = -boxMins[trace.hitside]; + pTrace->plane.normal[trace.hitside] = -1.0f; + pTrace->plane.type = trace.hitside; + } + return true; + } + + if ( pTrace->startsolid ) + { + pTrace->allsolid = (trace.t2 <= 0.0f) || (trace.t2 >= 1.0f); + pTrace->fraction = 0; + if ( pFractionLeftSolid ) + { + *pFractionLeftSolid = trace.t2; + } + pTrace->endpos = pTrace->startpos; + pTrace->contents = CONTENTS_SOLID; + pTrace->plane.dist = pTrace->startpos[0]; + pTrace->plane.normal.Init( 1.0f, 0.0f, 0.0f ); + pTrace->plane.type = 0; + pTrace->startpos = vecRayStart + (trace.t2 * vecRayDelta); + return true; + } + } + + return false; +} + + +//----------------------------------------------------------------------------- +// Intersects a ray against a box +//----------------------------------------------------------------------------- +bool IntersectRayWithBox( const Ray_t &ray, const Vector &boxMins, const Vector &boxMaxs, + float flTolerance, CBaseTrace *pTrace, float *pFractionLeftSolid ) +{ + if ( !ray.m_IsRay ) + { + Vector vecExpandedMins = boxMins - ray.m_Extents; + Vector vecExpandedMaxs = boxMaxs + ray.m_Extents; + bool bIntersects = IntersectRayWithBox( ray.m_Start, ray.m_Delta, vecExpandedMins, vecExpandedMaxs, flTolerance, pTrace, pFractionLeftSolid ); + pTrace->startpos += ray.m_StartOffset; + pTrace->endpos += ray.m_StartOffset; + return bIntersects; + } + return IntersectRayWithBox( ray.m_Start, ray.m_Delta, boxMins, boxMaxs, flTolerance, pTrace, pFractionLeftSolid ); +} + + +//----------------------------------------------------------------------------- +// Intersects a ray against an OBB, returns t1 and t2 +//----------------------------------------------------------------------------- +bool IntersectRayWithOBB( const Vector &vecRayStart, const Vector &vecRayDelta, + const matrix3x4_t &matOBBToWorld, const Vector &vecOBBMins, const Vector &vecOBBMaxs, + float flTolerance, BoxTraceInfo_t *pTrace ) +{ + // FIXME: Two transforms is pretty expensive. Should we optimize this? + Vector start, delta; + VectorITransform( vecRayStart, matOBBToWorld, start ); + VectorIRotate( vecRayDelta, matOBBToWorld, delta ); + + return IntersectRayWithBox( start, delta, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); +} + + + +//----------------------------------------------------------------------------- +// Intersects a ray against an OBB +//----------------------------------------------------------------------------- +bool IntersectRayWithOBB( const Vector &vecRayStart, const Vector &vecRayDelta, + const matrix3x4_t &matOBBToWorld, const Vector &vecOBBMins, const Vector &vecOBBMaxs, + float flTolerance, CBaseTrace *pTrace ) +{ + Collision_ClearTrace( vecRayStart, vecRayDelta, pTrace ); + + // FIXME: Make it work with tolerance + Assert( flTolerance == 0.0f ); + + // OPTIMIZE: Store this in the box instead of computing it here + // compute center in local space + Vector vecBoxExtents = (vecOBBMins + vecOBBMaxs) * 0.5; + Vector vecBoxCenter; + + // transform to world space + VectorTransform( vecBoxExtents, matOBBToWorld, vecBoxCenter ); + + // calc extents from local center + vecBoxExtents = vecOBBMaxs - vecBoxExtents; + + // OPTIMIZE: This is optimized for world space. If the transform is fast enough, it may make more + // sense to just xform and call UTIL_ClipToBox() instead. MEASURE THIS. + + // save the extents of the ray along + Vector extent, uextent; + Vector segmentCenter = vecRayStart + vecRayDelta - vecBoxCenter; + + extent.Init(); + + // check box axes for separation + for ( int j = 0; j < 3; j++ ) + { + extent[j] = vecRayDelta.x * matOBBToWorld[0][j] + vecRayDelta.y * matOBBToWorld[1][j] + vecRayDelta.z * matOBBToWorld[2][j]; + uextent[j] = fabsf(extent[j]); + float coord = segmentCenter.x * matOBBToWorld[0][j] + segmentCenter.y * matOBBToWorld[1][j] + segmentCenter.z * matOBBToWorld[2][j]; + coord = fabsf(coord); + + if ( coord > (vecBoxExtents[j] + uextent[j]) ) + return false; + } + + // now check cross axes for separation + float tmp, cextent; + Vector cross = vecRayDelta.Cross( segmentCenter ); + cextent = cross.x * matOBBToWorld[0][0] + cross.y * matOBBToWorld[1][0] + cross.z * matOBBToWorld[2][0]; + cextent = fabsf(cextent); + tmp = vecBoxExtents[1]*uextent[2] + vecBoxExtents[2]*uextent[1]; + if ( cextent > tmp ) + return false; + + cextent = cross.x * matOBBToWorld[0][1] + cross.y * matOBBToWorld[1][1] + cross.z * matOBBToWorld[2][1]; + cextent = fabsf(cextent); + tmp = vecBoxExtents[0]*uextent[2] + vecBoxExtents[2]*uextent[0]; + if ( cextent > tmp ) + return false; + + cextent = cross.x * matOBBToWorld[0][2] + cross.y * matOBBToWorld[1][2] + cross.z * matOBBToWorld[2][2]; + cextent = fabsf(cextent); + tmp = vecBoxExtents[0]*uextent[1] + vecBoxExtents[1]*uextent[0]; + if ( cextent > tmp ) + return false; + + // !!! We hit this box !!! compute intersection point and return + // Compute ray start in bone space + Vector start; + VectorITransform( vecRayStart, matOBBToWorld, start ); + + // extent is ray.m_Delta in bone space, recompute delta in bone space + extent *= 2.0f; + + // delta was prescaled by the current t, so no need to see if this intersection + // is closer + trace_t boxTrace; + if ( !IntersectRayWithBox( start, extent, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ) ) + return false; + + // Fix up the start/end pos and fraction + Vector vecTemp; + VectorTransform( pTrace->endpos, matOBBToWorld, vecTemp ); + pTrace->endpos = vecTemp; + + pTrace->startpos = vecRayStart; + pTrace->fraction *= 2.0f; + + // Fix up the plane information + float flSign = pTrace->plane.normal[ pTrace->plane.type ]; + pTrace->plane.normal[0] = flSign * matOBBToWorld[0][pTrace->plane.type]; + pTrace->plane.normal[1] = flSign * matOBBToWorld[1][pTrace->plane.type]; + pTrace->plane.normal[2] = flSign * matOBBToWorld[2][pTrace->plane.type]; + pTrace->plane.dist = DotProduct( pTrace->endpos, pTrace->plane.normal ); + pTrace->plane.type = 3; + + return true; +} + + +//----------------------------------------------------------------------------- +// Intersects a ray against an OBB +//----------------------------------------------------------------------------- +bool IntersectRayWithOBB( const Vector &vecRayOrigin, const Vector &vecRayDelta, + const Vector &vecBoxOrigin, const QAngle &angBoxRotation, + const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace ) +{ + if (angBoxRotation == vec3_angle) + { + Vector vecAbsMins, vecAbsMaxs; + VectorAdd( vecBoxOrigin, vecOBBMins, vecAbsMins ); + VectorAdd( vecBoxOrigin, vecOBBMaxs, vecAbsMaxs ); + return IntersectRayWithBox( vecRayOrigin, vecRayDelta, vecAbsMins, vecAbsMaxs, flTolerance, pTrace ); + } + + matrix3x4_t obbToWorld; + AngleMatrix( angBoxRotation, vecBoxOrigin, obbToWorld ); + return IntersectRayWithOBB( vecRayOrigin, vecRayDelta, obbToWorld, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); +} + + +//----------------------------------------------------------------------------- +// Box support map +//----------------------------------------------------------------------------- +inline void ComputeSupportMap( const Vector &vecDirection, const Vector &vecBoxMins, + const Vector &vecBoxMaxs, float pDist[2] ) +{ + int nIndex = (vecDirection.x > 0.0f); + pDist[nIndex] = vecBoxMaxs.x * vecDirection.x; + pDist[1 - nIndex] = vecBoxMins.x * vecDirection.x; + + nIndex = (vecDirection.y > 0.0f); + pDist[nIndex] += vecBoxMaxs.y * vecDirection.y; + pDist[1 - nIndex] += vecBoxMins.y * vecDirection.y; + + nIndex = (vecDirection.z > 0.0f); + pDist[nIndex] += vecBoxMaxs.z * vecDirection.z; + pDist[1 - nIndex] += vecBoxMins.z * vecDirection.z; +} + +inline void ComputeSupportMap( const Vector &vecDirection, int i1, int i2, + const Vector &vecBoxMins, const Vector &vecBoxMaxs, float pDist[2] ) +{ + int nIndex = (vecDirection[i1] > 0.0f); + pDist[nIndex] = vecBoxMaxs[i1] * vecDirection[i1]; + pDist[1 - nIndex] = vecBoxMins[i1] * vecDirection[i1]; + + nIndex = (vecDirection[i2] > 0.0f); + pDist[nIndex] += vecBoxMaxs[i2] * vecDirection[i2]; + pDist[1 - nIndex] += vecBoxMins[i2] * vecDirection[i2]; +} + +//----------------------------------------------------------------------------- +// Intersects a ray against an OBB +//----------------------------------------------------------------------------- +static int s_ExtIndices[3][2] = +{ + { 2, 1 }, + { 0, 2 }, + { 0, 1 }, +}; + +static int s_MatIndices[3][2] = +{ + { 1, 2 }, + { 2, 0 }, + { 1, 0 }, +}; + +bool IntersectRayWithOBB( const Ray_t &ray, const matrix3x4_t &matOBBToWorld, + const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace ) +{ + if ( ray.m_IsRay ) + { + return IntersectRayWithOBB( ray.m_Start, ray.m_Delta, matOBBToWorld, + vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); + } + + Collision_ClearTrace( ray.m_Start + ray.m_StartOffset, ray.m_Delta, pTrace ); + + // Compute a bounding sphere around the bloated OBB + Vector vecOBBCenter; + VectorAdd( vecOBBMins, vecOBBMaxs, vecOBBCenter ); + vecOBBCenter *= 0.5f; + vecOBBCenter.x += matOBBToWorld[0][3]; + vecOBBCenter.y += matOBBToWorld[1][3]; + vecOBBCenter.z += matOBBToWorld[2][3]; + + Vector vecOBBHalfDiagonal; + VectorSubtract( vecOBBMaxs, vecOBBMins, vecOBBHalfDiagonal ); + vecOBBHalfDiagonal *= 0.5f; + + float flRadius = vecOBBHalfDiagonal.Length() + ray.m_Extents.Length(); + if ( !IsRayIntersectingSphere( ray.m_Start, ray.m_Delta, vecOBBCenter, flRadius, flTolerance ) ) + return false; + + // Ok, we passed the trivial reject, so lets do the dirty deed. + // Basically we're going to do the GJK thing explicitly. We'll shrink the ray down + // to a point, and bloat the OBB by the ray's extents. This will generate facet + // planes which are perpendicular to all of the separating axes typically seen in + // a standard seperating axis implementation. + + // We're going to create a number of planes through various vertices in the OBB + // which represent all of the separating planes. Then we're going to bloat the planes + // by the ray extents. + + // We're going to do all work in OBB-space because it's easier to do the + // support-map in this case + + // First, transform the ray into the space of the OBB + Vector vecLocalRayOrigin, vecLocalRayDirection; + VectorITransform( ray.m_Start, matOBBToWorld, vecLocalRayOrigin ); + VectorIRotate( ray.m_Delta, matOBBToWorld, vecLocalRayDirection ); + + // Next compute all separating planes + Vector pPlaneNormal[15]; + float ppPlaneDist[15][2]; + + int i; + for ( i = 0; i < 3; ++i ) + { + // Each plane needs to be bloated an amount = to the abs dot product of + // the ray extents with the plane normal + // For the OBB planes, do it in world space; + // and use the direction of the OBB (the ith column of matOBBToWorld) in world space vs extents + pPlaneNormal[i].Init( ); + pPlaneNormal[i][i] = 1.0f; + + float flExtentDotNormal = + FloatMakePositive( matOBBToWorld[0][i] * ray.m_Extents.x ) + + FloatMakePositive( matOBBToWorld[1][i] * ray.m_Extents.y ) + + FloatMakePositive( matOBBToWorld[2][i] * ray.m_Extents.z ); + + ppPlaneDist[i][0] = vecOBBMins[i] - flExtentDotNormal; + ppPlaneDist[i][1] = vecOBBMaxs[i] + flExtentDotNormal; + + // For the ray-extents planes, they are bloated by the extents + // Use the support map to determine which + VectorCopy( matOBBToWorld[i], pPlaneNormal[i+3].Base() ); + ComputeSupportMap( pPlaneNormal[i+3], vecOBBMins, vecOBBMaxs, ppPlaneDist[i+3] ); + ppPlaneDist[i+3][0] -= ray.m_Extents[i]; + ppPlaneDist[i+3][1] += ray.m_Extents[i]; + + // Now the edge cases... (take the cross product of x,y,z axis w/ ray extent axes + // given by the rows of the obb to world matrix. + // Compute the ray extent bloat in world space because it's easier... + + // These are necessary to compute the world-space versions of + // the edges so we can compute the extent dot products + float flRayExtent0 = ray.m_Extents[s_ExtIndices[i][0]]; + float flRayExtent1 = ray.m_Extents[s_ExtIndices[i][1]]; + const float *pMatRow0 = matOBBToWorld[s_MatIndices[i][0]]; + const float *pMatRow1 = matOBBToWorld[s_MatIndices[i][1]]; + + // x axis of the OBB + world ith axis + pPlaneNormal[i+6].Init( 0.0f, -matOBBToWorld[i][2], matOBBToWorld[i][1] ); + ComputeSupportMap( pPlaneNormal[i+6], 1, 2, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+6] ); + flExtentDotNormal = + FloatMakePositive( pMatRow0[0] ) * flRayExtent0 + + FloatMakePositive( pMatRow1[0] ) * flRayExtent1; + ppPlaneDist[i+6][0] -= flExtentDotNormal; + ppPlaneDist[i+6][1] += flExtentDotNormal; + + // y axis of the OBB + world ith axis + pPlaneNormal[i+9].Init( matOBBToWorld[i][2], 0.0f, -matOBBToWorld[i][0] ); + ComputeSupportMap( pPlaneNormal[i+9], 0, 2, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+9] ); + flExtentDotNormal = + FloatMakePositive( pMatRow0[1] ) * flRayExtent0 + + FloatMakePositive( pMatRow1[1] ) * flRayExtent1; + ppPlaneDist[i+9][0] -= flExtentDotNormal; + ppPlaneDist[i+9][1] += flExtentDotNormal; + + // z axis of the OBB + world ith axis + pPlaneNormal[i+12].Init( -matOBBToWorld[i][1], matOBBToWorld[i][0], 0.0f ); + ComputeSupportMap( pPlaneNormal[i+12], 0, 1, vecOBBMins, vecOBBMaxs, ppPlaneDist[i+12] ); + flExtentDotNormal = + FloatMakePositive( pMatRow0[2] ) * flRayExtent0 + + FloatMakePositive( pMatRow1[2] ) * flRayExtent1; + ppPlaneDist[i+12][0] -= flExtentDotNormal; + ppPlaneDist[i+12][1] += flExtentDotNormal; + } + + float enterfrac, leavefrac; + float d1[2], d2[2]; + float f; + + int hitplane = -1; + int hitside = -1; + enterfrac = -1.0f; + leavefrac = 1.0f; + + pTrace->startsolid = true; + + Vector vecLocalRayEnd; + VectorAdd( vecLocalRayOrigin, vecLocalRayDirection, vecLocalRayEnd ); + + for ( i = 0; i < 15; ++i ) + { + // FIXME: Not particularly optimal since there's a lot of 0's in the plane normals + float flStartDot = DotProduct( pPlaneNormal[i], vecLocalRayOrigin ); + float flEndDot = DotProduct( pPlaneNormal[i], vecLocalRayEnd ); + + // NOTE: Negative here is because the plane normal + dist + // are defined in negative terms for the far plane (plane dist index 0) + d1[0] = -(flStartDot - ppPlaneDist[i][0]); + d2[0] = -(flEndDot - ppPlaneDist[i][0]); + + d1[1] = flStartDot - ppPlaneDist[i][1]; + d2[1] = flEndDot - ppPlaneDist[i][1]; + + int j; + for ( j = 0; j < 2; ++j ) + { + // if completely in front near plane or behind far plane no intersection + if (d1[j] > 0 && d2[j] > 0) + return false; + + // completely inside, check next plane set + if (d1[j] <= 0 && d2[j] <= 0) + continue; + + if (d1[j] > 0) + { + pTrace->startsolid = false; + } + + // crosses face + float flDenom = 1.0f / (d1[j] - d2[j]); + if (d1[j] > d2[j]) + { + f = d1[j] - flTolerance; + if ( f < 0 ) + { + f = 0; + } + f *= flDenom; + if (f > enterfrac) + { + enterfrac = f; + hitplane = i; + hitside = j; + } + } + else + { + // leave + f = (d1[j] + flTolerance) * flDenom; + if (f < leavefrac) + { + leavefrac = f; + } + } + } + } + + if (enterfrac < leavefrac && enterfrac >= 0.0f) + { + pTrace->fraction = enterfrac; + VectorMA( pTrace->startpos, enterfrac, ray.m_Delta, pTrace->endpos ); + pTrace->contents = CONTENTS_SOLID; + + // Need to transform the plane into world space... + cplane_t temp; + temp.normal = pPlaneNormal[hitplane]; + temp.dist = ppPlaneDist[hitplane][hitside]; + if (hitside == 0) + { + temp.normal *= -1.0f; + temp.dist *= -1.0f; + } + temp.type = 3; + + MatrixITransformPlane( matOBBToWorld, temp, pTrace->plane ); + return true; + } + + if ( pTrace->startsolid ) + { + pTrace->allsolid = (leavefrac <= 0.0f) || (leavefrac >= 1.0f); + pTrace->fraction = 0; + pTrace->endpos = pTrace->startpos; + pTrace->contents = CONTENTS_SOLID; + pTrace->plane.dist = pTrace->startpos[0]; + pTrace->plane.normal.Init( 1.0f, 0.0f, 0.0f ); + pTrace->plane.type = 0; + return true; + } + + return false; +} + + +//----------------------------------------------------------------------------- +// Intersects a ray against an OBB +//----------------------------------------------------------------------------- +bool IntersectRayWithOBB( const Ray_t &ray, const Vector &vecBoxOrigin, const QAngle &angBoxRotation, + const Vector &vecOBBMins, const Vector &vecOBBMaxs, float flTolerance, CBaseTrace *pTrace ) +{ + if ( angBoxRotation == vec3_angle ) + { + Vector vecWorldMins, vecWorldMaxs; + VectorAdd( vecBoxOrigin, vecOBBMins, vecWorldMins ); + VectorAdd( vecBoxOrigin, vecOBBMaxs, vecWorldMaxs ); + return IntersectRayWithBox( ray, vecWorldMins, vecWorldMaxs, flTolerance, pTrace ); + } + + if ( ray.m_IsRay ) + { + return IntersectRayWithOBB( ray.m_Start, ray.m_Delta, vecBoxOrigin, angBoxRotation, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); + } + + matrix3x4_t matOBBToWorld; + AngleMatrix( angBoxRotation, vecBoxOrigin, matOBBToWorld ); + return IntersectRayWithOBB( ray, matOBBToWorld, vecOBBMins, vecOBBMaxs, flTolerance, pTrace ); +} + + +//----------------------------------------------------------------------------- +// +//----------------------------------------------------------------------------- +void GetNonMajorAxes( const Vector &vNormal, Vector2D &axes ) +{ + axes[0] = 0; + axes[1] = 1; + + if( FloatMakePositive( vNormal.x ) > FloatMakePositive( vNormal.y ) ) + { + if( FloatMakePositive( vNormal.x ) > FloatMakePositive( vNormal.z ) ) + { + axes[0] = 1; + axes[1] = 2; + } + } + else + { + if( FloatMakePositive( vNormal.y ) > FloatMakePositive( vNormal.z ) ) + { + axes[0] = 0; + axes[1] = 2; + } + } +} + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +QuadBarycentricRetval_t QuadWithParallelEdges( const Vector &vecOrigin, + const Vector &vecU, float lengthU, const Vector &vecV, float lengthV, + const Vector &pt, Vector2D &vecUV ) +{ + Ray_t rayAxis; + Ray_t rayPt; + + // + // handle the u axis + // + rayAxis.m_Start = vecOrigin; + rayAxis.m_Delta = vecU; + rayAxis.m_IsRay = true; + + rayPt.m_Start = pt; + rayPt.m_Delta = vecV * -( lengthV * 10.0f ); + rayPt.m_IsRay = true; + + float s, t; + IntersectRayWithRay( rayAxis, rayPt, t, s ); + vecUV[0] = t / lengthU; + + // + // handle the v axis + // + rayAxis.m_Delta = vecV; + + rayPt.m_Delta = vecU * -( lengthU * 10.0f ); + + IntersectRayWithRay( rayAxis, rayPt, t, s ); + vecUV[1] = t / lengthV; + + // inside of the quad?? + if( ( vecUV[0] < 0.0f ) || ( vecUV[0] > 1.0f ) || + ( vecUV[1] < 0.0f ) || ( vecUV[1] > 1.0f ) ) + return BARY_QUADRATIC_FALSE; + + return BARY_QUADRATIC_TRUE; +} + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void ResolveQuadratic( double tPlus, double tMinus, + const Vector axisU0, const Vector axisU1, + const Vector axisV0, const Vector axisV1, + const Vector axisOrigin, const Vector pt, + int projU, double &s, double &t ) +{ + // calculate the sPlus, sMinus pair(s) + double sDenomPlus = ( axisU0[projU] * ( 1 - tPlus ) ) + ( axisU1[projU] * tPlus ); + double sDenomMinus = ( axisU0[projU] * ( 1 - tMinus ) ) + ( axisU1[projU] * tMinus ); + + double sPlus = UNINIT, sMinus = UNINIT; + if( FloatMakePositive( sDenomPlus ) >= 1e-5 ) + { + sPlus = ( pt[projU] - axisOrigin[projU] - ( axisV0[projU] * tPlus ) ) / sDenomPlus; + } + + if( FloatMakePositive( sDenomMinus ) >= 1e-5 ) + { + sMinus = ( pt[projU] - axisOrigin[projU] - ( axisV0[projU] * tMinus ) ) / sDenomMinus; + } + + if( ( tPlus >= 0.0 ) && ( tPlus <= 1.0 ) && ( sPlus >= 0.0 ) && ( sPlus <= 1.0 ) ) + { + s = sPlus; + t = tPlus; + return; + } + + if( ( tMinus >= 0.0 ) && ( tMinus <= 1.0 ) && ( sMinus >= 0.0 ) && ( sMinus <= 1.0 ) ) + { + s = sMinus; + t = tMinus; + return; + } + + double s0, t0, s1, t1; + + s0 = sPlus; + t0 = tPlus; + if( s0 >= 1.0 ) { s0 -= 1.0; } + if( t0 >= 1.0 ) { t0 -= 1.0; } + + s1 = sMinus; + t1 = tMinus; + if( s1 >= 1.0 ) { s1 -= 1.0; } + if( t1 >= 1.0 ) { t1 -= 1.0; } + + s0 = FloatMakePositive( s0 ); + t0 = FloatMakePositive( t0 ); + s1 = FloatMakePositive( s1 ); + t1 = FloatMakePositive( t1 ); + + double max0, max1; + max0 = s0; + if( t0 > max0 ) { max0 = t0; } + max1 = s1; + if( t1 > max1 ) { max1 = t1; } + + if( max0 > max1 ) + { + s = sMinus; + t = tMinus; + } + else + { + s = sPlus; + t = tPlus; + } +} + + +//----------------------------------------------------------------------------- +// +//----------------------------------------------------------------------------- + +QuadBarycentricRetval_t PointInQuadToBarycentric( const Vector &v1, const Vector &v2, + const Vector &v3, const Vector &v4, const Vector &point, Vector2D &uv ) +{ +#define PIQ_TEXTURE_EPSILON 0.001 +#define PIQ_PLANE_EPSILON 0.1 +#define PIQ_DOT_EPSILON 0.99f + + // + // Think of a quad with points v1, v2, v3, v4 and u, v line segments + // u0 = v2 - v1 + // u1 = v3 - v4 + // v0 = v4 - v1 + // v1 = v3 - v2 + // + Vector axisU[2], axisV[2]; + Vector axisUNorm[2], axisVNorm[2]; + axisU[0] = axisUNorm[0] = v2 - v1; + axisU[1] = axisUNorm[1] = v3 - v4; + axisV[0] = axisVNorm[0] = v4 - v1; + axisV[1] = axisVNorm[1] = v3 - v2; + + float lengthU[2], lengthV[2]; + lengthU[0] = VectorNormalize( axisUNorm[0] ); + lengthU[1] = VectorNormalize( axisUNorm[1] ); + lengthV[0] = VectorNormalize( axisVNorm[0] ); + lengthV[1] = VectorNormalize( axisVNorm[1] ); + + // + // check for an early out - parallel opposite edges! + // NOTE: quad property if 1 set of opposite edges is parallel and equal + // in length, then the other set of edges is as well + // + if( axisUNorm[0].Dot( axisUNorm[1] ) > PIQ_DOT_EPSILON ) + { + if( FloatMakePositive( lengthU[0] - lengthU[1] ) < PIQ_PLANE_EPSILON ) + { + return QuadWithParallelEdges( v1, axisUNorm[0], lengthU[0], axisVNorm[0], lengthV[0], point, uv ); + } + } + + // + // since we are solving for s in our equations below we need to ensure that + // the v axes are non-parallel + // + bool bFlipped = false; + if( axisVNorm[0].Dot( axisVNorm[1] ) > PIQ_DOT_EPSILON ) + { + Vector tmp[2]; + tmp[0] = axisV[0]; + tmp[1] = axisV[1]; + axisV[0] = axisU[0]; + axisV[1] = axisU[1]; + axisU[0] = tmp[0]; + axisU[1] = tmp[1]; + bFlipped = true; + } + + // + // get the "projection" axes + // + Vector2D projAxes; + Vector vNormal = axisU[0].Cross( axisV[0] ); + GetNonMajorAxes( vNormal, projAxes ); + + // + // NOTE: axisU[0][projAxes[0]] < axisU[0][projAxes[1]], + // this is done to decrease error when dividing later + // + if( FloatMakePositive( axisU[0][projAxes[0]] ) < FloatMakePositive( axisU[0][projAxes[1]] ) ) + { + int tmp = projAxes[0]; + projAxes[0] = projAxes[1]; + projAxes[1] = tmp; + } + + // Here's how we got these equations: + // + // Given the points and u,v line segments above... + // + // Then: + // + // (1.0) PT = P0 + U0 * s + V * t + // + // where + // + // (1.1) V = V0 + s * (V1 - V0) + // (1.2) U = U0 + t * (U1 - U0) + // + // Therefore (from 1.1 + 1.0): + // PT - P0 = U0 * s + (V0 + s * (V1-V0)) * t + // Group s's: + // PT - P0 - t * V0 = s * (U0 + t * (V1-V0)) + // Two equations and two unknowns in x and y get you the following quadratic: + // + // solve the quadratic + // + double s = 0.0, t = 0.0; + double A, negB, C; + + A = ( axisU[0][projAxes[1]] * axisV[0][projAxes[0]] ) - + ( axisU[0][projAxes[0]] * axisV[0][projAxes[1]] ) - + ( axisU[1][projAxes[1]] * axisV[0][projAxes[0]] ) + + ( axisU[1][projAxes[0]] * axisV[0][projAxes[1]] ); + C = ( v1[projAxes[1]] * axisU[0][projAxes[0]] ) - + ( point[projAxes[1]] * axisU[0][projAxes[0]] ) - + ( v1[projAxes[0]] * axisU[0][projAxes[1]] ) + + ( point[projAxes[0]] * axisU[0][projAxes[1]] ); + negB = C - + ( v1[projAxes[1]] * axisU[1][projAxes[0]] ) + + ( point[projAxes[1]] * axisU[1][projAxes[0]] ) + + ( v1[projAxes[0]] * axisU[1][projAxes[1]] ) - + ( point[projAxes[0]] * axisU[1][projAxes[1]] ) + + ( axisU[0][projAxes[1]] * axisV[0][projAxes[0]] ) - + ( axisU[0][projAxes[0]] * axisV[0][projAxes[1]] ); + + if( ( A > -PIQ_PLANE_EPSILON ) && ( A < PIQ_PLANE_EPSILON ) ) + { + // shouldn't be here -- this should have been take care of in the "early out" +// Assert( 0 ); + + Vector vecUAvg, vecVAvg; + vecUAvg = ( axisUNorm[0] + axisUNorm[1] ) * 0.5f; + vecVAvg = ( axisVNorm[0] + axisVNorm[1] ) * 0.5f; + + float fLengthUAvg = ( lengthU[0] + lengthU[1] ) * 0.5f; + float fLengthVAvg = ( lengthV[0] + lengthV[1] ) * 0.5f; + + return QuadWithParallelEdges( v1, vecUAvg, fLengthUAvg, vecVAvg, fLengthVAvg, point, uv ); + +#if 0 + // legacy code -- kept here for completeness! + + // not a quadratic -- solve linearly + t = C / negB; + + // See (1.2) above + float ui = axisU[0][projAxes[0]] + t * ( axisU[1][projAxes[0]] - axisU[0][projAxes[0]] ); + if( FloatMakePositive( ui ) >= 1e-5 ) + { + // See (1.0) above + s = ( point[projAxes[0]] - v1[projAxes[0]] - axisV[0][projAxes[0]] * t ) / ui; + } +#endif + } + else + { + // (-b +/- sqrt( b^2 - 4ac )) / 2a + double discriminant = (negB*negB) - (4.0f * A * C); + if( discriminant < 0.0f ) + { + uv[0] = -99999.0f; + uv[1] = -99999.0f; + return BARY_QUADRATIC_NEGATIVE_DISCRIMINANT; + } + + double quad = sqrt( discriminant ); + double QPlus = ( negB + quad ) / ( 2.0f * A ); + double QMinus = ( negB - quad ) / ( 2.0f * A ); + + ResolveQuadratic( QPlus, QMinus, axisU[0], axisU[1], axisV[0], axisV[1], v1, point, projAxes[0], s, t ); + } + + if( !bFlipped ) + { + uv[0] = ( float )s; + uv[1] = ( float )t; + } + else + { + uv[0] = ( float )t; + uv[1] = ( float )s; + } + + // inside of the quad?? + if( ( uv[0] < 0.0f ) || ( uv[0] > 1.0f ) || ( uv[1] < 0.0f ) || ( uv[1] > 1.0f ) ) + return BARY_QUADRATIC_FALSE; + + return BARY_QUADRATIC_TRUE; + +#undef PIQ_TEXTURE_EPSILON +#undef PIQ_PLANE_EPSILON +} + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void PointInQuadFromBarycentric( const Vector &v1, const Vector &v2, const Vector &v3, const Vector &v4, + const Vector2D &uv, Vector &point ) +{ + // + // Think of a quad with points v1, v2, v3, v4 and u, v line segments + // find the ray from v0 edge to v1 edge at v + // + Vector vPts[2]; + VectorLerp( v1, v4, uv[1], vPts[0] ); + VectorLerp( v2, v3, uv[1], vPts[1] ); + VectorLerp( vPts[0], vPts[1], uv[0], point ); +} + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void TexCoordInQuadFromBarycentric( const Vector2D &v1, const Vector2D &v2, const Vector2D &v3, const Vector2D &v4, + const Vector2D &uv, Vector2D &texCoord ) +{ + // + // Think of a quad with points v1, v2, v3, v4 and u, v line segments + // find the ray from v0 edge to v1 edge at v + // + Vector2D vCoords[2]; + Vector2DLerp( v1, v4, uv[1], vCoords[0] ); + Vector2DLerp( v2, v3, uv[1], vCoords[1] ); + Vector2DLerp( vCoords[0], vCoords[1], uv[0], texCoord ); +} + + +//----------------------------------------------------------------------------- +// Compute point from barycentric specification +// Edge u goes from v0 to v1, edge v goes from v0 to v2 +//----------------------------------------------------------------------------- +void ComputePointFromBarycentric( const Vector& v0, const Vector& v1, const Vector& v2, + float u, float v, Vector& pt ) +{ + Vector edgeU, edgeV; + VectorSubtract( v1, v0, edgeU ); + VectorSubtract( v2, v0, edgeV ); + VectorMA( v0, u, edgeU, pt ); + VectorMA( pt, v, edgeV, pt ); +} + +void ComputePointFromBarycentric( const Vector2D& v0, const Vector2D& v1, const Vector2D& v2, + float u, float v, Vector2D& pt ) +{ + Vector2D edgeU, edgeV; + Vector2DSubtract( v1, v0, edgeU ); + Vector2DSubtract( v2, v0, edgeV ); + Vector2DMA( v0, u, edgeU, pt ); + Vector2DMA( pt, v, edgeV, pt ); +} + + +//----------------------------------------------------------------------------- +// Compute a matrix that has the correct orientation but which has an origin at +// the center of the bounds +//----------------------------------------------------------------------------- +static void ComputeCenterMatrix( const Vector& origin, const QAngle& angles, + const Vector& mins, const Vector& maxs, matrix3x4_t& matrix ) +{ + Vector centroid; + VectorAdd( mins, maxs, centroid ); + centroid *= 0.5f; + AngleMatrix( angles, matrix ); + + Vector worldCentroid; + VectorRotate( centroid, matrix, worldCentroid ); + worldCentroid += origin; + MatrixSetColumn( worldCentroid, 3, matrix ); +} + +static void ComputeCenterIMatrix( const Vector& origin, const QAngle& angles, + const Vector& mins, const Vector& maxs, matrix3x4_t& matrix ) +{ + Vector centroid; + VectorAdd( mins, maxs, centroid ); + centroid *= -0.5f; + AngleIMatrix( angles, matrix ); + + // For the translational component here, note that the origin in world space + // is T = R * C + O, (R = rotation matrix, C = centroid in local space, O = origin in world space) + // The IMatrix translation = - transpose(R) * T = -C - transpose(R) * 0 + Vector localOrigin; + VectorRotate( origin, matrix, localOrigin ); + centroid -= localOrigin; + MatrixSetColumn( centroid, 3, matrix ); +} + + +//----------------------------------------------------------------------------- +// Compute a matrix which is the absolute value of another +//----------------------------------------------------------------------------- +static inline void ComputeAbsMatrix( const matrix3x4_t& in, matrix3x4_t& out ) +{ + FloatBits(out[0][0]) = FloatAbsBits(in[0][0]); + FloatBits(out[0][1]) = FloatAbsBits(in[0][1]); + FloatBits(out[0][2]) = FloatAbsBits(in[0][2]); + FloatBits(out[1][0]) = FloatAbsBits(in[1][0]); + FloatBits(out[1][1]) = FloatAbsBits(in[1][1]); + FloatBits(out[1][2]) = FloatAbsBits(in[1][2]); + FloatBits(out[2][0]) = FloatAbsBits(in[2][0]); + FloatBits(out[2][1]) = FloatAbsBits(in[2][1]); + FloatBits(out[2][2]) = FloatAbsBits(in[2][2]); +} + + +//----------------------------------------------------------------------------- +// Compute a separating plane between two boxes (expensive!) +// Returns false if no separating plane exists +//----------------------------------------------------------------------------- +static bool ComputeSeparatingPlane( const matrix3x4_t &worldToBox1, const matrix3x4_t &box2ToWorld, + const Vector& box1Size, const Vector& box2Size, float tolerance, cplane_t* pPlane ) +{ + // The various separating planes can be either + // 1) A plane parallel to one of the box face planes + // 2) A plane parallel to the cross-product of an edge from each box + + // First, compute the basis of second box in the space of the first box + // NOTE: These basis place the origin at the centroid of each box! + matrix3x4_t box2ToBox1; + ConcatTransforms( worldToBox1, box2ToWorld, box2ToBox1 ); + + // We're going to be using the origin of box2 in the space of box1 alot, + // lets extract it from the matrix.... + Vector box2Origin; + MatrixGetColumn( box2ToBox1, 3, box2Origin ); + + // Next get the absolute values of these entries and store in absbox2ToBox1. + matrix3x4_t absBox2ToBox1; + ComputeAbsMatrix( box2ToBox1, absBox2ToBox1 ); + + // There are 15 tests to make. The first 3 involve trying planes parallel + // to the faces of the first box. + + // NOTE: The algorithm here involves finding the projections of the two boxes + // onto a particular line. If the projections on the line do not overlap, + // that means that there's a plane perpendicular to the line which separates + // the two boxes; and we've therefore found a separating plane. + + // The way we check for overlay is we find the projections of the two boxes + // onto the line, and add them up. We compare the sum with the projection + // of the relative center of box2 onto the same line. + + Vector tmp; + float boxProjectionSum; + float originProjection; + + // NOTE: For these guys, we're taking advantage of the fact that the ith + // row of the box2ToBox1 is the direction of the box1 (x,y,z)-axis + // transformed into the space of box2. + + // First side of box 1 + boxProjectionSum = box1Size.x + MatrixRowDotProduct( absBox2ToBox1, 0, box2Size ); + originProjection = FloatMakePositive( box2Origin.x ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + VectorCopy( worldToBox1[0], pPlane->normal.Base() ); + return true; + } + + // Second side of box 1 + boxProjectionSum = box1Size.y + MatrixRowDotProduct( absBox2ToBox1, 1, box2Size ); + originProjection = FloatMakePositive( box2Origin.y ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + VectorCopy( worldToBox1[1], pPlane->normal.Base() ); + return true; + } + + // Third side of box 1 + boxProjectionSum = box1Size.z + MatrixRowDotProduct( absBox2ToBox1, 2, box2Size ); + originProjection = FloatMakePositive( box2Origin.z ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + VectorCopy( worldToBox1[2], pPlane->normal.Base() ); + return true; + } + + // The next three involve checking splitting planes parallel to the + // faces of the second box. + + // NOTE: For these guys, we're taking advantage of the fact that the 0th + // column of the box2ToBox1 is the direction of the box2 x-axis + // transformed into the space of box1. + // Here, we're determining the distance of box2's center from box1's center + // by projecting it onto a line parallel to box2's axis + + // First side of box 2 + boxProjectionSum = box2Size.x + MatrixColumnDotProduct( absBox2ToBox1, 0, box1Size ); + originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 0, box2Origin ) ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 0, pPlane->normal ); + return true; + } + + // Second side of box 2 + boxProjectionSum = box2Size.y + MatrixColumnDotProduct( absBox2ToBox1, 1, box1Size ); + originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 1, box2Origin ) ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 1, pPlane->normal ); + return true; + } + + // Third side of box 2 + boxProjectionSum = box2Size.z + MatrixColumnDotProduct( absBox2ToBox1, 2, box1Size ); + originProjection = FloatMakePositive( MatrixColumnDotProduct( box2ToBox1, 2, box2Origin ) ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 2, pPlane->normal ); + return true; + } + + // Next check the splitting planes which are orthogonal to the pairs + // of edges, one from box1 and one from box2. As only direction matters, + // there are 9 pairs since each box has 3 distinct edge directions. + + // Here, we take advantage of the fact that the edges from box 1 are all + // axis aligned; therefore the crossproducts are simplified. Let's walk through + // the example of b1e1 x b2e1: + + // In this example, the line to check is perpendicular to b1e1 + b2e2 + // we can compute this line by taking the cross-product: + // + // [ i j k ] + // [ 1 0 0 ] = - ez j + ey k = l1 + // [ ex ey ez ] + + // Where ex, ey, ez is the components of box2's x axis in the space of box 1, + // which is == to the 0th column of of box2toBox1 + + // The projection of box1 onto this line = the absolute dot product of the box size + // against the line, which = + // AbsDot( box1Size, l1 ) = abs( -ez * box1.y ) + abs( ey * box1.z ) + + // To compute the projection of box2 onto this line, we'll do it in the space of box 2 + // + // [ i j k ] + // [ fx fy fz ] = fz j - fy k = l2 + // [ 1 0 0 ] + + // Where fx, fy, fz is the components of box1's x axis in the space of box 2, + // which is == to the 0th row of of box2toBox1 + + // The projection of box2 onto this line = the absolute dot product of the box size + // against the line, which = + // AbsDot( box2Size, l2 ) = abs( fz * box2.y ) + abs ( fy * box2.z ) + + // The projection of the relative origin position on this line is done in the + // space of box 1: + // + // originProjection = DotProduct( <-ez j + ey k>, box2Origin ) = + // -ez * box2Origin.y + ey * box2Origin.z + + // NOTE: These checks can be bogus if both edges are parallel. The if + // checks at the beginning of each block are designed to catch that case + + // b1e1 x b2e1 + if ( absBox2ToBox1[0][0] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.y * absBox2ToBox1[2][0] + box1Size.z * absBox2ToBox1[1][0] + + box2Size.y * absBox2ToBox1[0][2] + box2Size.z * absBox2ToBox1[0][1]; + originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][0] + box2Origin.z * box2ToBox1[1][0] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 0, tmp ); + CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e1 x b2e2 + if ( absBox2ToBox1[0][1] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.y * absBox2ToBox1[2][1] + box1Size.z * absBox2ToBox1[1][1] + + box2Size.x * absBox2ToBox1[0][2] + box2Size.z * absBox2ToBox1[0][0]; + originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][1] + box2Origin.z * box2ToBox1[1][1] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 1, tmp ); + CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e1 x b2e3 + if ( absBox2ToBox1[0][2] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.y * absBox2ToBox1[2][2] + box1Size.z * absBox2ToBox1[1][2] + + box2Size.x * absBox2ToBox1[0][1] + box2Size.y * absBox2ToBox1[0][0]; + originProjection = FloatMakePositive( -box2Origin.y * box2ToBox1[2][2] + box2Origin.z * box2ToBox1[1][2] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 2, tmp ); + CrossProduct( worldToBox1[0], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e2 x b2e1 + if ( absBox2ToBox1[1][0] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.x * absBox2ToBox1[2][0] + box1Size.z * absBox2ToBox1[0][0] + + box2Size.y * absBox2ToBox1[1][2] + box2Size.z * absBox2ToBox1[1][1]; + originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][0] - box2Origin.z * box2ToBox1[0][0] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 0, tmp ); + CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e2 x b2e2 + if ( absBox2ToBox1[1][1] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.x * absBox2ToBox1[2][1] + box1Size.z * absBox2ToBox1[0][1] + + box2Size.x * absBox2ToBox1[1][2] + box2Size.z * absBox2ToBox1[1][0]; + originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][1] - box2Origin.z * box2ToBox1[0][1] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 1, tmp ); + CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e2 x b2e3 + if ( absBox2ToBox1[1][2] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.x * absBox2ToBox1[2][2] + box1Size.z * absBox2ToBox1[0][2] + + box2Size.x * absBox2ToBox1[1][1] + box2Size.y * absBox2ToBox1[1][0]; + originProjection = FloatMakePositive( box2Origin.x * box2ToBox1[2][2] - box2Origin.z * box2ToBox1[0][2] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 2, tmp ); + CrossProduct( worldToBox1[1], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e3 x b2e1 + if ( absBox2ToBox1[2][0] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.x * absBox2ToBox1[1][0] + box1Size.y * absBox2ToBox1[0][0] + + box2Size.y * absBox2ToBox1[2][2] + box2Size.z * absBox2ToBox1[2][1]; + originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][0] + box2Origin.y * box2ToBox1[0][0] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 0, tmp ); + CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e3 x b2e2 + if ( absBox2ToBox1[2][1] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.x * absBox2ToBox1[1][1] + box1Size.y * absBox2ToBox1[0][1] + + box2Size.x * absBox2ToBox1[2][2] + box2Size.z * absBox2ToBox1[2][0]; + originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][1] + box2Origin.y * box2ToBox1[0][1] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 1, tmp ); + CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + + // b1e3 x b2e3 + if ( absBox2ToBox1[2][2] < 1.0f - 1e-3f ) + { + boxProjectionSum = + box1Size.x * absBox2ToBox1[1][2] + box1Size.y * absBox2ToBox1[0][2] + + box2Size.x * absBox2ToBox1[2][1] + box2Size.y * absBox2ToBox1[2][0]; + originProjection = FloatMakePositive( -box2Origin.x * box2ToBox1[1][2] + box2Origin.y * box2ToBox1[0][2] ) + tolerance; + if ( FloatBits(originProjection) > FloatBits(boxProjectionSum) ) + { + MatrixGetColumn( box2ToWorld, 2, tmp ); + CrossProduct( worldToBox1[2], tmp.Base(), pPlane->normal.Base() ); + return true; + } + } + return false; +} + + +//----------------------------------------------------------------------------- +// Compute a separating plane between two boxes (expensive!) +// Returns false if no separating plane exists +//----------------------------------------------------------------------------- +bool ComputeSeparatingPlane( const Vector& org1, const QAngle& angles1, const Vector& min1, const Vector& max1, + const Vector& org2, const QAngle& angles2, const Vector& min2, const Vector& max2, + float tolerance, cplane_t* pPlane ) +{ + matrix3x4_t worldToBox1, box2ToWorld; + ComputeCenterIMatrix( org1, angles1, min1, max1, worldToBox1 ); + ComputeCenterMatrix( org2, angles2, min2, max2, box2ToWorld ); + + // Then compute the size of the two boxes + Vector box1Size, box2Size; + VectorSubtract( max1, min1, box1Size ); + VectorSubtract( max2, min2, box2Size ); + box1Size *= 0.5f; + box2Size *= 0.5f; + + return ComputeSeparatingPlane( worldToBox1, box2ToWorld, box1Size, box2Size, tolerance, pPlane ); +} + + +//----------------------------------------------------------------------------- +// Swept OBB test +//----------------------------------------------------------------------------- +bool IsRayIntersectingOBB( const Ray_t &ray, const Vector& org, const QAngle& angles, + const Vector& mins, const Vector& maxs ) +{ + if ( angles == vec3_angle ) + { + Vector vecWorldMins, vecWorldMaxs; + VectorAdd( org, mins, vecWorldMins ); + VectorAdd( org, maxs, vecWorldMaxs ); + return IsBoxIntersectingRay( vecWorldMins, vecWorldMaxs, ray ); + } + + if ( ray.m_IsRay ) + { + matrix3x4_t worldToBox; + AngleIMatrix( angles, org, worldToBox ); + + Ray_t rotatedRay; + VectorTransform( ray.m_Start, worldToBox, rotatedRay.m_Start ); + VectorRotate( ray.m_Delta, worldToBox, rotatedRay.m_Delta ); + rotatedRay.m_StartOffset = vec3_origin; + rotatedRay.m_Extents = vec3_origin; + rotatedRay.m_IsRay = ray.m_IsRay; + rotatedRay.m_IsSwept = ray.m_IsSwept; + + return IsBoxIntersectingRay( mins, maxs, rotatedRay ); + } + + if ( !ray.m_IsSwept ) + { + cplane_t plane; + return ComputeSeparatingPlane( ray.m_Start, vec3_angle, -ray.m_Extents, ray.m_Extents, + org, angles, mins, maxs, 0.0f, &plane ) == false; + } + + // NOTE: See the comments in ComputeSeparatingPlane to understand this math + + // First, compute the basis of box in the space of the ray + // NOTE: These basis place the origin at the centroid of each box! + matrix3x4_t worldToBox1, box2ToWorld; + ComputeCenterMatrix( org, angles, mins, maxs, box2ToWorld ); + + // Find the center + extents of an AABB surrounding the ray + Vector vecRayCenter; + VectorMA( ray.m_Start, 0.5, ray.m_Delta, vecRayCenter ); + vecRayCenter *= -1.0f; + SetIdentityMatrix( worldToBox1 ); + MatrixSetColumn( vecRayCenter, 3, worldToBox1 ); + + Vector box1Size; + box1Size.x = ray.m_Extents.x + FloatMakePositive( ray.m_Delta.x ) * 0.5f; + box1Size.y = ray.m_Extents.y + FloatMakePositive( ray.m_Delta.y ) * 0.5f; + box1Size.z = ray.m_Extents.z + FloatMakePositive( ray.m_Delta.z ) * 0.5f; + + // Then compute the size of the box + Vector box2Size; + VectorSubtract( maxs, mins, box2Size ); + box2Size *= 0.5f; + + // Do an OBB test of the box with the AABB surrounding the ray + cplane_t plane; + if ( ComputeSeparatingPlane( worldToBox1, box2ToWorld, box1Size, box2Size, 0.0f, &plane ) ) + return false; + + // Now deal with the planes which are the cross products of the ray sweep direction vs box edges + Vector vecRayDirection = ray.m_Delta; + VectorNormalize( vecRayDirection ); + + // Need a vector between ray center vs box center measured in the space of the ray (world) + Vector vecCenterDelta; + vecCenterDelta.x = box2ToWorld[0][3] - ray.m_Start.x; + vecCenterDelta.y = box2ToWorld[1][3] - ray.m_Start.y; + vecCenterDelta.z = box2ToWorld[2][3] - ray.m_Start.z; + + // Rotate the ray direction into the space of the OBB + Vector vecAbsRayDirBox2; + VectorIRotate( vecRayDirection, box2ToWorld, vecAbsRayDirBox2 ); + + // Make abs versions of the ray in world space + ray in box2 space + VectorAbs( vecAbsRayDirBox2, vecAbsRayDirBox2 ); + + // Now do the work for the planes which are perpendicular to the edges of the AABB + // and the sweep direction edges... + + // In this example, the line to check is perpendicular to box edge x + ray delta + // we can compute this line by taking the cross-product: + // + // [ i j k ] + // [ 1 0 0 ] = - dz j + dy k = l1 + // [ dx dy dz ] + + // Where dx, dy, dz is the ray delta (normalized) + + // The projection of the box onto this line = the absolute dot product of the box size + // against the line, which = + // AbsDot( vecBoxHalfDiagonal, l1 ) = abs( -dz * vecBoxHalfDiagonal.y ) + abs( dy * vecBoxHalfDiagonal.z ) + + // Because the plane contains the sweep direction, the sweep will produce + // no extra projection onto the line normal to the plane. + // Therefore all we need to do is project the ray extents onto this line also: + // AbsDot( ray.m_Extents, l1 ) = abs( -dz * ray.m_Extents.y ) + abs( dy * ray.m_Extents.z ) + + Vector vecPlaneNormal; + + // box x x ray delta + CrossProduct( vecRayDirection, Vector( box2ToWorld[0][0], box2ToWorld[1][0], box2ToWorld[2][0] ), vecPlaneNormal ); + float flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) ); + float flBoxProjectionSum = + vecAbsRayDirBox2.z * box2Size.y + vecAbsRayDirBox2.y * box2Size.z + + DotProductAbs( vecPlaneNormal, ray.m_Extents ); + if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) ) + return false; + + // box y x ray delta + CrossProduct( vecRayDirection, Vector( box2ToWorld[0][1], box2ToWorld[1][1], box2ToWorld[2][1] ), vecPlaneNormal ); + flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) ); + flBoxProjectionSum = + vecAbsRayDirBox2.z * box2Size.x + vecAbsRayDirBox2.x * box2Size.z + + DotProductAbs( vecPlaneNormal, ray.m_Extents ); + if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) ) + return false; + + // box z x ray delta + CrossProduct( vecRayDirection, Vector( box2ToWorld[0][2], box2ToWorld[1][2], box2ToWorld[2][2] ), vecPlaneNormal ); + flCenterDeltaProjection = FloatMakePositive( DotProduct( vecPlaneNormal, vecCenterDelta ) ); + flBoxProjectionSum = + vecAbsRayDirBox2.y * box2Size.x + vecAbsRayDirBox2.x * box2Size.y + + DotProductAbs( vecPlaneNormal, ray.m_Extents ); + if ( FloatBits(flCenterDeltaProjection) > FloatBits(flBoxProjectionSum) ) + return false; + + return true; +} + +//-------------------------------------------------------------------------- +// Purpose: +// +// NOTE: +// triangle points are given in clockwise order (aabb-triangle test) +// +// 1 edge0 = 1 - 0 +// | \ edge1 = 2 - 1 +// | \ edge2 = 0 - 2 +// | \ . +// | \ . +// 0-----2 . +// +//-------------------------------------------------------------------------- + +//----------------------------------------------------------------------------- +// Purpose: find the minima and maxima of the 3 given values +//----------------------------------------------------------------------------- +inline void FindMinMax( float v1, float v2, float v3, float &min, float &max ) +{ + min = max = v1; + if ( v2 < min ) { min = v2; } + if ( v2 > max ) { max = v2; } + if ( v3 < min ) { min = v3; } + if ( v3 > max ) { max = v3; } +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +inline bool AxisTestEdgeCrossX2( float flEdgeZ, float flEdgeY, float flAbsEdgeZ, float flAbsEdgeY, + const Vector &p1, const Vector &p3, const Vector &vecExtents, + float flTolerance ) +{ + // Cross Product( axialX(1,0,0) x edge ): x = 0.0f, y = edge.z, z = -edge.y + // Triangle Point Distances: dist(x) = normal.y * pt(x).y + normal.z * pt(x).z + float flDist1 = flEdgeZ * p1.y - flEdgeY * p1.z; + float flDist3 = flEdgeZ * p3.y - flEdgeY * p3.z; + + // Extents are symmetric: dist = abs( normal.y ) * extents.y + abs( normal.z ) * extents.z + float flDistBox = flAbsEdgeZ * vecExtents.y + flAbsEdgeY * vecExtents.z; + + // Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). + if ( flDist1 < flDist3 ) + { + if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) + return false; + } + else + { + if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) + return false; + } + + return true; +} + +//-------------------------------------------------------------------------- +// Purpose: +//-------------------------------------------------------------------------- +inline bool AxisTestEdgeCrossX3( float flEdgeZ, float flEdgeY, float flAbsEdgeZ, float flAbsEdgeY, + const Vector &p1, const Vector &p2, const Vector &vecExtents, + float flTolerance ) +{ + // Cross Product( axialX(1,0,0) x edge ): x = 0.0f, y = edge.z, z = -edge.y + // Triangle Point Distances: dist(x) = normal.y * pt(x).y + normal.z * pt(x).z + float flDist1 = flEdgeZ * p1.y - flEdgeY * p1.z; + float flDist2 = flEdgeZ * p2.y - flEdgeY * p2.z; + + // Extents are symmetric: dist = abs( normal.y ) * extents.y + abs( normal.z ) * extents.z + float flDistBox = flAbsEdgeZ * vecExtents.y + flAbsEdgeY * vecExtents.z; + + // Either dist1, dist2 is the closest point to the box, determine which and test of overlap with box(AABB). + if ( flDist1 < flDist2 ) + { + if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) ) + return false; + } + else + { + if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) + return false; + } + + return true; +} + +//-------------------------------------------------------------------------- +//-------------------------------------------------------------------------- +inline bool AxisTestEdgeCrossY2( float flEdgeZ, float flEdgeX, float flAbsEdgeZ, float flAbsEdgeX, + const Vector &p1, const Vector &p3, const Vector &vecExtents, + float flTolerance ) +{ + // Cross Product( axialY(0,1,0) x edge ): x = -edge.z, y = 0.0f, z = edge.x + // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.z * pt(x).z + float flDist1 = -flEdgeZ * p1.x + flEdgeX * p1.z; + float flDist3 = -flEdgeZ * p3.x + flEdgeX * p3.z; + + // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.z ) * extents.z + float flDistBox = flAbsEdgeZ * vecExtents.x + flAbsEdgeX * vecExtents.z; + + // Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). + if ( flDist1 < flDist3 ) + { + if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) + return false; + } + else + { + if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) + return false; + } + + return true; +} + +//-------------------------------------------------------------------------- +//-------------------------------------------------------------------------- +inline bool AxisTestEdgeCrossY3( float flEdgeZ, float flEdgeX, float flAbsEdgeZ, float flAbsEdgeX, + const Vector &p1, const Vector &p2, const Vector &vecExtents, + float flTolerance ) +{ + // Cross Product( axialY(0,1,0) x edge ): x = -edge.z, y = 0.0f, z = edge.x + // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.z * pt(x).z + float flDist1 = -flEdgeZ * p1.x + flEdgeX * p1.z; + float flDist2 = -flEdgeZ * p2.x + flEdgeX * p2.z; + + // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.z ) * extents.z + float flDistBox = flAbsEdgeZ * vecExtents.x + flAbsEdgeX * vecExtents.z; + + // Either dist1, dist2 is the closest point to the box, determine which and test of overlap with box(AABB). + if ( flDist1 < flDist2 ) + { + if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) ) + return false; + } + else + { + if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) + return false; + } + + return true; +} + +//-------------------------------------------------------------------------- +//-------------------------------------------------------------------------- +inline bool AxisTestEdgeCrossZ1( float flEdgeY, float flEdgeX, float flAbsEdgeY, float flAbsEdgeX, + const Vector &p2, const Vector &p3, const Vector &vecExtents, + float flTolerance ) +{ + // Cross Product( axialZ(0,0,1) x edge ): x = edge.y, y = -edge.x, z = 0.0f + // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.y * pt(x).y + float flDist2 = flEdgeY * p2.x - flEdgeX * p2.y; + float flDist3 = flEdgeY * p3.x - flEdgeX * p3.y; + + // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.y ) * extents.y + float flDistBox = flAbsEdgeY * vecExtents.x + flAbsEdgeX * vecExtents.y; + + // Either dist2, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). + if ( flDist3 < flDist2 ) + { + if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist2 < -( flDistBox + flTolerance ) ) ) + return false; + } + else + { + if ( ( flDist2 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) + return false; + } + + return true; +} + +//-------------------------------------------------------------------------- +//-------------------------------------------------------------------------- +inline bool AxisTestEdgeCrossZ2( float flEdgeY, float flEdgeX, float flAbsEdgeY, float flAbsEdgeX, + const Vector &p1, const Vector &p3, const Vector &vecExtents, + float flTolerance ) +{ + // Cross Product( axialZ(0,0,1) x edge ): x = edge.y, y = -edge.x, z = 0.0f + // Triangle Point Distances: dist(x) = normal.x * pt(x).x + normal.y * pt(x).y + float flDist1 = flEdgeY * p1.x - flEdgeX * p1.y; + float flDist3 = flEdgeY * p3.x - flEdgeX * p3.y; + + // Extents are symmetric: dist = abs( normal.x ) * extents.x + abs( normal.y ) * extents.y + float flDistBox = flAbsEdgeY * vecExtents.x + flAbsEdgeX * vecExtents.y; + + // Either dist1, dist3 is the closest point to the box, determine which and test of overlap with box(AABB). + if ( flDist1 < flDist3 ) + { + if ( ( flDist1 > ( flDistBox + flTolerance ) ) || ( flDist3 < -( flDistBox + flTolerance ) ) ) + return false; + } + else + { + if ( ( flDist3 > ( flDistBox + flTolerance ) ) || ( flDist1 < -( flDistBox + flTolerance ) ) ) + return false; + } + + return true; +} + +//----------------------------------------------------------------------------- +// Purpose: Test for an intersection (overlap) between an axial-aligned bounding +// box (AABB) and a triangle. +// +// Using the "Separating-Axis Theorem" to test for intersections between +// a triangle and an axial-aligned bounding box (AABB). +// 1. 3 Axis Planes - x, y, z +// 2. 9 Edge Planes Tests - the 3 edges of the triangle crossed with all 3 axial +// planes (x, y, z) +// 3. 1 Face Plane - the triangle plane (cplane_t plane below) +// Output: false = separating axis (no intersection) +// true = intersection +//----------------------------------------------------------------------------- +bool IsBoxIntersectingTriangle( const Vector &vecBoxCenter, const Vector &vecBoxExtents, + const Vector &v1, const Vector &v2, const Vector &v3, + const cplane_t &plane, float flTolerance ) +{ + // Test the axial planes (x,y,z) against the min, max of the triangle. + float flMin, flMax; + Vector p1, p2, p3; + + // x plane + p1.x = v1.x - vecBoxCenter.x; + p2.x = v2.x - vecBoxCenter.x; + p3.x = v3.x - vecBoxCenter.x; + FindMinMax( p1.x, p2.x, p3.x, flMin, flMax ); + if ( ( flMin > ( vecBoxExtents.x + flTolerance ) ) || ( flMax < -( vecBoxExtents.x + flTolerance ) ) ) + return false; + + // y plane + p1.y = v1.y - vecBoxCenter.y; + p2.y = v2.y - vecBoxCenter.y; + p3.y = v3.y - vecBoxCenter.y; + FindMinMax( p1.y, p2.y, p3.y, flMin, flMax ); + if ( ( flMin > ( vecBoxExtents.y + flTolerance ) ) || ( flMax < -( vecBoxExtents.y + flTolerance ) ) ) + return false; + + // z plane + p1.z = v1.z - vecBoxCenter.z; + p2.z = v2.z - vecBoxCenter.z; + p3.z = v3.z - vecBoxCenter.z; + FindMinMax( p1.z, p2.z, p3.z, flMin, flMax ); + if ( ( flMin > ( vecBoxExtents.z + flTolerance ) ) || ( flMax < -( vecBoxExtents.z + flTolerance ) ) ) + return false; + + // Test the 9 edge cases. + Vector vecEdge, vecAbsEdge; + + // edge 0 (cross x,y,z) + vecEdge = p2 - p1; + vecAbsEdge.y = FloatMakePositive( vecEdge.y ); + vecAbsEdge.z = FloatMakePositive( vecEdge.z ); + if ( !AxisTestEdgeCrossX2( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p3, vecBoxExtents, flTolerance ) ) + return false; + + vecAbsEdge.x = FloatMakePositive( vecEdge.x ); + if ( !AxisTestEdgeCrossY2( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p3, vecBoxExtents, flTolerance ) ) + return false; + + if ( !AxisTestEdgeCrossZ1( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p2, p3, vecBoxExtents, flTolerance ) ) + return false; + + // edge 1 (cross x,y,z) + vecEdge = p3 - p2; + vecAbsEdge.y = FloatMakePositive( vecEdge.y ); + vecAbsEdge.z = FloatMakePositive( vecEdge.z ); + if ( !AxisTestEdgeCrossX2( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p2, vecBoxExtents, flTolerance ) ) + return false; + + vecAbsEdge.x = FloatMakePositive( vecEdge.x ); + if ( !AxisTestEdgeCrossY2( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p2, vecBoxExtents, flTolerance ) ) + return false; + + if ( !AxisTestEdgeCrossZ2( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p1, p3, vecBoxExtents, flTolerance ) ) + return false; + + // edge 2 (cross x,y,z) + vecEdge = p1 - p3; + vecAbsEdge.y = FloatMakePositive( vecEdge.y ); + vecAbsEdge.z = FloatMakePositive( vecEdge.z ); + if ( !AxisTestEdgeCrossX3( vecEdge.z, vecEdge.y, vecAbsEdge.z, vecAbsEdge.y, p1, p2, vecBoxExtents, flTolerance ) ) + return false; + + vecAbsEdge.x = FloatMakePositive( vecEdge.x ); + if ( !AxisTestEdgeCrossY3( vecEdge.z, vecEdge.x, vecAbsEdge.z, vecAbsEdge.x, p1, p2, vecBoxExtents, flTolerance ) ) + return false; + + if ( !AxisTestEdgeCrossZ1( vecEdge.y, vecEdge.x, vecAbsEdge.y, vecAbsEdge.x, p2, p3, vecBoxExtents, flTolerance ) ) + return false; + + // Test against the triangle face plane. + Vector vecMin, vecMax; + VectorSubtract( vecBoxCenter, vecBoxExtents, vecMin ); + VectorAdd( vecBoxCenter, vecBoxExtents, vecMax ); + if ( BoxOnPlaneSide( vecMin, vecMax, &plane ) != 3 ) + return false; + + return true; +} + +// NOTE: JAY: This is untested code based on Real-time Collision Detection by Ericson +#if 0 +Vector CalcClosestPointOnTriangle( const Vector &P, const Vector &v0, const Vector &v1, const Vector &v2 ) +{ + Vector e0 = v1 - v0; + Vector e1 = v2 - v0; + Vector p0 = P - v0; + + // voronoi region of v0 + float d1 = DotProduct( e0, p0 ); + float d2 = DotProduct( e1, p0 ); + if (d1 <= 0.0f && d2 <= 0.0f) + return v0; + + // voronoi region of v1 + Vector p1 = P - v1; + float d3 = DotProduct( e0, p1 ); + float d4 = DotProduct( e1, p1 ); + if (d3 >=0.0f && d4 <= d3) + return v1; + + // voronoi region of e0 (v0-v1) + float ve2 = d1*d4 - d3*d2; + if ( ve2 <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + float v = d1 / (d1-d3); + return v0 + v * e0; + } + // voronoi region of v2 + Vector p2 = P - v2; + float d5 = DotProduct( e0, p2 ); + float d6 = DotProduct( e1, p2 ); + if (d6 >= 0.0f && d5 <= d6) + return v2; + // voronoi region of e1 + float ve1 = d5*d2 - d1*d6; + if (ve1 <= 0.0f && d2 >= 0.0f && d6 >= 0.0f) + { + float w = d2 / (d2-d6); + return v0 + w * e1; + } + // voronoi region on e2 + float ve0 = d3*d6 - d5*d4; + if ( ve0 <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) + { + float w = (d4-d3)/((d4-d3) + (d5-d6)); + return v1 + w * (v2-v1); + } + // voronoi region of v0v1v2 triangle + float denom = 1.0f / (ve0+ve1+ve2); + float v = ve1*denom; + float w = ve2 * denom; + return v0 + e0 * v + e1 * w; +} +#endif + + +bool OBBHasFullyContainedIntersectionWithQuad( const Vector &vOBBExtent1_Scaled, const Vector &vOBBExtent2_Scaled, const Vector &vOBBExtent3_Scaled, const Vector &ptOBBCenter, + const Vector &vQuadNormal, float fQuadPlaneDist, const Vector &ptQuadCenter, + const Vector &vQuadExtent1_Normalized, float fQuadExtent1Length, + const Vector &vQuadExtent2_Normalized, float fQuadExtent2Length ) +{ + Vector ptOBB[8]; //this specific ordering helps us web out from a point to its 3 connecting points with some bit math (most importantly, no if's) + ptOBB[0] = ptOBBCenter - vOBBExtent1_Scaled - vOBBExtent2_Scaled - vOBBExtent3_Scaled; + ptOBB[1] = ptOBBCenter - vOBBExtent1_Scaled - vOBBExtent2_Scaled + vOBBExtent3_Scaled; + ptOBB[2] = ptOBBCenter - vOBBExtent1_Scaled + vOBBExtent2_Scaled + vOBBExtent3_Scaled; + ptOBB[3] = ptOBBCenter - vOBBExtent1_Scaled + vOBBExtent2_Scaled - vOBBExtent3_Scaled; + ptOBB[4] = ptOBBCenter + vOBBExtent1_Scaled - vOBBExtent2_Scaled - vOBBExtent3_Scaled; + ptOBB[5] = ptOBBCenter + vOBBExtent1_Scaled - vOBBExtent2_Scaled + vOBBExtent3_Scaled; + ptOBB[6] = ptOBBCenter + vOBBExtent1_Scaled + vOBBExtent2_Scaled + vOBBExtent3_Scaled; + ptOBB[7] = ptOBBCenter + vOBBExtent1_Scaled + vOBBExtent2_Scaled - vOBBExtent3_Scaled; + + float fDists[8]; + for( int i = 0; i != 8; ++i ) + fDists[i] = vQuadNormal.Dot( ptOBB[i] ) - fQuadPlaneDist; + + int iSides[8]; + int iSideMask = 0; + for( int i = 0; i != 8; ++i ) + { + if( fDists[i] > 0.0f ) + { + iSides[i] = 1; + iSideMask |= 1; + } + else + { + iSides[i] = 2; + iSideMask |= 2; + } + } + + if( iSideMask != 3 ) //points reside entirely on one side of the quad's plane + return false; + + Vector ptPlaneIntersections[12]; //only have 12 lines, can only possibly generate 12 split points + int iPlaneIntersectionsCount = 0; + + for( int i = 0; i != 8; ++i ) + { + if( iSides[i] == 2 ) //point behind the plane + { + int iAxisCrossings[3]; + iAxisCrossings[0] = i ^ 4; //upper 4 vs lower 4 crosses vOBBExtent1 axis + iAxisCrossings[1] = ((i + 1) & 3) + (i & 4); //cycle to the next element while staying within the upper 4 or lower 4, this will cross either vOBBExtent2 or vOBBExtent3 axis, we don't care which + iAxisCrossings[2] = ((i - 1) & 3) + (i & 4); //cylce to the previous element while staying within the upper 4 or lower 4, this will cross the axis iAxisCrossings[1] didn't cross + + for( int j = 0; j != 3; ++j ) + { + if( iSides[iAxisCrossings[j]] == 1 ) //point in front of the plane + { + //line between ptOBB[i] and ptOBB[iAxisCrossings[j]] intersects the plane, generate a point at the intersection for further testing + float fTotalDist = fDists[iAxisCrossings[j]] - fDists[i]; //remember that fDists[i] is a negative value + ptPlaneIntersections[iPlaneIntersectionsCount] = (ptOBB[iAxisCrossings[j]] * (-fDists[i]/fTotalDist)) + (ptOBB[i] * (fDists[iAxisCrossings[j]]/fTotalDist)); + + Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane + + ++iPlaneIntersectionsCount; + } + } + } + } + + Assert( iPlaneIntersectionsCount != 0 ); + + for( int i = 0; i != iPlaneIntersectionsCount; ++i ) + { + //these points are guaranteed to be on the plane, now just check to see if they're within the quad's extents + Vector vToPointFromQuadCenter = ptPlaneIntersections[i] - ptQuadCenter; + + float fExt1Dist = vQuadExtent1_Normalized.Dot( vToPointFromQuadCenter ); + if( fabs( fExt1Dist ) > fQuadExtent1Length ) + return false; //point is outside boundaries + + //vToPointFromQuadCenter -= vQuadExtent1_Normalized * fExt1Dist; //to handle diamond shaped quads + + float fExt2Dist = vQuadExtent2_Normalized.Dot( vToPointFromQuadCenter ); + if( fabs( fExt2Dist ) > fQuadExtent2Length ) + return false; //point is outside boundaries + } + + return true; //there were lines crossing the quad plane, and every line crossing that plane had its intersection with the plane within the quad's boundaries +} + +//----------------------------------------------------------------------------- +// Compute if the Ray intersects the quad plane, and whether the entire +// Ray/Quad intersection is contained within the quad itself +// +// False if no intersection exists, or if part of the intersection is +// outside the quad's extents +//----------------------------------------------------------------------------- +bool RayHasFullyContainedIntersectionWithQuad( const Ray_t &ray, + const Vector &vQuadNormal, float fQuadPlaneDist, const Vector &ptQuadCenter, + const Vector &vQuadExtent1_Normalized, float fQuadExtent1Length, + const Vector &vQuadExtent2_Normalized, float fQuadExtent2Length ) +{ + Vector ptPlaneIntersections[(12 + 12 + 8)]; //absolute max possible: 12 lines to connect the start box, 12 more to connect the end box, 8 to connect the boxes to eachother + + //8 points to make an AABB, 8 lines to connect each point from it's start to end point along the ray, 8 possible intersections + int iPlaneIntersectionsCount = 0; + + if( ray.m_IsRay ) + { + //just 1 line + if( ray.m_IsSwept ) + { + Vector ptEndPoints[2]; + ptEndPoints[0] = ray.m_Start; + ptEndPoints[1] = ptEndPoints[0] + ray.m_Delta; + + int i; + float fDists[2]; + for( i = 0; i != 2; ++i ) + fDists[i] = vQuadNormal.Dot( ptEndPoints[i] ) - fQuadPlaneDist; + + for( i = 0; i != 2; ++i ) + { + if( fDists[i] <= 0.0f ) + { + int j = 1-i; + if( fDists[j] >= 0.0f ) + { + float fInvTotalDist = 1.0f / (fDists[j] - fDists[i]); //fDists[i] <= 0, ray is swept so no chance that the denom was 0 + ptPlaneIntersections[0] = (ptEndPoints[i] * (fDists[j] * fInvTotalDist)) - (ptEndPoints[j] * (fDists[i] * fInvTotalDist)); //fDists[i] <= 0 + Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane + iPlaneIntersectionsCount = 1; + } + else + { + return false; + } + break; + } + } + + if( i == 2 ) + return false; + } + else //not swept, so this is actually a point on quad question + { + if( fabs( vQuadNormal.Dot( ray.m_Start ) - fQuadPlaneDist ) < 1e-6 ) + { + ptPlaneIntersections[0] = ray.m_Start; + iPlaneIntersectionsCount = 1; + } + else + { + return false; + } + } + } + else + { + Vector ptEndPoints[2][8]; + //this specific ordering helps us web out from a point to its 3 connecting points with some bit math (most importantly, no if's) + ptEndPoints[0][0] = ray.m_Start; ptEndPoints[0][0].x -= ray.m_Extents.x; ptEndPoints[0][0].y -= ray.m_Extents.y; ptEndPoints[0][0].z -= ray.m_Extents.z; + ptEndPoints[0][1] = ray.m_Start; ptEndPoints[0][1].x -= ray.m_Extents.x; ptEndPoints[0][1].y -= ray.m_Extents.y; ptEndPoints[0][1].z += ray.m_Extents.z; + ptEndPoints[0][2] = ray.m_Start; ptEndPoints[0][2].x -= ray.m_Extents.x; ptEndPoints[0][2].y += ray.m_Extents.y; ptEndPoints[0][2].z += ray.m_Extents.z; + ptEndPoints[0][3] = ray.m_Start; ptEndPoints[0][3].x -= ray.m_Extents.x; ptEndPoints[0][3].y += ray.m_Extents.y; ptEndPoints[0][3].z -= ray.m_Extents.z; + ptEndPoints[0][4] = ray.m_Start; ptEndPoints[0][4].x += ray.m_Extents.x; ptEndPoints[0][4].y -= ray.m_Extents.y; ptEndPoints[0][4].z -= ray.m_Extents.z; + ptEndPoints[0][5] = ray.m_Start; ptEndPoints[0][5].x += ray.m_Extents.x; ptEndPoints[0][5].y -= ray.m_Extents.y; ptEndPoints[0][5].z += ray.m_Extents.z; + ptEndPoints[0][6] = ray.m_Start; ptEndPoints[0][6].x += ray.m_Extents.x; ptEndPoints[0][6].y += ray.m_Extents.y; ptEndPoints[0][6].z += ray.m_Extents.z; + ptEndPoints[0][7] = ray.m_Start; ptEndPoints[0][7].x += ray.m_Extents.x; ptEndPoints[0][7].y += ray.m_Extents.y; ptEndPoints[0][7].z -= ray.m_Extents.z; + + float fDists[2][8]; + int iSides[2][8]; + int iSideMask[2] = { 0, 0 }; + for( int i = 0; i != 8; ++i ) + { + fDists[0][i] = vQuadNormal.Dot( ptEndPoints[0][i] ) - fQuadPlaneDist; + if( fDists[0][i] > 0.0f ) + { + iSides[0][i] = 1; + iSideMask[0] |= 1; + } + else + { + iSides[0][i] = 2; + iSideMask[0] |= 2; + } + } + + if( ray.m_IsSwept ) + { + for( int i = 0; i != 8; ++i ) + ptEndPoints[1][i] = ptEndPoints[0][i] + ray.m_Delta; + + for( int i = 0; i != 8; ++i ) + { + fDists[1][i] = vQuadNormal.Dot( ptEndPoints[1][i] ) - fQuadPlaneDist; + if( fDists[1][i] > 0.0f ) + { + iSides[1][i] = 1; + iSideMask[1] |= 1; + } + else + { + iSides[1][i] = 2; + iSideMask[1] |= 2; + } + } + } + + if( (iSideMask[0] | iSideMask[1]) != 3 ) + { + //Assert( (iSideMask[0] | iSideMask[1]) != 2 ); + return false; //all points resides entirely on one side of the quad + } + + + //generate intersections for boxes split by the plane at either end of the ray + for( int k = 0; k != 2; ++k ) + { + if( iSideMask[k] == 3 ) //box is split by the plane + { + for( int i = 0; i != 8; ++i ) + { + if( iSides[k][i] == 2 ) //point behind the plane + { + int iAxisCrossings[3]; + iAxisCrossings[0] = i ^ 4; //upper 4 vs lower 4 crosses X axis + iAxisCrossings[1] = ((i + 1) & 3) + (i & 4); //cycle to the next element while staying within the upper 4 or lower 4, this will cross either Y or Z axis, we don't care which + iAxisCrossings[2] = ((i - 1) & 3) + (i & 4); //cylce to the previous element while staying within the upper 4 or lower 4, this will cross the axis iAxisCrossings[1] didn't cross + + for( int j = 0; j != 3; ++j ) + { + if( iSides[k][iAxisCrossings[j]] == 1 ) //point in front of the plane + { + //line between ptEndPoints[i] and ptEndPoints[iAxisCrossings[j]] intersects the plane, generate a point at the intersection for further testing + float fInvTotalDist = 1.0f / (fDists[k][iAxisCrossings[j]] - fDists[k][i]); //remember that fDists[k][i] is a negative value + ptPlaneIntersections[iPlaneIntersectionsCount] = (ptEndPoints[k][iAxisCrossings[j]] * (-fDists[k][i] * fInvTotalDist)) + (ptEndPoints[k][i] * (fDists[k][iAxisCrossings[j]] * fInvTotalDist)); + + Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane + + ++iPlaneIntersectionsCount; + } + } + } + } + } + } + + if( ray.m_IsSwept ) + { + for( int i = 0; i != 8; ++i ) + { + if( iSides[0][i] != iSides[1][i] ) + { + int iPosSide, iNegSide; + if( iSides[0][i] == 1 ) + { + iPosSide = 0; + iNegSide = 1; + } + else + { + iPosSide = 1; + iNegSide = 0; + } + + Assert( (fDists[iPosSide][i] >= 0.0f) && (fDists[iNegSide][i] <= 0.0f) ); + + float fInvTotalDist = 1.0f / (fDists[iPosSide][i] - fDists[iNegSide][i]); //remember that fDists[iNegSide][i] is a negative value + ptPlaneIntersections[iPlaneIntersectionsCount] = (ptEndPoints[iPosSide][i] * (-fDists[iNegSide][i] * fInvTotalDist)) + (ptEndPoints[iNegSide][i] * (fDists[iPosSide][i] * fInvTotalDist)); + + Assert( fabs( ptPlaneIntersections[iPlaneIntersectionsCount].Dot( vQuadNormal ) - fQuadPlaneDist ) < 0.1f ); //intersection point is on plane + + ++iPlaneIntersectionsCount; + } + } + } + } + + //down here, we should simply have a collection of plane intersections, now we see if they reside within the quad + Assert( iPlaneIntersectionsCount != 0 ); + + for( int i = 0; i != iPlaneIntersectionsCount; ++i ) + { + //these points are guaranteed to be on the plane, now just check to see if they're within the quad's extents + Vector vToPointFromQuadCenter = ptPlaneIntersections[i] - ptQuadCenter; + + float fExt1Dist = vQuadExtent1_Normalized.Dot( vToPointFromQuadCenter ); + if( fabs( fExt1Dist ) > fQuadExtent1Length ) + return false; //point is outside boundaries + + //vToPointFromQuadCenter -= vQuadExtent1_Normalized * fExt1Dist; //to handle diamond shaped quads + + float fExt2Dist = vQuadExtent2_Normalized.Dot( vToPointFromQuadCenter ); + if( fabs( fExt2Dist ) > fQuadExtent2Length ) + return false; //point is outside boundaries + } + + return true; //there were lines crossing the quad plane, and every line crossing that plane had its intersection with the plane within the quad's boundaries +} + +#endif // !_STATIC_LINKED || _SHARED_LIB -- cgit v1.2.3