-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtrace.cpp
2474 lines (2230 loc) · 70.7 KB
/
trace.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
//========= 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;