H3cJP/source-engine
1
0
Fork 0
mirror of https://github.com/nillerusr/source-engine.git synced 2024-12-22 06:06:50 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity
master
source-engine/vphysics/trace.cpp

2475 lines
71 KiB
C++
Raw Permalink Blame History

//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose:
//
// $NoKeywords: $
//
//=============================================================================//
#include "cbase.h"
#include "cmodel.h"
#include "physics_trace.h"
#include "ivp_surman_polygon.hxx"
#include "ivp_compact_ledge.hxx"
#include "ivp_compact_ledge_solver.hxx"
#include "ivp_compact_surface.hxx"
#include "tier0/vprof.h"
#include "mathlib/ssemath.h"
#include "tier0/tslist.h"
// memdbgon must be the last include file in a .cpp file!!!
#include "tier0/memdbgon.h"
// this skips the sphere tree stuff for tracing
#define DEBUG_TEST_ALL_LEDGES 0
// this skips the optimization that shrinks the ray as each intersection is encountered
#define DEBUG_KEEP_FULL_RAY 0
// this skips the optimization that looks up the first vert in a cubemap
#define USE_COLLIDE_MAP 1
// objects with small numbers of verts build a cache of pre-transformed verts
#define USE_VERT_CACHE 1
#define USE_RLE_SPANS 1
// UNDONE: This is a boost on PC, but doesn't work yet on x360 - investigate
#define SIMD_MATRIX 0
// turn this on to get asserts in the low-level collision solver
#define CHECK_TOI_CALCS 0
#define BRUTE_FORCE_VERT_COUNT 128
// NOTE: This is in inches (HL units)
#define TEST_EPSILON (g_PhysicsUnits.collisionSweepIncrementalEpsilon)
struct simplexvert_t
{
Vector position;
unsigned short testIndex : 15;
unsigned short sweepIndex : 1;
unsigned short obstacleIndex;
};
struct simplex_t
{
simplexvert_t verts[4];
int vertCount;
inline bool PointSimplex( const simplexvert_t &newPoint, Vector *pOut );
inline bool EdgeSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &edge, Vector *pOut );
inline bool TriangleSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &faceNormal, Vector *pOut );
bool SolveGJKSet( const simplexvert_t &newPoint, Vector *pOut );
bool SolveVoronoiRegion2( const simplexvert_t &newPoint, Vector *pOut );
bool SolveVoronoiRegion3( const simplexvert_t &newPoint, Vector *pOut );
bool SolveVoronoiRegion4( const simplexvert_t &newPoint, Vector *pOut );
Vector ClipRayToTetrahedronBase( const Vector &dir );
Vector ClipRayToTetrahedron( const Vector &dir );
float ClipRayToTriangle( const Vector &dir, float epsilon );
};
class CTraceCone : public ITraceObject
{
public:
CTraceCone( const truncatedcone_t &cone, const Vector &translation )
{
m_cone = cone;
m_cone.origin += translation;
float cosTheta;
SinCos( DEG2RAD(m_cone.theta), &m_sinTheta, &cosTheta );
m_radius = m_cone.h * m_sinTheta / cosTheta;
m_centerBase = m_cone.origin + m_cone.h * m_cone.normal;
}
virtual int SupportMap( const Vector &dir, Vector *pOut ) const
{
Vector unitDir = dir;
VectorNormalize(unitDir);
float dot = DotProduct( unitDir, m_cone.normal );
// anti-cone is -normal, angle = 90 - theta
// If the normal is in the anti-cone, then return the apex
// not in anti-cone, support map is on the surface of the disc
if ( dot > -m_sinTheta )
{
unitDir -= m_cone.normal * dot;
float len = VectorNormalize( unitDir );
if ( len > 1e-4f )
{
*pOut = m_centerBase + (unitDir * m_radius);
return 0;
}
*pOut = m_centerBase;
return 0;
}
// outside the cone's angle, support map is on the surface of the cone
*pOut = m_cone.origin;
return 0;
}
// BUGBUG: Doesn't work!
virtual Vector GetVertByIndex( int index ) const { return m_cone.origin; }
virtual float Radius( void ) const { return m_cone.h + m_radius; }
truncatedcone_t m_cone;
float m_radius;
float m_sinTheta;
Vector m_centerBase;
};
// really this is indexing a vertex, but the iteration code needs a triangle + edge index.
// edge is always 0-2 so return it in the bottom 2 bits
static unsigned short GetPackedIndex( const IVP_Compact_Ledge *pLedge, const IVP_U_Float_Point &dir )
{
const IVP_Compact_Poly_Point *RESTRICT pPoints = pLedge->get_point_array();
const IVP_Compact_Triangle *RESTRICT pTri = pLedge->get_first_triangle();
const IVP_Compact_Edge *RESTRICT pEdge = pTri->get_edge( 0 );
int best = pEdge->get_start_point_index();
float bestDot = pPoints[best].dot_product( &dir );
int triCount = pLedge->get_n_triangles();
const IVP_Compact_Triangle *RESTRICT pBestTri = pTri;
// this loop will early out, but keep it from being infinite
int i;
// hillclimbing search to find the best support vert
for ( i = 0; i < triCount; i++ )
{
// get the index to the end vert of this edge (start vert on next edge)
pEdge = pEdge->get_prev();
int stopVert = pEdge->get_start_point_index();
// loop through the verts that can be reached along edges from this vert
// stop if you get back to the one you're starting on.
int vert = stopVert;
do
{
float dot = pPoints[vert].dot_product( &dir );
if ( dot > bestDot )
{
bestDot = dot;
best = vert;
pBestTri = pEdge->get_triangle();
break;
}
// tri opposite next edge, same starting vert as next edge
pEdge = pEdge->get_opposite()->get_prev();
vert = pEdge->get_start_point_index();
} while ( vert != stopVert );
// if you exhausted the possibilities for this vert, it must be the best vert
if ( vert != best )
break;
}
int triIndex = pBestTri - pLedge->get_first_triangle();
int edgeIndex = 0;
// just do a search for the edge containing this vert instead of storing it along the way
for ( i = 0; i < 3; i++ )
{
if ( pBestTri->get_edge(i)->get_start_point_index() == best )
{
edgeIndex = i;
break;
}
}
return (unsigned short) ( (triIndex<<2) + edgeIndex );
}
void InitLeafmap( IVP_Compact_Ledge *pLedge, leafmap_t *pLeafmapOut )
{
pLeafmapOut->pLeaf = pLedge;
pLeafmapOut->vertCount = 0;
pLeafmapOut->flags = 0;
pLeafmapOut->spanCount = 0;
if ( pLedge && pLedge->is_terminal() )
{
// for small numbers of verts it's much faster to simply do dot products with all verts
// since the best case for hillclimbing is to touch the start vert plus all neighbors (avg_valence+1 dots)
// in t
int triCount = pLedge->get_n_triangles();
// this is a guess that anything with more than brute_force * 4 tris will have at least brute_force verts
if ( triCount <= BRUTE_FORCE_VERT_COUNT*4 )
{
Assert(triCount>0);
int minV = MAX_CONVEX_VERTS;
int maxV = 0;
for ( int i = 0; i < triCount; i++ )
{
const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i;
for ( int j = 0; j < 3; j++ )
{
const IVP_Compact_Edge *pEdge = pTri->get_edge( j );
int v = pEdge->get_start_point_index();
if ( v < minV )
{
minV = v;
}
if ( v > maxV )
{
maxV = v;
}
}
}
int vertCount = (maxV-minV) + 1;
// max possible verts is < 48, so this is just here for some real failure
// or vert sharing with a large collection of convexes. In that case the
// number could be high, but this approach to implementing support is invalid
// because the vert range is polluted
if ( vertCount < BRUTE_FORCE_VERT_COUNT )
{
char hasVert[BRUTE_FORCE_VERT_COUNT];
memset(hasVert, 0, sizeof(hasVert[0])*vertCount);
for ( int i = 0; i < triCount; i++ )
{
const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i;
for ( int j = 0; j < 3; j++ )
{
// mark each vert in the list
const IVP_Compact_Edge *pEdge = pTri->get_edge( j );
int v = pEdge->get_start_point_index();
hasVert[v-minV] = true;
}
}
// now find the vertex spans and encode them
byte spans[BRUTE_FORCE_VERT_COUNT];
int spanIndex = 0;
char has = hasVert[0];
Assert(has);
byte count = 1;
for ( int i = 1; i < vertCount && spanIndex < BRUTE_FORCE_VERT_COUNT; i++ )
{
// each change of state is a new span
if ( has != hasVert[i] )
{
spans[spanIndex] = count;
has = hasVert[i];
count = 0;
spanIndex++;
}
count++;
Assert(count < 255);
}
// rle spans only supported with vertex caching
#if USE_VERT_CACHE && USE_RLE_SPANS
if ( spanIndex < BRUTE_FORCE_VERT_COUNT )
#else
if ( spanIndex < 1 )
#endif
{
spans[spanIndex] = count;
spanIndex++;
pLeafmapOut->SetRLESpans( minV, spanIndex, spans );
}
}
}
}
if ( !pLeafmapOut->HasSpans() )
{
// otherwise make a 8-way directional map to pick the best start vert for hillclimbing
pLeafmapOut->SetHasCubemap();
for ( int i = 0; i < 8; i++ )
{
IVP_U_Float_Point tmp;
tmp.k[0] = ( i & 1 ) ? -1 : 1;
tmp.k[1] = ( i & 2 ) ? -1 : 1;
tmp.k[2] = ( i & 4 ) ? -1 : 1;
pLeafmapOut->startVert[i] = GetPackedIndex( pLedge, tmp );
}
}
}
void GetStartVert( const leafmap_t *pLeafmap, const IVP_U_Float_Point &localDirection, int &triIndex, int &edgeIndex )
{
if ( !pLeafmap || !pLeafmap->HasCubemap() )
return;
// map dir to index
int cacheIndex = (localDirection.k[0] < 0 ? 1 : 0) + (localDirection.k[1] < 0 ? 2 : 0) + (localDirection.k[2] < 0 ? 4 : 0 );
triIndex = pLeafmap->startVert[cacheIndex] >> 2;
edgeIndex = pLeafmap->startVert[cacheIndex] & 0x3;
}
CTSPool<CVisitHash> g_VisitHashPool;
CVisitHash *AllocVisitHash()
{
return g_VisitHashPool.GetObject();
}
void FreeVisitHash(CVisitHash *pFree)
{
if ( pFree )
{
g_VisitHashPool.PutObject(pFree);
}
}
//-----------------------------------------------------------------------------
// Purpose: Implementation for Trace against an IVP object
//-----------------------------------------------------------------------------
class CTraceIVP : public ITraceObject
{
public:
CTraceIVP( const CPhysCollide *pCollide, const Vector &origin, const QAngle &angles );
~CTraceIVP()
{
if ( m_pVisitHash )
FreeVisitHash(m_pVisitHash);
}
virtual int SupportMap( const Vector &dir, Vector *pOut ) const;
virtual Vector GetVertByIndex( int index ) const;
// UNDONE: Do general ITraceObject center/offset computation and move the ray to account
// for this delta like we do in TraceSweepIVP()
// Then we can shrink the radius of objects with mass centers NOT at the origin
virtual float Radius( void ) const
{
return m_radius;
}
inline float TransformLengthToLocal( float length )
{
return ConvertDistanceToIVP( length );
}
// UNDONE: Optimize this by storing 3 matrices? (one for each transform that includes rot/scale for HL/IVP)?
// UNDONE: Not necessary if we remove the coordinate conversion
inline void TransformDirectionToLocal( const Vector &dir, IVP_U_Float_Point &local ) const
{
IVP_U_Float_Point tmp;
ConvertDirectionToIVP( dir, tmp );
m_matrix.vimult3( &tmp, &local );
}
inline void RotateRelativePositionToLocal( const Vector &delta, IVP_U_Float_Point &local ) const
{
IVP_U_Float_Point tmp;
ConvertPositionToIVP( delta, tmp );
m_matrix.vimult3( &tmp, &local );
}
inline void TransformPositionToLocal( const Vector &pos, IVP_U_Float_Point &local ) const
{
IVP_U_Float_Point tmp;
ConvertPositionToIVP( pos, tmp );
m_matrix.vimult4( &tmp, &local );
}
inline void TransformPositionFromLocal( const IVP_U_Float_Point &local, Vector &out ) const
{
VectorTransform( *(Vector *)&local, *((const matrix3x4_t *)&m_ivpLocalToHLWorld), out );
}
#if USE_VERT_CACHE
inline Vector CachedVertByIndex(int index) const
{
int subIndex = index & 3;
return m_vertCache[index>>2].Vec(subIndex);
}
#endif
bool IsValid( void ) { return m_pLedge != NULL; }
void AllocateVisitHash()
{
if ( !m_pVisitHash )
m_pVisitHash = AllocVisitHash();
}
void SetLedge( const IVP_Compact_Ledge *pLedge )
{
m_pLedge = pLedge;
m_pLeafmap = NULL;
if ( !pLedge )
return;
#if USE_VERT_CACHE
m_cacheCount = 0;
#endif
if ( m_pCollideMap )
{
for ( int i = 0; i < m_pCollideMap->leafCount; i++ )
{
if ( m_pCollideMap->leafmap[i].pLeaf == pLedge )
{
m_pLeafmap = &m_pCollideMap->leafmap[i];
if ( !BuildLeafmapCache( &m_pCollideMap->leafmap[i] ) )
{
AllocateVisitHash();
}
return;
}
}
}
AllocateVisitHash();
}
bool SetSingleConvex( void )
{
const IVP_Compact_Ledgetree_Node *node = m_pSurface->get_compact_ledge_tree_root();
if ( node->is_terminal() == IVP_TRUE )
{
SetLedge( node->get_compact_ledge() );
return true;
}
SetLedge( NULL );
return false;
}
bool BuildLeafmapCache(const leafmap_t * RESTRICT pLeafmap);
bool BuildLeafmapCacheRLE( const leafmap_t * RESTRICT pLeafmap );
inline int SupportMapCached( const Vector &dir, Vector *pOut ) const;
const collidemap_t *m_pCollideMap;
const IVP_Compact_Surface *m_pSurface;
private:
const leafmap_t *m_pLeafmap;
const IVP_Compact_Ledge *m_pLedge;
CVisitHash *m_pVisitHash;
#if SIMD_MATRIX
FourVectors m_ivpLocalToHLWorld;
#else
matrix3x4_t m_ivpLocalToHLWorld;
#endif
IVP_U_Matrix m_matrix;
// transform that includes scale from IVP to HL coords, do not VectorITransform or VectorRotate with this
float m_radius;
int m_nPointTest;
int m_nStartPoint;
bool m_bHasTranslation;
#if USE_VERT_CACHE
int m_cacheCount; // number of FourVectors used
FourVectors m_vertCache[BRUTE_FORCE_VERT_COUNT/4];
#endif
};
// GCC 4.2.1 can't handle loading a static const into a m128 register :(
#ifdef WIN32
static const
#endif
fltx4 g_IVPToHLDir = { 1.0f, -1.0f, 1.0f, 1.0f };
//static const fltx4 g_IVPToHLPosition = { IVP2HL(1.0f), -IVP2HL(1.0f), IVP2HL(1.0f), IVP2HL(1.0f) };
#if defined(_X360)
FORCEINLINE fltx4 ConvertDirectionToIVP( const fltx4 & a )
{
fltx4 t = __vpermwi( a, VPERMWI_CONST( 0, 2, 1, 3 ) );
// negate Y
return MulSIMD( t, g_IVPToHLDir );
}
#else
FORCEINLINE fltx4 ConvertDirectionToIVP( const fltx4 & a )
{
// swap Z & Y
fltx4 t = _mm_shuffle_ps( a, a, MM_SHUFFLE_REV( 0, 2, 1, 3 ) );
// negate Y
return MulSIMD( t, g_IVPToHLDir );
}
#endif
CTraceIVP::CTraceIVP( const CPhysCollide *pCollide, const Vector &origin, const QAngle &angles )
{
#if USE_COLLIDE_MAP
m_pCollideMap = pCollide->GetCollideMap();
#else
m_pCollideMap = NULL;
#endif
m_pSurface = pCollide->GetCompactSurface();
m_pLedge = NULL;
m_pVisitHash = NULL;
m_bHasTranslation = (origin==vec3_origin) ? false : true;
// UNDONE: Move this offset calculation into the tracing routines
// I didn't do this now because it seems to require changes to most of the
// transform routines - and this would cause bugs.
float centerOffset = VectorLength( m_pSurface->mass_center.k );
#if SIMD_MATRIX
VectorAligned forward, right, up;
IVP_U_Float_Point ivpForward, ivpLeft, ivpUp;
AngleVectors( angles, &forward, &right, &up );
Vector left = -right;
Vector down = -up;
ConvertDirectionToIVP( forward, ivpForward );
ConvertDirectionToIVP( left, ivpLeft );
ConvertDirectionToIVP( down, ivpUp );
m_matrix.set_col( IVP_INDEX_X, &ivpForward );
m_matrix.set_col( IVP_INDEX_Z, &ivpLeft );
m_matrix.set_col( IVP_INDEX_Y, &ivpUp );
ConvertPositionToIVP( origin, m_matrix.vv );
forward.w = HL2IVP(origin.x);
// This vector is supposed to be left, so we'll negate it later, but we don't want to
// negate the position, so add another minus to cancel out
right.w = -HL2IVP(origin.y);
up.w = HL2IVP(origin.z);
fltx4 rx = ConvertDirectionToIVP(LoadAlignedSIMD(forward.Base()));
fltx4 ry = ConvertDirectionToIVP(SubSIMD( Four_Zeros, LoadAlignedSIMD(right.Base())) );
fltx4 rz = ConvertDirectionToIVP(LoadAlignedSIMD(up.Base()) );
fltx4 scaleHL = ReplicateX4(IVP2HL(1.0f));
m_ivpLocalToHLWorld.x = MulSIMD( scaleHL, rx );
m_ivpLocalToHLWorld.y = MulSIMD( scaleHL, ry );
m_ivpLocalToHLWorld.z = MulSIMD( scaleHL, rz );
#else
ConvertRotationToIVP( angles, m_matrix );
ConvertPositionToIVP( origin, m_matrix.vv );
float scale = IVP2HL(1.0f);
float negScale = IVP2HL(-1.0f);
// copy the existing IVP local->world matrix (swap Y & Z)
m_ivpLocalToHLWorld.m_flMatVal[0][0] = m_matrix.get_elem(IVP_INDEX_X,0) * scale;
m_ivpLocalToHLWorld.m_flMatVal[0][1] = m_matrix.get_elem(IVP_INDEX_X,1) * scale;
m_ivpLocalToHLWorld.m_flMatVal[0][2] = m_matrix.get_elem(IVP_INDEX_X,2) * scale;
m_ivpLocalToHLWorld.m_flMatVal[1][0] = m_matrix.get_elem(IVP_INDEX_Z,0) * scale;
m_ivpLocalToHLWorld.m_flMatVal[1][1] = m_matrix.get_elem(IVP_INDEX_Z,1) * scale;
m_ivpLocalToHLWorld.m_flMatVal[1][2] = m_matrix.get_elem(IVP_INDEX_Z,2) * scale;
m_ivpLocalToHLWorld.m_flMatVal[2][0] = m_matrix.get_elem(IVP_INDEX_Y,0) * negScale;
m_ivpLocalToHLWorld.m_flMatVal[2][1] = m_matrix.get_elem(IVP_INDEX_Y,1) * negScale;
m_ivpLocalToHLWorld.m_flMatVal[2][2] = m_matrix.get_elem(IVP_INDEX_Y,2) * negScale;
m_ivpLocalToHLWorld.m_flMatVal[0][3] = m_matrix.vv.k[0] * scale;
m_ivpLocalToHLWorld.m_flMatVal[1][3] = m_matrix.vv.k[2] * scale;
m_ivpLocalToHLWorld.m_flMatVal[2][3] = m_matrix.vv.k[1] * negScale;
#endif
m_radius = ConvertDistanceToHL( m_pSurface->upper_limit_radius + centerOffset );
}
bool CTraceIVP::BuildLeafmapCacheRLE( const leafmap_t * RESTRICT pLeafmap )
{
// iterate the rle spans of verts and output them to a buffer in post-transform space
int startPoint = pLeafmap->startVert[0];
int pointCount = pLeafmap->vertCount;
m_cacheCount = (pointCount + 3)>>2;
const byte *RESTRICT pSpans = pLeafmap->GetSpans();
int countThisSpan = pSpans[0];
int spanIndex = 1;
int baseVert = 0;
const VectorAligned * RESTRICT pVerts = (const VectorAligned *)&m_pLedge->get_point_array()[startPoint];
for ( int i = 0; i < m_cacheCount-1; i++ )
{
if ( countThisSpan < 4 )
{
// unrolled for perf
// we need a batch of four verts, but they aren't in a single span
int v0, v1, v2, v3;
if ( !countThisSpan )
{
baseVert += pSpans[spanIndex];
countThisSpan = pSpans[spanIndex+1];
spanIndex += 2;
}
v0 = baseVert++;
countThisSpan--;
if ( !countThisSpan )
{
baseVert += pSpans[spanIndex];
countThisSpan = pSpans[spanIndex+1];
spanIndex += 2;
}
v1 = baseVert++;
countThisSpan--;
if ( !countThisSpan )
{
baseVert += pSpans[spanIndex];
countThisSpan = pSpans[spanIndex+1];
spanIndex += 2;
}
v2 = baseVert++;
countThisSpan--;
if ( !countThisSpan )
{
baseVert += pSpans[spanIndex];
countThisSpan = pSpans[spanIndex+1];
spanIndex += 2;
}
v3 = baseVert++;
countThisSpan--;
m_vertCache[i].LoadAndSwizzleAligned( pVerts[v0].Base(), pVerts[v1].Base(), pVerts[v2].Base(), pVerts[v3].Base() );
}
else
{
// we have four verts in this span, just grab the next four
m_vertCache[i].LoadAndSwizzleAligned( pVerts[baseVert+0].Base(), pVerts[baseVert+1].Base(), pVerts[baseVert+2].Base(), pVerts[baseVert+3].Base() );
baseVert += 4;
countThisSpan -= 4;
}
}
// the last iteration needs multiple spans and clamping to the last vert
int v[4];
for ( int i = 0; i < 4; i++ )
{
if ( spanIndex < pLeafmap->spanCount && !countThisSpan )
{
baseVert += pSpans[spanIndex];
countThisSpan = pSpans[spanIndex+1];
spanIndex += 2;
}
if ( spanIndex < pLeafmap->spanCount )
{
v[i] = baseVert;
baseVert++;
countThisSpan--;
}
else
{
v[i] = baseVert;
if ( countThisSpan > 1 )
{
countThisSpan--;
baseVert++;
}
}
}
m_vertCache[m_cacheCount-1].LoadAndSwizzleAligned( pVerts[v[0]].Base(), pVerts[v[1]].Base(), pVerts[v[2]].Base(), pVerts[v[3]].Base() );
FourVectors::RotateManyBy( &m_vertCache[0], m_cacheCount, *((const matrix3x4_t *)&m_ivpLocalToHLWorld) );
return true;
}
bool CTraceIVP::BuildLeafmapCache( const leafmap_t * RESTRICT pLeafmap )
{
#if !USE_VERT_CACHE
return false;
#else
if ( !pLeafmap || !pLeafmap->HasSpans() || m_bHasTranslation )
return false;
if ( pLeafmap->HasRLESpans() )
{
return BuildLeafmapCacheRLE(pLeafmap);
}
// single vertex span, just xform + copy
// iterate the span of verts and output them to a buffer in post-transform space
// just iterate the range if one is specified
int startPoint = pLeafmap->startVert[0];
int pointCount = pLeafmap->vertCount;
m_cacheCount = (pointCount + 3)>>2;
Assert(m_cacheCount>=0 && m_cacheCount<= (BRUTE_FORCE_VERT_COUNT/4));
const VectorAligned * RESTRICT pVerts = (const VectorAligned *)&m_pLedge->get_point_array()[startPoint];
for ( int i = 0; i < m_cacheCount-1; i++ )
{
m_vertCache[i].LoadAndSwizzleAligned( pVerts[0].Base(), pVerts[1].Base(), pVerts[2].Base(), pVerts[3].Base() );
pVerts += 4;
}
int remIndex = (pointCount-1) & 3;
int x0 = 0;
int x1 = min(1,remIndex);
int x2 = min(2,remIndex);
int x3 = min(3,remIndex);
m_vertCache[m_cacheCount-1].LoadAndSwizzleAligned( pVerts[x0].Base(), pVerts[x1].Base(), pVerts[x2].Base(), pVerts[x3].Base() );
FourVectors::RotateManyBy( &m_vertCache[0], m_cacheCount, *((const matrix3x4_t *)&m_ivpLocalToHLWorld) );
return true;
#endif
}
static const fltx4 g_IndexBase = {0,1,2,3};
int CTraceIVP::SupportMapCached( const Vector &dir, Vector *pOut ) const
{
VPROF("SupportMapCached");
#if USE_VERT_CACHE
FourVectors fourDir;
#if defined(_X360)
fltx4 vec = LoadUnaligned3SIMD( dir.Base() );
fourDir.x = SplatXSIMD(vec);
fourDir.y = SplatYSIMD(vec);
fourDir.z = SplatZSIMD(vec);
#else
fourDir.DuplicateVector(dir);
#endif
fltx4 index = g_IndexBase;
fltx4 maxIndex = g_IndexBase;
fltx4 maxDot = fourDir * m_vertCache[0];
for ( int i = 1; i < m_cacheCount; i++ )
{
index = AddSIMD(index, Four_Fours);
fltx4 dot = fourDir * m_vertCache[i];
fltx4 cmpMask = CmpGtSIMD(dot,maxDot);
maxIndex = MaskedAssign( cmpMask, index, maxIndex );
maxDot = MaxSIMD(dot, maxDot);
}
// find highest of 4
fltx4 rot = RotateLeft2(maxDot);
fltx4 rotIndex = RotateLeft2(maxIndex);
fltx4 cmpMask = CmpGtSIMD(rot,maxDot);
maxIndex = MaskedAssign(cmpMask, rotIndex, maxIndex);
maxDot = MaxSIMD(rot,maxDot);
rotIndex = RotateLeft(maxIndex);
rot = RotateLeft(maxDot);
cmpMask = CmpGtSIMD(rot,maxDot);
maxIndex = MaskedAssign(cmpMask, rotIndex, maxIndex);
// not needed unless we need the actual max dot at the end
// maxDot = MaxSIMD(rot,maxDot);
int bestIndex = SubFloatConvertToInt(maxIndex,0);
*pOut = CachedVertByIndex(bestIndex);
return bestIndex;
#else
Assert(0);
#endif
}
int CTraceIVP::SupportMap( const Vector &dir, Vector *pOut ) const
{
#if USE_VERT_CACHE
if ( m_cacheCount )
return SupportMapCached( dir, pOut );
#endif
if ( m_pLeafmap && m_pLeafmap->HasSingleVertexSpan() )
{
VPROF("SupportMap_Leaf");
const IVP_U_Float_Point *pPoints = m_pLedge->get_point_array();
IVP_U_Float_Point mapdir;
TransformDirectionToLocal( dir, mapdir );
// just iterate the range if one is specified
int startPoint = m_pLeafmap->startVert[0];
int pointCount = m_pLeafmap->vertCount;
float bestDot = pPoints[startPoint].dot_product(&mapdir);
int best = startPoint;
for ( int i = 1; i < pointCount; i++ )
{
float dot = pPoints[startPoint+i].dot_product(&mapdir);
if ( dot > bestDot )
{
bestDot = dot;
best = startPoint+i;
}
}
TransformPositionFromLocal( pPoints[best], *pOut ); // transform point position to world space
return best;
}
else
{
VPROF("SupportMap_Walk");
const IVP_U_Float_Point *pPoints = m_pLedge->get_point_array();
IVP_U_Float_Point mapdir;
TransformDirectionToLocal( dir, mapdir );
int triCount = m_pLedge->get_n_triangles();
Assert( m_pVisitHash );
m_pVisitHash->NewVisit();
float dot;
int triIndex = 0, edgeIndex = 0;
GetStartVert( m_pLeafmap, mapdir, triIndex, edgeIndex );
const IVP_Compact_Triangle *RESTRICT pTri = m_pLedge->get_first_triangle() + triIndex;
const IVP_Compact_Edge *RESTRICT pEdge = pTri->get_edge( edgeIndex );
int best = pEdge->get_start_point_index();
float bestDot = pPoints[best].dot_product( &mapdir );
m_pVisitHash->VisitVert(best);
// This should never happen. MAX_CONVEX_VERTS is very large (millions), none of our
// models have anywhere near this many verts in a convex piece
Assert(triCount*3<MAX_CONVEX_VERTS);
// this loop will early out, but keep it from being infinite
for ( int i = 0; i < triCount; i++ )
{
// get the index to the end vert of this edge (start vert on next edge)
pEdge = pEdge->get_prev();
int stopVert = pEdge->get_start_point_index();
// loop through the verts that can be reached along edges from this vert
// stop if you get back to the one you're starting on.
int vert = stopVert;
do
{
if ( !m_pVisitHash->WasVisited(vert) )
{
// this lets us skip doing dot products on this vert
m_pVisitHash->VisitVert(vert);
dot = pPoints[vert].dot_product( &mapdir );
if ( dot > bestDot )
{
bestDot = dot;
best = vert;
break;
}
}
// tri opposite next edge, same starting vert as next edge
pEdge = pEdge->get_opposite()->get_prev();
vert = pEdge->get_start_point_index();
} while ( vert != stopVert );
// if you exhausted the possibilities for this vert, it must be the best vert
if ( vert != best )
break;
}
// code to do the brute force method with no hill-climbing
#if 0
for ( i = 0; i < triCount; i++ )
{
pTri = m_pLedge->get_first_triangle() + i;
for ( int j = 0; j < 3; j++ )
{
pEdge = pTri->get_edge( j );
int test = pEdge->get_start_point_index();
dot = pPoints[test].dot_product( &mapdir );
if ( dot > bestDot )
{
Assert(0); // shouldn't hit this unless the hill-climb is broken
bestDot = dot;
best = test;
}
}
}
#endif
TransformPositionFromLocal( pPoints[best], *pOut ); // transform point position to world space
return best;
}
}
Vector CTraceIVP::GetVertByIndex( int index ) const
{
#if USE_VERT_CACHE
if ( m_cacheCount )
{
return CachedVertByIndex(index);
}
#endif
const IVP_Compact_Poly_Point *pPoints = m_pLedge->get_point_array();
Vector out;
TransformPositionFromLocal( pPoints[index], out );
return out;
}
//-----------------------------------------------------------------------------
// Purpose: Implementation for Trace against an AABB
//-----------------------------------------------------------------------------
class CTraceAABB : public ITraceObject
{
public:
CTraceAABB( const Vector &hlmins, const Vector &hlmaxs, bool isPoint );
virtual int SupportMap( const Vector &dir, Vector *pOut ) const;
virtual Vector GetVertByIndex( int index ) const;
virtual float Radius( void ) const { return m_radius; }
private:
float m_x[2];
float m_y[2];
float m_z[2];
float m_radius;
bool m_empty;
};
CTraceAABB::CTraceAABB( const Vector &hlmins, const Vector &hlmaxs, bool isPoint )
{
if ( isPoint )
{
m_x[0] = m_x[1] = 0;
m_y[0] = m_y[1] = 0;
m_z[0] = m_z[1] = 0;
m_radius = 0;
m_empty = true;
}
else
{
m_x[0] = hlmaxs[0];
m_x[1] = hlmins[0];
m_y[0] = hlmaxs[1];
m_y[1] = hlmins[1];
m_z[0] = hlmaxs[2];
m_z[1] = hlmins[2];
m_radius = hlmaxs.Length();
m_empty = false;
}
}
int CTraceAABB::SupportMap( const Vector &dir, Vector *pOut ) const
{
Vector out;
if ( m_empty )
{
pOut->Init();
return 0;
}
// index is formed by the 3-bit bitfield SzSySx (negative is 1, positive is 0)
int x = ((*((unsigned int *)&dir.x)) & 0x80000000UL) >> 31;
int y = ((*((unsigned int *)&dir.y)) & 0x80000000UL) >> 31;
int z = ((*((unsigned int *)&dir.z)) & 0x80000000UL) >> 31;
pOut->x = m_x[x];
pOut->y = m_y[y];
pOut->z = m_z[z];
return (z<<2) | (y<<1) | x;
}
Vector CTraceAABB::GetVertByIndex( int index ) const
{
Vector out;
out.x = m_x[(index&1)];
out.y = m_y[(index&2)>>1];
out.z = m_z[(index&4)>>2];
return out;
}
//-----------------------------------------------------------------------------
// Purpose: Implementation for Trace against an IVP object
//-----------------------------------------------------------------------------
class CTraceRay
{
public:
CTraceRay( const Vector &hlstart, const Vector &hlend );
CTraceRay( const Ray_t &ray );
CTraceRay( const Ray_t &ray, const Vector &offset );
void Init( const Vector &hlstart, const Vector &delta );
int SupportMap( const Vector &dir, Vector *pOut ) const;
Vector GetVertByIndex( int index ) const { return ( index ) ? m_end : m_start; }
float Radius( void ) const { return m_length * 0.5f; }
void Reset( float fraction );
Vector m_start;
Vector m_end;
Vector m_delta;
Vector m_dir;
float m_length;
float m_baseLength;
float m_ooBaseLength;
float m_bestDist;
};
CTraceRay::CTraceRay( const Vector &hlstart, const Vector &hlend )
{
Init(hlstart, hlend-hlstart);
}
void CTraceRay::Init( const Vector &hlstart, const Vector &delta )
{
m_start = hlstart;
m_end = hlstart + delta;
m_delta = delta;
m_dir = delta;
float len = DotProduct(delta, delta);
// don't use fast/sse sqrt here we need the precision
m_length = sqrt(len);
m_ooBaseLength = 0.0f;
if ( m_length > 0 )
{
m_ooBaseLength = 1.0f / m_length;
m_dir *= m_ooBaseLength;
}
m_baseLength = m_length;
m_bestDist = 0.f;
}
CTraceRay::CTraceRay( const Ray_t &ray )
{
Init( ray.m_Start, ray.m_Delta );
}
CTraceRay::CTraceRay( const Ray_t &ray, const Vector &offset )
{
Vector start;
VectorAdd( ray.m_Start, offset, start );
Init( start, ray.m_Delta );
}
void CTraceRay::Reset( float fraction )
{
// recompute from base values for max precision
m_length = m_baseLength * fraction;
m_end = m_start + fraction * m_delta;
m_bestDist = 0.f;
}
int CTraceRay::SupportMap( const Vector &dir, Vector *pOut ) const
{
if ( DotProduct( dir, m_delta ) > 0 )
{
*pOut = m_end;
return 1;
}
*pOut = m_start;
return 0;
}
static char *map_nullname = "**empty**";
static csurface_t nullsurface = { map_nullname, 0 };
static void CM_ClearTrace( trace_t *trace )
{
memset( trace, 0, sizeof(*trace));
trace->fraction = 1.f;
trace->fractionleftsolid = 0;
trace->surface = nullsurface;
}
class CDefConvexInfo : public IConvexInfo
{
public:
IConvexInfo *GetPtr() { return this; }
virtual unsigned int GetContents( int convexGameData ) { return CONTENTS_SOLID; }
};
class CTraceSolver
{
public:
CTraceSolver( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, ITraceObject *obstacle, const Vector &axis )
{
m_pTotalTrace = ptr;
m_sweepObject = sweepobject;
m_sweepObjectRadius = m_sweepObject->Radius();
m_obstacle = obstacle;
m_ray = ray;
m_traceLength = 0;
m_totalTraceLength = max( ray->m_baseLength, 1e-8f );
m_pointClosestToIntersection = axis;
m_epsilon = g_PhysicsUnits.collisionSweepEpsilon;
}
bool SweepSingleConvex( void );
float SolveMeshIntersection( simplex_t &simplex );
float SolveMeshIntersection2D( simplex_t &simplex );
virtual void DoSweep( void )
{
SweepSingleConvex();
*m_pTotalTrace = m_trace;
}
void SetEpsilon( float epsilon )
{
m_epsilon = epsilon;
}
protected:
trace_t m_trace;
Vector m_pointClosestToIntersection;
ITraceObject *m_sweepObject;
ITraceObject *m_obstacle;
CTraceRay *m_ray;
trace_t *m_pTotalTrace;
float m_traceLength;
float m_totalTraceLength;
float m_sweepObjectRadius;
float m_epsilon;
private:
CTraceSolver( const CTraceSolver & );
};
class CTraceSolverSweptObject : public CTraceSolver
{
public:
CTraceSolverSweptObject( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, CTraceIVP *obstacle, const Vector &axis, unsigned int contentsMask, IConvexInfo *pConvexInfo );
void InitOSRay( void );
void SweepLedgeTree_r( const IVP_Compact_Ledgetree_Node *node );
inline bool SweepHitsSphereOS( const IVP_U_Float_Point *sphereCenter, float radius );
virtual void DoSweep( void );
inline void SweepAgainstNode( const IVP_Compact_Ledgetree_Node *node );
CTraceIVP *m_obstacleIVP;
IConvexInfo *m_pConvexInfo;
unsigned int m_contentsMask;
CDefConvexInfo m_fakeConvexInfo;
IVP_U_Float_Point m_rayCenterOS;
IVP_U_Float_Point m_rayStartOS;
IVP_U_Float_Point m_rayDirOS;
IVP_U_Float_Point m_rayDeltaOS;
float m_rayLengthOS;
private:
CTraceSolverSweptObject( const CTraceSolverSweptObject & ); // no implementation, quells compiler warning
};
CTraceSolverSweptObject::CTraceSolverSweptObject( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, CTraceIVP *obstacle, const Vector &axis, unsigned int contentsMask, IConvexInfo *pConvexInfo )
: CTraceSolver( ptr, sweepobject, ray, obstacle, axis )
{
m_obstacleIVP = obstacle;
m_contentsMask = contentsMask;
m_pConvexInfo = (pConvexInfo != NULL) ? pConvexInfo : m_fakeConvexInfo.GetPtr();
}
bool CTraceSolverSweptObject::SweepHitsSphereOS( const IVP_U_Float_Point *sphereCenter, float radius )
{
// disable this to help find bugs
#if DEBUG_TEST_ALL_LEDGES
return true;
#endif
// the ray is actually a line-swept-sphere with sweep object's radius
IVP_U_Float_Point delta_vec; // quick check for ends of ray
delta_vec.subtract( sphereCenter, &m_rayCenterOS );
radius += m_sweepObjectRadius;
// Is the sphere close enough to the ray at the center?
float qsphere_rad = radius * radius;
// If this is a 0 length ray, then the conservative test is 100% accurate
if ( m_rayLengthOS > 0 )
{
// Calculate the perpendicular distance to the sphere
// The perpendicular forms a right triangle with the vector between the ray/sphere centers
// and the ray direction vector. Calculate the projection of the hypoteneuse along the perpendicular
IVP_U_Float_Point h;
h.inline_calc_cross_product(&m_rayDirOS, &delta_vec);
if( h.quad_length() < qsphere_rad )
return true;
}
else
{
float quad_center_dist = delta_vec.quad_length();
if ( quad_center_dist < qsphere_rad )
{
return true;
}
// Could a ray in any direction away from the ray center intersect this sphere?
float qrad_sum = m_rayLengthOS * 0.5f + radius;
qrad_sum *= qrad_sum;
if ( quad_center_dist >= qrad_sum )
{
return false;
}
}
return false;
}
inline void CTraceSolverSweptObject::SweepAgainstNode(const IVP_Compact_Ledgetree_Node *node)
{
const IVP_Compact_Ledge *ledge = node->get_compact_ledge();
unsigned int ledgeContents = m_pConvexInfo->GetContents( ledge->get_client_data() );
if (m_contentsMask & ledgeContents)
{
m_obstacleIVP->SetLedge( ledge );
if ( SweepSingleConvex() )
{
if ( m_traceLength < m_totalTraceLength )
{
m_pTotalTrace->plane.normal = m_trace.plane.normal;
m_pTotalTrace->startsolid = m_trace.startsolid;
m_pTotalTrace->allsolid = m_trace.allsolid;
m_totalTraceLength = m_traceLength;
m_pTotalTrace->fraction = m_traceLength * m_ray->m_ooBaseLength;
Assert(m_pTotalTrace->fraction >= 0 && m_pTotalTrace->fraction <= 1.0f);
#if !DEBUG_KEEP_FULL_RAY
// shrink the ray to the shortened length, but leave a buffer of collisionSweepEpsilon units
// at the end to make sure that precision doesn't make you miss something slightly closer
float testFraction = (m_traceLength + m_epsilon*2) * m_ray->m_ooBaseLength;
if ( testFraction < 1.0f )
{
m_ray->Reset( testFraction );
// Update OS ray to limit tests
m_rayLengthOS = m_obstacleIVP->TransformLengthToLocal( m_ray->m_length );
m_rayCenterOS.add_multiple( &m_rayStartOS, &m_rayDeltaOS, 0.5f * testFraction );
}
#endif
m_pTotalTrace->contents = ledgeContents;
}
}
}
}
void CTraceSolverSweptObject::SweepLedgeTree_r( const IVP_Compact_Ledgetree_Node *node )
{
IVP_U_Float_Point center;
center.set(node->center.k);
if ( !SweepHitsSphereOS( &center, node->radius ) )
return;
// fast path for single leaf collision models
if ( node->is_terminal() == IVP_TRUE )
{
SweepAgainstNode(node);
return;
}
// use an array to implement a simple stack
CUtlVectorFixedGrowable<const IVP_Compact_Ledgetree_Node *, 64> list;
// pull the last item in the array (top of stack)
// this is nearly a priority queue, but not actually, but it's cheaper (and faster in the benchmarks)
// this code is trying to visit the nodes closest to the ray start first - which helps performance
// since we're only interested in the first intersection of the swept object with the physcollide.
while ( 1 )
{
// don't use the temp storage unless you have to.
loop_without_store:
if ( node->is_terminal() == IVP_TRUE )
{
// leaf, do the test
SweepAgainstNode(node);
}
else
{
// check node's children
const IVP_Compact_Ledgetree_Node *node0 = node->left_son();
center.set(node0->center.k);
// if we don't insert, this is larger than any quad distance
float lastDist = 1e16f;
if ( SweepHitsSphereOS( &center, node0->radius ) )
{
lastDist = m_rayStartOS.quad_distance_to(&center);
}
else
{
node0 = NULL;
}
const IVP_Compact_Ledgetree_Node *node1 = node->right_son();
center.set(node1->center.k);
if ( SweepHitsSphereOS( &center, node1->radius ) )
{
if ( node0 )
{
// can hit, push on stack
int index = list.AddToTail();
float dist1 = m_rayStartOS.quad_distance_to(&center);
if ( lastDist < dist1 )
{
node = node0;
list[index] = node1;
}
else
{
node = node1;
list[index] = node0;
}
}
else
{
node = node1;
}
goto loop_without_store;
}
if ( node0 )
{
node = node0;
goto loop_without_store;
}
}
int last = list.Count()-1;
if ( last < 0 )
break;
node = list[last];
list.FastRemove(last);
}
}
void CTraceSolverSweptObject::InitOSRay( void )
{
// transform ray into object space
m_rayLengthOS = m_obstacleIVP->TransformLengthToLocal( m_ray->m_length );
m_obstacleIVP->TransformPositionToLocal( m_ray->m_start, m_rayStartOS );
// no translation on matrix mult because this is a vector
m_obstacleIVP->RotateRelativePositionToLocal( m_ray->m_delta, m_rayDeltaOS );
m_rayDirOS.set(&m_rayDeltaOS);
m_rayDirOS.normize();
// add_multiple with 3 params assumes no initial value (should be set_add_multiple)
m_rayCenterOS.add_multiple( &m_rayStartOS, &m_rayDeltaOS, 0.5f );
}
void CTraceSolverSweptObject::DoSweep( void )
{
VPROF("TraceSolver::DoSweep");
InitOSRay();
// iterate ledge tree of obstacle
const IVP_Compact_Surface *pSurface = m_obstacleIVP->m_pSurface;
const IVP_Compact_Ledgetree_Node *lt_node_root;
lt_node_root = pSurface->get_compact_ledge_tree_root();
SweepLedgeTree_r( lt_node_root );
}
void CPhysicsTrace::SweepBoxIVP( const Vector &start, const Vector &end, const Vector &mins, const Vector &maxs, const CPhysCollide *pCollide, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr )
{
Ray_t ray;
ray.Init( start, end, mins, maxs );
SweepBoxIVP( ray, MASK_ALL, NULL, pCollide, surfaceOrigin, surfaceAngles, ptr );
}
void CPhysicsTrace::SweepBoxIVP( const Ray_t &raySrc, unsigned int contentsMask, IConvexInfo *pConvexInfo, const CPhysCollide *pCollide, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr )
{
CM_ClearTrace( ptr );
CTraceAABB box( -raySrc.m_Extents, raySrc.m_Extents, raySrc.m_IsRay );
CTraceIVP ivp( pCollide, vec3_origin, surfaceAngles );
// offset the space of this sweep so that the surface is at the origin of the solution space
CTraceRay ray( raySrc, -surfaceOrigin );
CTraceSolverSweptObject solver( ptr, &box, &ray, &ivp, ray.m_start, contentsMask, pConvexInfo );
solver.DoSweep();
VectorAdd( raySrc.m_Start, raySrc.m_StartOffset, ptr->startpos );
VectorMA( ptr->startpos, ptr->fraction, raySrc.m_Delta, ptr->endpos );
// The plane was shifted because we shifted everything over by surfaceOrigin, shift it back
if ( ptr->DidHit() )
{
ptr->plane.dist = DotProduct( ptr->endpos, ptr->plane.normal );
}
}
void CPhysicsTrace::SweepIVP( const Vector &start, const Vector &end, const CPhysCollide *pSweptSurface, const QAngle &sweptAngles, const CPhysCollide *pSurface, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr )
{
CM_ClearTrace( ptr );
CTraceIVP sweptObject( pSweptSurface, vec3_origin, sweptAngles );
// offset the space of this sweep so that the surface is at the origin of the solution space
CTraceIVP ivp( pSurface, vec3_origin, surfaceAngles );
CTraceRay ray( start - surfaceOrigin, end - surfaceOrigin );
IVP_U_BigVector<IVP_Compact_Ledge> objectLedges(32);
IVP_Compact_Ledge_Solver::get_all_ledges( pSweptSurface->GetCompactSurface(), &objectLedges );
for ( int i = objectLedges.len() - 1; i >= 0; --i )
{
trace_t tr;
CM_ClearTrace( &tr );
sweptObject.SetLedge( objectLedges.element_at(i) );
CTraceSolverSweptObject solver( &tr, &sweptObject, &ray, &ivp, start - surfaceOrigin, MASK_ALL, NULL );
// UNDONE: Need just more than 0.25" tolerance here because the output position will be used by vphysics
// UNDONE: Really this should be the collision radius from the environment.
solver.SetEpsilon( g_PhysicsUnits.globalCollisionTolerance );
solver.DoSweep();
if ( tr.fraction < ptr->fraction )
{
*ptr = tr;
}
}
ptr->endpos = start*(1.f-ptr->fraction) + end * ptr->fraction;
if ( ptr->DidHit() )
{
ptr->plane.dist = DotProduct( ptr->endpos, ptr->plane.normal );
}
}
static void CalculateSeparatingPlane( trace_t *ptr, ITraceObject *sweepObject, CTraceRay *ray, ITraceObject *obstacle, simplex_t &simplex );
//-----------------------------------------------------------------------------
// What is this doing? It's going to be hard to understand without reading a
// reference on the GJK algorithm. But here's a quick overview:
// Basically (remember this is glossing over a ton of details!) the
// algorithm is building up a simplex that is trying to contain the origin.
// A simplex is a point, line segment, triangle, or tetrahedron - depending on
// how many verts you have.
// Anyway it slowly builds one of these one vert at a time with a directed search.
// So you start out with a point, then it guesses the next point that would be
// most likely to form a line through the origin. If the line doesn't go quite
// through the origin it tries to find a third point to capture the origin
// within a triangle. If that doesn't work it tries to make a
// tent (tetrahedron) out of the triangle to capture the origin.
//
// But at each step if the origin is not contained within, it tries to
// find which sub-feature is most likely to be in the solution. In
// the point case it's always just the point. In the line/edge case it
// can reduce back to a point (origin is closest to one of the points)
// or be the line (origin is closest to some point between them).
// Same with the triangle (origin is closest to one vert - vert, origin is
// closest to one edge - reduce to that edge, origin is closes to some point
// in the triangle's surface - keep the whole triangle). With a tetrahedron
// keeping the whole isn't possible unless the origin is inside and you're
// done (the origin has been captured).
//
// "You're done" means that there is an intersection between the two
// volumes. Assuming you're testing a sweep, it still has to test whether that
// sweep can be shrunk back until there is no intersection. So it checks that.
// If it's a swept test so it does the search with SolveMeshIntersection
// Otherwise, there's nothing to shrink, so you set startsolid and allsolid
// because it's a point/box in solid test, not a swept box/ray hits solid test.
//
// Why is it trying to capture the origin? Basically GJK sets up a space
// and a convex hull (the minkowski sum) in that space. The convex hull
// represents a field of the distances between different features of the pair
// of objects (e.g. for two circles, this minkowski sum is just a circle).
// So the origin is the point in the field where the distance between the
// objects is zero. This means they intersect.
//-----------------------------------------------------------------------------
#if defined(_X360)
inline void VectorNormalize_FastLowPrecision( Vector &a )
{
float quad = (a.x*a.x) + (a.y*a.y) + (a.z*a.z);
float ilen = __frsqrte(quad);
a.x *= ilen;
a.y *= ilen;
a.z *= ilen;
}
#else
#define VectorNormalize_FastLowPrecision VectorNormalize
#endif
bool CTraceSolver::SweepSingleConvex( void )
{
VPROF("TraceSolver::SweepSingleConvex");
simplex_t simplex;
simplexvert_t vert;
Vector tmp;
simplex.vertCount = 0;
if ( m_pointClosestToIntersection == vec3_origin )
{
m_pointClosestToIntersection.Init(1,0,0);
}
float testLen = 1;
Vector dir = -m_pointClosestToIntersection;
VectorNormalize_FastLowPrecision(dir);
// safe loop, max 100 iterations
for ( int i = 0; i < 100; i++ )
{
// map the direction into the minkowski sum, get a new surface point
vert.testIndex = m_sweepObject->SupportMap( dir, &vert.position );
vert.sweepIndex = m_ray->SupportMap( dir, &tmp );
VectorAdd( vert.position, tmp, vert.position );
vert.obstacleIndex = m_obstacle->SupportMap( -dir, &tmp );
VectorSubtract( vert.position, tmp, vert.position );
testLen = DotProduct( dir, vert.position );
// found a separating axis, no intersection
if ( testLen < 0 )
{
VPROF("SolveSeparation");
// make sure we're separated by at least m_epsilon
testLen = fabs(testLen);
if ( testLen < m_epsilon && m_ray->m_length > 0 )
{
// not separated by enough
Vector normal = dir;
if ( testLen > 0 )
{
// try to find a better separating plane or clip the ray to the current one
for ( int j = 0; j < 20; j++ )
{
Vector lastVert = vert.position;
simplex.SolveGJKSet( vert, &m_pointClosestToIntersection );
dir = -m_pointClosestToIntersection;
VectorNormalize_FastLowPrecision( dir );
// map the direction into the minkowski sum, get a new surface point
vert.testIndex = m_sweepObject->SupportMap( dir, &vert.position );
vert.sweepIndex = m_ray->SupportMap( dir, &tmp );
VectorAdd( vert.position, tmp, vert.position );
vert.obstacleIndex = m_obstacle->SupportMap( -dir, &tmp );
VectorSubtract( vert.position, tmp, vert.position );
// found a separating axis, no intersection
float est = -DotProduct( dir, vert.position );
if ( est > m_epsilon ) // big enough separation, no hit
return false;
// take plane with the most separation
if ( est > testLen )
{
testLen = est;
normal = dir;
}
float last = -DotProduct( dir, lastVert );
// search is not converging, exit.
if ( (est - last) > -1e-4f )
break;
}
}
// This trace is going to miss, but not by enough.
// Hit the separating plane instead
float dot = -DotProduct( m_ray->m_delta, normal );
if ( dot < -(m_epsilon*0.1) || (dot < -1e-4f && testLen < (m_epsilon*0.9)) )
{
CM_ClearTrace( &m_trace );
float backupDistance = m_epsilon - testLen;
backupDistance = -(backupDistance * m_ray->m_baseLength) / dot;
m_traceLength = m_ray->m_length - backupDistance;
if ( m_traceLength < 0 )
{
m_traceLength = 0;
// try sliding along the surface of the minkowski sum
backupDistance = SolveMeshIntersection2D( simplex );
if ( m_ray->m_length > backupDistance )
{
m_traceLength = m_ray->m_length - backupDistance;
}
}
m_trace.plane.normal = -normal;
// this is fixed up by the outer code
//m_trace.endpos = m_ray->m_start*(1.f-m_trace.fraction) + m_ray->m_end*m_trace.fraction;
m_trace.contents = CONTENTS_SOLID;
return true;
}
}
return false;
}
// contains the origin
if ( simplex.SolveGJKSet( vert, &m_pointClosestToIntersection ) )
{
VPROF("TraceSolver::SolveMeshIntersection");
CM_ClearTrace( &m_trace );
// now solve for t along the sweep
if ( m_ray->m_length != 0 )
{
float dist = SolveMeshIntersection( simplex );
if ( dist < m_ray->m_length && dist > 0.f )
{
m_traceLength = (m_ray->m_length - dist);
CalculateSeparatingPlane( &m_trace, m_sweepObject, m_ray, m_obstacle, simplex );
float dot = DotProduct( m_ray->m_dir, m_trace.plane.normal );
if ( dot < 0 )
{
m_traceLength += (m_epsilon / dot);
}
if ( m_traceLength < 0 )
{
m_traceLength = 0;
}
//m_trace.fraction = m_traceLength * m_ray->m_ooBaseLength;
//m_trace.endpos = m_ray->m_start*(1.f-m_trace.fraction) + m_ray->m_end*m_trace.fraction;
m_trace.contents = CONTENTS_SOLID;
}
else
{
// UNDONE: This case happens when you start solid as well as when a false
// intersection is detected at the very end of the trace
m_trace.startsolid = true;
m_trace.allsolid = true;
m_traceLength = 0;
}
}
else
{
m_trace.startsolid = true;
m_trace.allsolid = true;
m_traceLength = 0;
}
return true;
}
dir = -m_pointClosestToIntersection;
VectorNormalize_FastLowPrecision( dir );
}
// BUGBUG: The solution never converged - something is probably wrong!
AssertMsg( false, "Solution never converged.");
return false;
}
// NEW SWEPT GJK SOLVER 2/16/2006
// convenience routines - just makes the code a little simpler.
FORCEINLINE bool simplex_t::TriangleSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &faceNormal, Vector *pOut )
{
vertCount = 3;
verts[outIndex] = newPoint;
*pOut = -faceNormal;
return false;
}
FORCEINLINE bool simplex_t::EdgeSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &edge, Vector *pOut )
{
vertCount = 2;
verts[outIndex] = newPoint;
Vector cross;
CrossProduct( edge, newPoint.position, cross );
CrossProduct( cross, edge, *pOut );
return false;
}
FORCEINLINE bool simplex_t::PointSimplex( const simplexvert_t &newPoint, Vector *pOut )
{
vertCount = 1;
verts[0] = newPoint;
*pOut = newPoint.position;
return false;
}
// In general the voronoi region routines have comments referring to the verts
// All of the code assumes that vert A is the new vert being added to the set
// verts B, C, D are the previous set. If BCD is a triangle it is assumed to be
// counter-clockwise winding order. This must be maintained by the code!
// parametric value for closes point on a line segment (p0->p1) to the origin.
bool simplex_t::SolveVoronoiRegion2( const simplexvert_t &newPoint, Vector *pOut )
{
// solve the line segment AB (where A is the new point)
Vector AB = verts[0].position - newPoint.position;
float d = DotProduct(AB, newPoint.position);
if ( d < 0 )
{
return EdgeSimplex(newPoint, 1, AB, pOut);
}
else
{
return PointSimplex(newPoint, pOut);
}
}
// UNDONE: Collapse these routines into a single general routine?
bool simplex_t::SolveVoronoiRegion3( const simplexvert_t &newPoint, Vector *pOut )
{
// solve the triangle ABC (where A is the new point)
Vector AB = verts[0].position - newPoint.position;
Vector AC = verts[1].position - newPoint.position;
Vector ABC;
CrossProduct(AB, AC, ABC);
Vector ABCxAC;
CrossProduct(ABC, AC, ABCxAC);
float d = DotProduct(ABCxAC, newPoint.position);
// edge AC or edgeAB / A?
if ( d < 0 )
{
d = DotProduct(AC, newPoint.position);
if ( d < 0 )
{
// edge AC
return EdgeSimplex(newPoint, 0, AC, pOut);
}
}
else
{
// face or A / edge AB?
Vector ABxABC;
CrossProduct(AB, ABC, ABxABC);
d = DotProduct(ABxABC, newPoint.position);
if ( d > 0 )
{
// closest to face
vertCount = 3;
d = DotProduct(ABC, newPoint.position);
// in front of face, return opposite direction
if ( d < 0 )
{
verts[2] = newPoint;
*pOut = -ABC;
return false;
}
verts[2] = verts[1]; // swap to keep CCW
verts[1] = newPoint;
*pOut = ABC;
return false;
}
}
// edge AB or A
d = DotProduct(AB, newPoint.position);
if ( d < 0 )
{
return EdgeSimplex(newPoint, 1, AB, pOut);
}
return PointSimplex(newPoint, pOut);
}
bool simplex_t::SolveVoronoiRegion4( const simplexvert_t &newPoint, Vector *pOut )
{
// solve the tetrahedron ABCD (where A is the new point)
// compute edge vectors
Vector AB = verts[0].position - newPoint.position;
Vector AC = verts[1].position - newPoint.position;
Vector AD = verts[2].position - newPoint.position;
// compute face normals
Vector ABC, ACD, ADB;
CrossProduct( AB, AC, ABC );
CrossProduct( AC, AD, ACD );
CrossProduct( AD, AB, ADB );
// edge plane normals
Vector ABCxAC, ABxABC;
CrossProduct( ABC, AC, ABCxAC );
CrossProduct( AB, ABC, ABxABC );
Vector ACDxAD, ACxACD;
CrossProduct( ACD, AD, ACDxAD );
CrossProduct( AC, ACD, ACxACD );
Vector ADBxAB, ADxADB;
CrossProduct( ADB, AB, ADBxAB );
CrossProduct( AD, ADB, ADxADB );
int faceFlags = 0;
float d;
d = DotProduct(ABC,newPoint.position);
if ( d < 0 )
{
faceFlags |= 0x1;
}
d = DotProduct(ACD,newPoint.position);
if ( d < 0 )
{
faceFlags |= 0x2;
}
d = DotProduct(ADB,newPoint.position);
if ( d < 0 )
{
faceFlags |= 0x4;
}
switch( faceFlags )
{
case 0:
// inside all 3 faces, we're done
verts[3] = newPoint;
vertCount = 4;
return true;
case 1:
// ABC only, solve as a triangle
return SolveVoronoiRegion3(newPoint, pOut);
case 2:
// ACD only, solve as a triangle
verts[0] = verts[2]; // collapse BCD to DC
return SolveVoronoiRegion3(newPoint, pOut);
case 4:
// ADB only, solve as a triangle
verts[1] = verts[2]; // collapse BCD to BD
return SolveVoronoiRegion3(newPoint, pOut);
case 3:
{
// in front of ABC & ACD
d = DotProduct(ABCxAC, newPoint.position);
if ( d < 0 )
{
d = DotProduct(ACxACD, newPoint.position);
if ( d < 0 )
{
d = DotProduct(AC,newPoint.position);
if ( d < 0 )
{
// edge AC
return EdgeSimplex( newPoint, 0, AC, pOut);
}
// point A
return PointSimplex(newPoint, pOut);
}
else
{
d = DotProduct(ACDxAD, newPoint.position);
if ( d < 0 )
{
// edge AD
verts[0] = verts[2]; // collapse BCD to D
return EdgeSimplex(newPoint, 1, AD, pOut);
}
// face ACD
return TriangleSimplex(newPoint,0,ACD, pOut);
}
}
else
{
d = DotProduct(ABxABC, newPoint.position);
if ( d < 0 )
{
d = DotProduct(AB, newPoint.position);
if ( d < 0 )
{
// edge AB
return EdgeSimplex(newPoint, 1, AB, pOut);
}
return PointSimplex(newPoint, pOut);
}
return TriangleSimplex(newPoint,2,ABC,pOut);
}
}
break;
case 5:
{
// in front of ADB & ABC
d = DotProduct(ADBxAB, newPoint.position);
if ( d < 0 )
{
d = DotProduct(ABxABC, newPoint.position);
if ( d < 0 )
{
d = DotProduct(AB,newPoint.position);
if ( d < 0 )
{
// edge AB
return EdgeSimplex( newPoint, 1, AB , pOut);
}
// point A
return PointSimplex(newPoint, pOut);
}
else
{
d = DotProduct(ABCxAC, newPoint.position);
if ( d < 0 )
{
// edge AC
return EdgeSimplex(newPoint, 0, AC, pOut);
}
// face ABC
return TriangleSimplex(newPoint,2,ABC,pOut);
}
}
else
{
d = DotProduct(ADxADB, newPoint.position);
if ( d < 0 )
{
d = DotProduct(AD, newPoint.position);
if ( d < 0 )
{
// edge AD
verts[0] = verts[2]; // collapse BCD to D
return EdgeSimplex(newPoint, 1, AD, pOut);
}
return PointSimplex(newPoint, pOut);
}
return TriangleSimplex(newPoint,1,ADB,pOut);
}
}
break;
case 6:
{
// in front of ACD & ADB
d = DotProduct(ACDxAD, newPoint.position);
if ( d < 0 )
{
d = DotProduct(ADxADB, newPoint.position);
if ( d < 0 )
{
d = DotProduct(AD,newPoint.position);
if ( d < 0 )
{
// edge AD
verts[0] = verts[2]; // collapse BCD to D
return EdgeSimplex(newPoint, 1, AD, pOut);
}
// point A
return PointSimplex(newPoint, pOut);
}
else
{
d = DotProduct(ADBxAB, newPoint.position);
if ( d < 0 )
{
// edge AB
return EdgeSimplex(newPoint, 1, AB, pOut);
}
// face ADB
return TriangleSimplex(newPoint,1,ADB, pOut);
}
}
else
{
d = DotProduct(ACxACD, newPoint.position);
if ( d < 0 )
{
d = DotProduct(AC, newPoint.position);
if ( d < 0 )
{
// edge AC
return EdgeSimplex(newPoint, 0, AC, pOut);
}
return PointSimplex(newPoint, pOut);
}
return TriangleSimplex(newPoint,0,ACD, pOut);
}
}
break;
case 7:
{
d = DotProduct(AB, newPoint.position);
if ( d < 0 )
{
return EdgeSimplex(newPoint, 1, AB, pOut);
}
else
{
d = DotProduct(AC, newPoint.position);
if ( d < 0 )
{
return EdgeSimplex(newPoint, 0, AC, pOut);
}
else
{
d = DotProduct(AD, newPoint.position);
if ( d < 0 )
{
verts[0] = verts[2]; // collapse BCD to D
return EdgeSimplex(newPoint, 1, AD, pOut);
}
return PointSimplex(newPoint, pOut);
}
}
}
}
verts[3] = newPoint;
vertCount = 4;
return true;
}
bool simplex_t::SolveGJKSet( const simplexvert_t &w, Vector *pOut )
{
VPROF("TraceSolver::simplex::SolveGJKSet");
#if 0
for ( int v = 0; v < vertCount; v++ )
{
for ( int v2 = v+1; v2 < vertCount; v2++ )
{
if ( (verts[v].obstacleIndex == verts[v2].obstacleIndex) &&
(verts[v].sweepIndex == verts[v2].sweepIndex) &&
(verts[v].testIndex == verts[v2].testIndex) )
{
// same vert in the list twice! degenerate
Assert(0);
}
}
}
#endif
switch( vertCount )
{
case 0:
vertCount = 1;
verts[0] = w;
*pOut = w.position;
return false;
case 1:
return SolveVoronoiRegion2( w, pOut );
case 2:
return SolveVoronoiRegion3( w, pOut );
case 3:
return SolveVoronoiRegion4( w, pOut );
}
return true;
}
void CalculateSeparatingPlane( trace_t *ptr, ITraceObject *sweepObject, CTraceRay *ray, ITraceObject *obstacle, simplex_t &simplex )
{
int testCount = 1, obstacleCount = 1;
unsigned int testIndex[4], obstacleIndex[4];
testIndex[0] = simplex.verts[0].testIndex;
obstacleIndex[0] = simplex.verts[0].obstacleIndex;
Assert( simplex.vertCount <= 4 );
int i, j;
for ( i = 1; i < simplex.vertCount; i++ )
{
for ( j = 0; j < obstacleCount; j++ )
{
if ( obstacleIndex[j] == simplex.verts[i].obstacleIndex )
break;
}
if ( j == obstacleCount )
{
obstacleIndex[obstacleCount++] = simplex.verts[i].obstacleIndex;
}
for ( j = 0; j < testCount; j++ )
{
if ( testIndex[j] == simplex.verts[i].testIndex )
break;
}
if ( j == testCount )
{
testIndex[testCount++] = simplex.verts[i].testIndex;
}
}
if ( simplex.vertCount < 3 )
{
if ( simplex.vertCount == 2 && testCount == 2 )
{
// edge / point
Vector t0 = sweepObject->GetVertByIndex( testIndex[0] );
Vector t1 = sweepObject->GetVertByIndex( testIndex[1] );
Vector edge = t1-t0;
Vector tangent = CrossProduct( edge, ray->m_delta );
ptr->plane.normal = CrossProduct( edge, tangent );
VectorNormalize( ptr->plane.normal );
ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal );
return;
}
}
if ( testCount == 3 )
{
// face / xxx
Vector t0 = sweepObject->GetVertByIndex( testIndex[0] );
Vector t1 = sweepObject->GetVertByIndex( testIndex[1] );
Vector t2 = sweepObject->GetVertByIndex( testIndex[2] );
ptr->plane.normal = CrossProduct( t1-t0, t2-t0 );
VectorNormalize( ptr->plane.normal );
if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 )
{
ptr->plane.normal = -ptr->plane.normal;
}
ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal );
}
else if ( testCount == 2 && obstacleCount == 2 )
{
// edge / edge
Vector t0 = sweepObject->GetVertByIndex( testIndex[0] );
Vector t1 = sweepObject->GetVertByIndex( testIndex[1] );
Vector t2 = obstacle->GetVertByIndex( obstacleIndex[0] );
Vector t3 = obstacle->GetVertByIndex( obstacleIndex[1] );
ptr->plane.normal = CrossProduct( t1-t0, t3-t2 );
VectorNormalize( ptr->plane.normal );
if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 )
{
ptr->plane.normal = -ptr->plane.normal;
}
ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal );
}
else if ( obstacleCount == 3 )
{
// xxx / face
Vector t0 = obstacle->GetVertByIndex( obstacleIndex[0] );
Vector t1 = obstacle->GetVertByIndex( obstacleIndex[1] );
Vector t2 = obstacle->GetVertByIndex( obstacleIndex[2] );
ptr->plane.normal = CrossProduct( t1-t0, t2-t0 );
VectorNormalize( ptr->plane.normal );
if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 )
{
ptr->plane.normal = -ptr->plane.normal;
}
ptr->plane.dist = DotProduct( t0, ptr->plane.normal );
}
else
{
ptr->plane.normal = -ray->m_dir;
if ( simplex.vertCount )
{
ptr->plane.dist = DotProduct( ptr->plane.normal, obstacle->GetVertByIndex( simplex.verts[0].obstacleIndex ) );
}
else
ptr->plane.dist = 0.f;
}
}
// clip a direction vector to a plane (ray begins at the origin)
inline float Clip( const Vector &dir, const Vector &pos, const Vector &normal )
{
float dist = DotProduct(pos, normal);
float cosTheta = DotProduct(dir,normal);
if ( cosTheta > 0 )
return dist / cosTheta;
// parallel or not facing the plane
return 1e16f;
}
// This is the first iteration of solving time of intersection.
// It needs to test all 4 faces of the tetrahedron to find the one the ray leaves through
// this is done by finding the closest plane by clipping the ray to each plane
Vector simplex_t::ClipRayToTetrahedronBase( const Vector &dir )
{
Vector AB = verts[0].position - verts[3].position;
Vector AC = verts[1].position - verts[3].position;
Vector AD = verts[2].position - verts[3].position;
Vector BC = verts[1].position - verts[0].position;
Vector BD = verts[2].position - verts[0].position;
// compute face normals
Vector ABC, ACD, ADB, BCD;
CrossProduct( AB, AC, ABC );
CrossProduct( AC, AD, ACD );
CrossProduct( AD, AB, ADB );
CrossProduct( BD, BC, BCD ); // flipped to point out of the tetrahedron
// NOTE: These cancel out in the clipping equation
//VectorNormalize(ABC);
//VectorNormalize(ACD);
//VectorNormalize(ADB);
//VectorNormalize(BCD);
// Assert valid tetrahedron/winding order
#if CHECK_TOI_CALCS
Assert(DotProduct(verts[2].position, ABC) < DotProduct(verts[3].position, ABC ));
Assert(DotProduct(verts[0].position, ACD) < DotProduct(verts[3].position, ACD ));
Assert(DotProduct(verts[1].position, ADB) < DotProduct(verts[3].position, ADB ));
Assert(DotProduct(verts[3].position, BCD) < DotProduct(verts[0].position, BCD ));
#endif
float dists[4];
dists[0] = Clip( dir, verts[3].position, ABC );
dists[1] = Clip( dir, verts[3].position, ACD );
dists[2] = Clip( dir, verts[3].position, ADB );
dists[3] = Clip( dir, verts[0].position, BCD );
float dmin = dists[3];
int best = 3;
for ( int i = 0; i < 3; i++ )
{
if ( dists[i] < dmin )
{
best = i;
dmin = dists[i];
}
}
vertCount = 3;
// push back down to a triangle
// Note that we need to preserve winding so that the vector order assumptions above are still valid!
switch( best )
{
case 0:
verts[2] = verts[3];
return ABC;
case 1:
verts[0] = verts[3];
return ACD;
case 2:
verts[1] = verts[3];
return ADB;
case 3:
// swap 1 & 2
verts[3] = verts[1];
verts[1] = verts[2];
verts[2] = verts[3];
return BCD;
}
Assert(0); // failed!
return vec3_origin;
}
// for subsequent iterations you don't need to test one of the faces (face you previously left through)
// so only test the three faces involving the new point A
Vector simplex_t::ClipRayToTetrahedron( const Vector &dir )
{
Vector AB = verts[0].position - verts[3].position;
Vector AC = verts[1].position - verts[3].position;
Vector AD = verts[2].position - verts[3].position;
// compute face normals
Vector ABC, ACD, ADB;
CrossProduct( AB, AC, ABC );
CrossProduct( AC, AD, ACD );
CrossProduct( AD, AB, ADB );
// NOTE: These cancel out in the clipping equation
//VectorNormalize(ABC);
//VectorNormalize(ACD);
//VectorNormalize(ADB);
// Assert valid tetrahedron/winding order
#if CHECK_TOI_CALCS
Assert(DotProduct(verts[2].position, ABC) < DotProduct(verts[3].position, ABC ));
Assert(DotProduct(verts[0].position, ACD) < DotProduct(verts[3].position, ACD ));
Assert(DotProduct(verts[1].position, ADB) < DotProduct(verts[3].position, ADB ));
#endif
float dists[3];
dists[0] = Clip( dir, verts[3].position, ABC );
dists[1] = Clip( dir, verts[3].position, ACD );
dists[2] = Clip( dir, verts[3].position, ADB );
float dmin = dists[2];
int best = 2;
for ( int i = 0; i < 2; i++ )
{
if ( dists[i] < dmin )
{
best = i;
dmin = dists[i];
}
}
vertCount = 3;
// push back down to a triangle
// Note that we need to preserve winding so that the vector order assumptions above are still valid!
switch( best )
{
case 0:
verts[2] = verts[3];
return ABC;
case 1:
verts[0] = verts[3];
return ACD;
case 2:
verts[1] = verts[3];
return ADB;
}
return vec3_origin;
}
float simplex_t::ClipRayToTriangle( const Vector &dir, float epsilon )
{
Vector AB = verts[0].position - verts[2].position;
Vector AC = verts[1].position - verts[2].position;
Vector BC = verts[1].position - verts[0].position;
// compute face normal
Vector ABC;
CrossProduct( AB, AC, ABC ); // this points toward the origin
VectorNormalize(ABC);
Vector edgeAB, edgeAC, edgeBC;
// these should point out of the triangle
CrossProduct( AB, ABC, edgeAB );
CrossProduct( ABC, AC, edgeAC );
CrossProduct( BC, ABC, edgeBC );
// NOTE: These cancel out in the clipping equation
VectorNormalize(edgeAB);
VectorNormalize(edgeAC);
VectorNormalize(edgeBC);
#if CHECK_TOI_CALCS
Assert(DotProduct(verts[2].position, ABC) < 0); // points toward
// Assert valid triangle/winding order (all normals point away from the origin)
Assert(DotProduct(verts[2].position, edgeAB) > 0);
Assert(DotProduct(verts[2].position, edgeAC) > 0);
Assert(DotProduct(verts[0].position, edgeBC) > 0);
#endif
float dists[3];
dists[0] = Clip( dir, verts[0].position, edgeAB );
dists[1] = Clip( dir, verts[1].position, edgeAC );
dists[2] = Clip( dir, verts[1].position, edgeBC );
Vector *normals[3] = {&edgeAB, &edgeAC, &edgeBC};
float dmin = dists[0];
int best = 0;
if ( dists[1] < dmin )
{
dmin = dists[1];
best = 1;
}
if ( dists[2] < dmin )
{
best = 2;
dmin = dists[2];
}
float dot = DotProduct( dir, *normals[best] );
if ( dot <= 0 )
return 1e16f;
dmin += epsilon/dot;
return dmin;
}
// Solve for time of intersection along the sweep
// Do this by iteratively clipping the ray to the tetrahedron containing the origin
// when a triangle is found intersecting the ray, reduce the simplex to that triangle
// and then re-expand it to a tetrahedron using the support point normal to the triangle (away from the origin)
// iterate until no new points can be found. That's the surface of the sum.
float CTraceSolver::SolveMeshIntersection( simplex_t &simplex )
{
Vector tmp;
Assert( simplex.vertCount == 4 );
if ( simplex.vertCount < 4 )
return 0.0f;
Vector v = simplex.ClipRayToTetrahedronBase( m_ray->m_dir );
simplexvert_t vert;
// safe loop, max 100 iterations
for ( int i = 0; i < 100; i++ )
{
VectorNormalize(v);
vert.testIndex = m_sweepObject->SupportMap( v, &vert.position );
vert.sweepIndex = m_ray->SupportMap( v, &tmp );
VectorAdd( vert.position, tmp, vert.position );
vert.obstacleIndex = m_obstacle->SupportMap( -v, &tmp );
VectorSubtract( vert.position, tmp, vert.position );
// map the new separating axis (NOTE: This test is inverted from the GJK - we are trying to get out, not in)
// found a separating axis, we've moved the sweep back enough
float dist = DotProduct( v, simplex.verts[0].position ) + TEST_EPSILON;
if ( DotProduct( v, vert.position ) <= dist )
{
Vector BC = simplex.verts[1].position - simplex.verts[0].position;
Vector BD = simplex.verts[2].position - simplex.verts[0].position;
// compute face normals
Vector BCD;
CrossProduct( BC, BD, BCD );
// NOTE: This cancels out inside Clip()
//VectorNormalize( BCD );
// clip ray to triangle
return Clip( m_ray->m_dir, simplex.verts[0].position, BCD );
}
// add the new vert
simplex.verts[simplex.vertCount] = vert;
simplex.vertCount++;
v = simplex.ClipRayToTetrahedron( m_ray->m_dir );
}
Assert(0);
return 0.0f;
}
// similar to SolveMeshIntersection, but solves projected into the 2D triangle simplex remaining
// this is used for the near miss case
float CTraceSolver::SolveMeshIntersection2D( simplex_t &simplex )
{
AssertMsg( simplex.vertCount == 3, "simplex.vertCount != 3: %d", simplex.vertCount );
if ( simplex.vertCount != 3 )
return 0.0f;
// note: This should really do this iteratively in case the triangle is coplanar with another triangle that
// is between this one and the edge of the sum in this plane
float dist = simplex.ClipRayToTriangle( m_ray->m_dir, m_epsilon );
return dist;
}
static const Vector g_xpos(1,0,0), g_xneg(-1,0,0);
static const Vector g_ypos(0,1,0), g_yneg(0,-1,0);
static const Vector g_zpos(0,0,1), g_zneg(0,0,-1);
void TraceGetAABB_r( Vector *pMins, Vector *pMaxs, const IVP_Compact_Ledgetree_Node *node, CTraceIVP &ivp )
{
if ( node->is_terminal() == IVP_TRUE )
{
Vector tmp;
ivp.SetLedge( node->get_compact_ledge() );
ivp.SupportMap( g_xneg, &tmp );
AddPointToBounds( tmp, *pMins, *pMaxs );
ivp.SupportMap( g_yneg, &tmp );
AddPointToBounds( tmp, *pMins, *pMaxs );
ivp.SupportMap( g_zneg, &tmp );
AddPointToBounds( tmp, *pMins, *pMaxs );
ivp.SupportMap( g_xpos, &tmp );
AddPointToBounds( tmp, *pMins, *pMaxs );
ivp.SupportMap( g_ypos, &tmp );
AddPointToBounds( tmp, *pMins, *pMaxs );
ivp.SupportMap( g_zpos, &tmp );
AddPointToBounds( tmp, *pMins, *pMaxs );
return;
}
TraceGetAABB_r( pMins, pMaxs, node->left_son(), ivp );
TraceGetAABB_r( pMins, pMaxs, node->right_son(), ivp );
}
void CPhysicsTrace::GetAABB( Vector *pMins, Vector *pMaxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles )
{
CTraceIVP ivp( pCollide, collideOrigin, collideAngles );
if ( ivp.SetSingleConvex() )
{
Vector tmp;
ivp.SupportMap( g_xneg, &tmp );
pMins->x = tmp.x;
ivp.SupportMap( g_yneg, &tmp );
pMins->y = tmp.y;
ivp.SupportMap( g_zneg, &tmp );
pMins->z = tmp.z;
ivp.SupportMap( g_xpos, &tmp );
pMaxs->x = tmp.x;
ivp.SupportMap( g_ypos, &tmp );
pMaxs->y = tmp.y;
ivp.SupportMap( g_zpos, &tmp );
pMaxs->z = tmp.z;
}
else
{
const IVP_Compact_Ledgetree_Node *lt_node_root;
lt_node_root = pCollide->GetCompactSurface()->get_compact_ledge_tree_root();
ClearBounds( *pMins, *pMaxs );
TraceGetAABB_r( pMins, pMaxs, lt_node_root, ivp );
}
// JAY: Disable this here, do it in the engine instead. That way the tools get
// accurate bboxes
#if 0
const float radius = g_PhysicsUnits.collisionSweepEpsilon;
mins -= Vector(radius,radius,radius);
maxs += Vector(radius,radius,radius);
#endif
}
void TraceGetExtent_r( const IVP_Compact_Ledgetree_Node *node, CTraceIVP &ivp, const Vector &dir, float &dot, Vector &point )
{
if ( node->is_terminal() == IVP_TRUE )
{
ivp.SetLedge( node->get_compact_ledge() );
Vector tmp;
ivp.SupportMap( dir, &tmp );
float newDot = DotProduct( tmp, dir );
if ( newDot > dot )
{
dot = newDot;
point = tmp;
}
return;
}
TraceGetExtent_r( node->left_son(), ivp, dir, dot, point );
TraceGetExtent_r( node->right_son(), ivp, dir, dot, point );
}
Vector CPhysicsTrace::GetExtent( const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, const Vector &direction )
{
CTraceIVP ivp( pCollide, collideOrigin, collideAngles );
if ( ivp.SetSingleConvex() )
{
Vector tmp;
ivp.SupportMap( direction, &tmp );
return tmp;
}
else
{
const IVP_Compact_Ledgetree_Node *lt_node_root;
lt_node_root = pCollide->GetCompactSurface()->get_compact_ledge_tree_root();
Vector out = vec3_origin;
float tmp = -1e6f;
TraceGetExtent_r( lt_node_root, ivp, direction, tmp, out );
return out;
}
}
bool CPhysicsTrace::IsBoxIntersectingCone( const Vector &boxAbsMins, const Vector &boxAbsMaxs, const truncatedcone_t &cone )
{
trace_t tr;
CM_ClearTrace( &tr );
bool bPoint = (boxAbsMins == boxAbsMaxs) ? true : false;
CTraceAABB box( boxAbsMins - cone.origin, boxAbsMaxs - cone.origin, bPoint );
CTraceCone traceCone( cone, -cone.origin );
// offset the space of this sweep so that the surface is at the origin of the solution space
CTraceRay ray( vec3_origin, vec3_origin );
CTraceSolver solver( &tr, &box, &ray, &traceCone, boxAbsMaxs );
solver.DoSweep();
return tr.startsolid;
}
CPhysicsTrace::CPhysicsTrace()
{
}
CPhysicsTrace::~CPhysicsTrace()
{
}
CVisitHash::CVisitHash()
{
m_vertVisitID = 1;
memset( m_vertVisit, 0, sizeof(m_vertVisit) );
}
Reference in New Issue View Git Blame Copy Permalink