2017-08-01 20:30:58 +08:00
/*
Bullet Continuous Collision Detection and Physics Library
Copyright ( c ) 2003 - 2007 Erwin Coumans http : //continuousphysics.com/Bullet/
This software is provided ' as - is ' , without any express or implied warranty .
In no event will the authors be held liable for any damages arising from the use of this software .
Permission is granted to anyone to use this software for any purpose ,
including commercial applications , and to alter it and redistribute it freely ,
subject to the following restrictions :
1. The origin of this software must not be misrepresented ; you must not claim that you wrote the original software . If you use this software in a product , an acknowledgment in the product documentation would be appreciated but is not required .
2. Altered source versions must be plainly marked as such , and must not be misrepresented as being the original software .
3. This notice may not be removed or altered from any source distribution .
*/
///btDbvt implementation by Nathanael Presson
# ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
# define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
# include "LinearMath/btAlignedObjectArray.h"
# include "LinearMath/btVector3.h"
# include "LinearMath/btTransform.h"
# include "LinearMath/btAabbUtil2.h"
//
// Compile time configuration
//
// Implementation profiles
2019-01-03 21:26:51 +08:00
# define DBVT_IMPL_GENERIC 0 // Generic implementation
# define DBVT_IMPL_SSE 1 // SSE
2017-08-01 20:30:58 +08:00
// Template implementation of ICollide
# ifdef _WIN32
2019-01-03 21:26:51 +08:00
# if (defined(_MSC_VER) && _MSC_VER >= 1400)
# define DBVT_USE_TEMPLATE 1
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
# define DBVT_USE_TEMPLATE 0
2017-08-01 20:30:58 +08:00
# endif
# else
2019-01-03 21:26:51 +08:00
# define DBVT_USE_TEMPLATE 0
2017-08-01 20:30:58 +08:00
# endif
// Use only intrinsics instead of inline asm
2019-01-03 21:26:51 +08:00
# define DBVT_USE_INTRINSIC_SSE 1
2017-08-01 20:30:58 +08:00
// Using memmov for collideOCL
2019-01-03 21:26:51 +08:00
# define DBVT_USE_MEMMOVE 1
2017-08-01 20:30:58 +08:00
// Enable benchmarking code
2019-01-03 21:26:51 +08:00
# define DBVT_ENABLE_BENCHMARK 0
2017-08-01 20:30:58 +08:00
// Inlining
2019-01-03 21:26:51 +08:00
# define DBVT_INLINE SIMD_FORCE_INLINE
2017-08-01 20:30:58 +08:00
// Specific methods implementation
//SSE gives errors on a MSVC 7.1
2019-01-03 21:26:51 +08:00
# if defined(BT_USE_SSE) //&& defined (_WIN32)
# define DBVT_SELECT_IMPL DBVT_IMPL_SSE
# define DBVT_MERGE_IMPL DBVT_IMPL_SSE
# define DBVT_INT0_IMPL DBVT_IMPL_SSE
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
# define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC
# define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC
# define DBVT_INT0_IMPL DBVT_IMPL_GENERIC
2017-08-01 20:30:58 +08:00
# endif
2019-01-03 21:26:51 +08:00
# if (DBVT_SELECT_IMPL == DBVT_IMPL_SSE) || \
( DBVT_MERGE_IMPL = = DBVT_IMPL_SSE ) | | \
( DBVT_INT0_IMPL = = DBVT_IMPL_SSE )
2017-08-01 20:30:58 +08:00
# include <emmintrin.h>
# endif
//
// Auto config and checks
//
# if DBVT_USE_TEMPLATE
2019-01-03 21:26:51 +08:00
# define DBVT_VIRTUAL
2017-08-01 20:30:58 +08:00
# define DBVT_VIRTUAL_DTOR(a)
2019-01-03 21:26:51 +08:00
# define DBVT_PREFIX template <typename T>
# define DBVT_IPOLICY T& policy
# define DBVT_CHECKTYPE \
static const ICollide & typechecker = * ( T * ) 1 ; \
( void ) typechecker ;
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
# define DBVT_VIRTUAL_DTOR(a) \
virtual ~ a ( ) { }
# define DBVT_VIRTUAL virtual
2017-08-01 20:30:58 +08:00
# define DBVT_PREFIX
2019-01-03 21:26:51 +08:00
# define DBVT_IPOLICY ICollide& policy
2017-08-01 20:30:58 +08:00
# define DBVT_CHECKTYPE
# endif
# if DBVT_USE_MEMMOVE
2019-01-03 21:26:51 +08:00
# if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
2017-08-01 20:30:58 +08:00
# include <memory.h>
# endif
# include <string.h>
# endif
# ifndef DBVT_USE_TEMPLATE
# error "DBVT_USE_TEMPLATE undefined"
# endif
# ifndef DBVT_USE_MEMMOVE
# error "DBVT_USE_MEMMOVE undefined"
# endif
# ifndef DBVT_ENABLE_BENCHMARK
# error "DBVT_ENABLE_BENCHMARK undefined"
# endif
# ifndef DBVT_SELECT_IMPL
# error "DBVT_SELECT_IMPL undefined"
# endif
# ifndef DBVT_MERGE_IMPL
# error "DBVT_MERGE_IMPL undefined"
# endif
# ifndef DBVT_INT0_IMPL
# error "DBVT_INT0_IMPL undefined"
# endif
//
// Defaults volumes
//
2019-01-03 21:26:51 +08:00
/* btDbvtAabbMm */
struct btDbvtAabbMm
2017-08-01 20:30:58 +08:00
{
2020-01-09 01:05:43 +08:00
DBVT_INLINE btDbvtAabbMm ( ) { }
2019-01-03 21:26:51 +08:00
DBVT_INLINE btVector3 Center ( ) const { return ( ( mi + mx ) / 2 ) ; }
DBVT_INLINE btVector3 Lengths ( ) const { return ( mx - mi ) ; }
DBVT_INLINE btVector3 Extents ( ) const { return ( ( mx - mi ) / 2 ) ; }
DBVT_INLINE const btVector3 & Mins ( ) const { return ( mi ) ; }
DBVT_INLINE const btVector3 & Maxs ( ) const { return ( mx ) ; }
static inline btDbvtAabbMm FromCE ( const btVector3 & c , const btVector3 & e ) ;
static inline btDbvtAabbMm FromCR ( const btVector3 & c , btScalar r ) ;
static inline btDbvtAabbMm FromMM ( const btVector3 & mi , const btVector3 & mx ) ;
static inline btDbvtAabbMm FromPoints ( const btVector3 * pts , int n ) ;
static inline btDbvtAabbMm FromPoints ( const btVector3 * * ppts , int n ) ;
DBVT_INLINE void Expand ( const btVector3 & e ) ;
DBVT_INLINE void SignedExpand ( const btVector3 & e ) ;
DBVT_INLINE bool Contain ( const btDbvtAabbMm & a ) const ;
DBVT_INLINE int Classify ( const btVector3 & n , btScalar o , int s ) const ;
DBVT_INLINE btScalar ProjectMinimum ( const btVector3 & v , unsigned signs ) const ;
DBVT_INLINE friend bool Intersect ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b ) ;
DBVT_INLINE friend bool Intersect ( const btDbvtAabbMm & a ,
const btVector3 & b ) ;
DBVT_INLINE friend btScalar Proximity ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b ) ;
DBVT_INLINE friend int Select ( const btDbvtAabbMm & o ,
const btDbvtAabbMm & a ,
const btDbvtAabbMm & b ) ;
DBVT_INLINE friend void Merge ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b ,
btDbvtAabbMm & r ) ;
DBVT_INLINE friend bool NotEqual ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b ) ;
DBVT_INLINE btVector3 & tMins ( ) { return ( mi ) ; }
DBVT_INLINE btVector3 & tMaxs ( ) { return ( mx ) ; }
2017-08-01 20:30:58 +08:00
private :
2019-01-03 21:26:51 +08:00
DBVT_INLINE void AddSpan ( const btVector3 & d , btScalar & smi , btScalar & smx ) const ;
2017-08-01 20:30:58 +08:00
private :
2019-01-03 21:26:51 +08:00
btVector3 mi , mx ;
2017-08-01 20:30:58 +08:00
} ;
2019-01-03 21:26:51 +08:00
// Types
typedef btDbvtAabbMm btDbvtVolume ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
/* btDbvtNode */
struct btDbvtNode
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
btDbvtVolume volume ;
btDbvtNode * parent ;
DBVT_INLINE bool isleaf ( ) const { return ( childs [ 1 ] = = 0 ) ; }
DBVT_INLINE bool isinternal ( ) const { return ( ! isleaf ( ) ) ; }
union {
btDbvtNode * childs [ 2 ] ;
void * data ;
int dataAsInt ;
2017-08-01 20:30:58 +08:00
} ;
} ;
2020-01-09 01:05:43 +08:00
/* btDbv(normal)tNode */
struct btDbvntNode
{
btDbvtVolume volume ;
btVector3 normal ;
btScalar angle ;
DBVT_INLINE bool isleaf ( ) const { return ( childs [ 1 ] = = 0 ) ; }
DBVT_INLINE bool isinternal ( ) const { return ( ! isleaf ( ) ) ; }
btDbvntNode * childs [ 2 ] ;
void * data ;
btDbvntNode ( const btDbvtNode * n )
: volume ( n - > volume )
, normal ( 0 , 0 , 0 )
2020-04-27 16:15:23 +08:00
, angle ( 0 )
2020-01-09 01:05:43 +08:00
, data ( n - > data )
{
childs [ 0 ] = 0 ;
childs [ 1 ] = 0 ;
}
~ btDbvntNode ( )
{
if ( childs [ 0 ] )
delete childs [ 0 ] ;
if ( childs [ 1 ] )
delete childs [ 1 ] ;
}
} ;
2017-08-01 20:30:58 +08:00
typedef btAlignedObjectArray < const btDbvtNode * > btNodeStack ;
///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
2019-01-03 21:26:51 +08:00
struct btDbvt
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
/* Stack element */
struct sStkNN
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btDbvtNode * a ;
const btDbvtNode * b ;
2017-08-01 20:30:58 +08:00
sStkNN ( ) { }
2019-01-03 21:26:51 +08:00
sStkNN ( const btDbvtNode * na , const btDbvtNode * nb ) : a ( na ) , b ( nb ) { }
2017-08-01 20:30:58 +08:00
} ;
2019-01-03 21:26:51 +08:00
struct sStkNP
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btDbvtNode * node ;
int mask ;
sStkNP ( const btDbvtNode * n , unsigned m ) : node ( n ) , mask ( m ) { }
2017-08-01 20:30:58 +08:00
} ;
2019-01-03 21:26:51 +08:00
struct sStkNPS
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btDbvtNode * node ;
int mask ;
btScalar value ;
2017-08-01 20:30:58 +08:00
sStkNPS ( ) { }
2019-01-03 21:26:51 +08:00
sStkNPS ( const btDbvtNode * n , unsigned m , btScalar v ) : node ( n ) , mask ( m ) , value ( v ) { }
2017-08-01 20:30:58 +08:00
} ;
2019-01-03 21:26:51 +08:00
struct sStkCLN
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btDbvtNode * node ;
btDbvtNode * parent ;
sStkCLN ( const btDbvtNode * n , btDbvtNode * p ) : node ( n ) , parent ( p ) { }
2017-08-01 20:30:58 +08:00
} ;
2020-01-09 01:05:43 +08:00
struct sStknNN
{
const btDbvntNode * a ;
const btDbvntNode * b ;
sStknNN ( ) { }
sStknNN ( const btDbvntNode * na , const btDbvntNode * nb ) : a ( na ) , b ( nb ) { }
} ;
2017-08-01 20:30:58 +08:00
// Policies/Interfaces
2019-01-03 21:26:51 +08:00
/* ICollide */
struct ICollide
{
2017-08-01 20:30:58 +08:00
DBVT_VIRTUAL_DTOR ( ICollide )
2019-01-03 21:26:51 +08:00
DBVT_VIRTUAL void Process ( const btDbvtNode * , const btDbvtNode * ) { }
DBVT_VIRTUAL void Process ( const btDbvtNode * ) { }
DBVT_VIRTUAL void Process ( const btDbvtNode * n , btScalar ) { Process ( n ) ; }
2020-01-09 01:05:43 +08:00
DBVT_VIRTUAL void Process ( const btDbvntNode * , const btDbvntNode * ) { }
2019-01-03 21:26:51 +08:00
DBVT_VIRTUAL bool Descent ( const btDbvtNode * ) { return ( true ) ; }
DBVT_VIRTUAL bool AllLeaves ( const btDbvtNode * ) { return ( true ) ; }
2017-08-01 20:30:58 +08:00
} ;
2019-01-03 21:26:51 +08:00
/* IWriter */
struct IWriter
2017-08-01 20:30:58 +08:00
{
virtual ~ IWriter ( ) { }
2019-01-03 21:26:51 +08:00
virtual void Prepare ( const btDbvtNode * root , int numnodes ) = 0 ;
virtual void WriteNode ( const btDbvtNode * , int index , int parent , int child0 , int child1 ) = 0 ;
virtual void WriteLeaf ( const btDbvtNode * , int index , int parent ) = 0 ;
2017-08-01 20:30:58 +08:00
} ;
2019-01-03 21:26:51 +08:00
/* IClone */
struct IClone
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
virtual ~ IClone ( ) { }
virtual void CloneLeaf ( btDbvtNode * ) { }
2017-08-01 20:30:58 +08:00
} ;
// Constants
2019-01-03 21:26:51 +08:00
enum
{
SIMPLE_STACKSIZE = 64 ,
DOUBLE_STACKSIZE = SIMPLE_STACKSIZE * 2
2017-08-01 20:30:58 +08:00
} ;
// Fields
2019-01-03 21:26:51 +08:00
btDbvtNode * m_root ;
btDbvtNode * m_free ;
int m_lkhd ;
int m_leaves ;
unsigned m_opath ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
btAlignedObjectArray < sStkNN > m_stkStack ;
2017-08-01 20:30:58 +08:00
// Methods
btDbvt ( ) ;
~ btDbvt ( ) ;
2019-01-03 21:26:51 +08:00
void clear ( ) ;
bool empty ( ) const { return ( 0 = = m_root ) ; }
void optimizeBottomUp ( ) ;
void optimizeTopDown ( int bu_treshold = 128 ) ;
void optimizeIncremental ( int passes ) ;
btDbvtNode * insert ( const btDbvtVolume & box , void * data ) ;
void update ( btDbvtNode * leaf , int lookahead = - 1 ) ;
void update ( btDbvtNode * leaf , btDbvtVolume & volume ) ;
bool update ( btDbvtNode * leaf , btDbvtVolume & volume , const btVector3 & velocity , btScalar margin ) ;
bool update ( btDbvtNode * leaf , btDbvtVolume & volume , const btVector3 & velocity ) ;
bool update ( btDbvtNode * leaf , btDbvtVolume & volume , btScalar margin ) ;
void remove ( btDbvtNode * leaf ) ;
void write ( IWriter * iwriter ) const ;
void clone ( btDbvt & dest , IClone * iclone = 0 ) const ;
static int maxdepth ( const btDbvtNode * node ) ;
static int countLeaves ( const btDbvtNode * node ) ;
static void extractLeaves ( const btDbvtNode * node , btAlignedObjectArray < const btDbvtNode * > & leaves ) ;
2017-08-01 20:30:58 +08:00
# if DBVT_ENABLE_BENCHMARK
2019-01-03 21:26:51 +08:00
static void benchmark ( ) ;
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
static void benchmark ( )
{
}
2017-08-01 20:30:58 +08:00
# endif
// DBVT_IPOLICY must support ICollide policy/interface
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
static void enumNodes ( const btDbvtNode * root ,
DBVT_IPOLICY ) ;
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
static void enumLeaves ( const btDbvtNode * root ,
DBVT_IPOLICY ) ;
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
void collideTT ( const btDbvtNode * root0 ,
const btDbvtNode * root1 ,
DBVT_IPOLICY ) ;
2020-01-09 01:05:43 +08:00
DBVT_PREFIX
void selfCollideT ( const btDbvntNode * root ,
DBVT_IPOLICY ) ;
DBVT_PREFIX
void selfCollideTT ( const btDbvtNode * root ,
DBVT_IPOLICY ) ;
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
void collideTTpersistentStack ( const btDbvtNode * root0 ,
const btDbvtNode * root1 ,
DBVT_IPOLICY ) ;
2017-08-01 20:30:58 +08:00
#if 0
DBVT_PREFIX
void collideTT ( const btDbvtNode * root0 ,
const btDbvtNode * root1 ,
const btTransform & xform ,
DBVT_IPOLICY ) ;
DBVT_PREFIX
void collideTT ( const btDbvtNode * root0 ,
const btTransform & xform0 ,
const btDbvtNode * root1 ,
const btTransform & xform1 ,
DBVT_IPOLICY ) ;
# endif
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
void collideTV ( const btDbvtNode * root ,
const btDbvtVolume & volume ,
DBVT_IPOLICY ) const ;
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
void collideTVNoStackAlloc ( const btDbvtNode * root ,
const btDbvtVolume & volume ,
btNodeStack & stack ,
DBVT_IPOLICY ) const ;
2017-08-01 20:30:58 +08:00
///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
static void rayTest ( const btDbvtNode * root ,
const btVector3 & rayFrom ,
const btVector3 & rayTo ,
DBVT_IPOLICY ) ;
2017-08-01 20:30:58 +08:00
///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
void rayTestInternal ( const btDbvtNode * root ,
const btVector3 & rayFrom ,
const btVector3 & rayTo ,
const btVector3 & rayDirectionInverse ,
unsigned int signs [ 3 ] ,
btScalar lambda_max ,
const btVector3 & aabbMin ,
const btVector3 & aabbMax ,
btAlignedObjectArray < const btDbvtNode * > & stack ,
DBVT_IPOLICY ) const ;
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
static void collideKDOP ( const btDbvtNode * root ,
const btVector3 * normals ,
const btScalar * offsets ,
int count ,
DBVT_IPOLICY ) ;
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
static void collideOCL ( const btDbvtNode * root ,
const btVector3 * normals ,
const btScalar * offsets ,
const btVector3 & sortaxis ,
int count ,
DBVT_IPOLICY ,
bool fullsort = true ) ;
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
static void collideTU ( const btDbvtNode * root ,
DBVT_IPOLICY ) ;
// Helpers
static DBVT_INLINE int nearest ( const int * i , const btDbvt : : sStkNPS * a , btScalar v , int l , int h )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
int m = 0 ;
while ( l < h )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
m = ( l + h ) > > 1 ;
if ( a [ i [ m ] ] . value > = v )
l = m + 1 ;
else
h = m ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
return ( h ) ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
static DBVT_INLINE int allocate ( btAlignedObjectArray < int > & ifree ,
btAlignedObjectArray < sStkNPS > & stock ,
const sStkNPS & value )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
int i ;
if ( ifree . size ( ) > 0 )
{
i = ifree [ ifree . size ( ) - 1 ] ;
ifree . pop_back ( ) ;
stock [ i ] = value ;
}
2017-08-01 20:30:58 +08:00
else
2019-01-03 21:26:51 +08:00
{
i = stock . size ( ) ;
stock . push_back ( value ) ;
}
return ( i ) ;
2017-08-01 20:30:58 +08:00
}
//
private :
2019-01-03 21:26:51 +08:00
btDbvt ( const btDbvt & ) { }
2017-08-01 20:30:58 +08:00
} ;
//
// Inline's
//
//
2019-01-03 21:26:51 +08:00
inline btDbvtAabbMm btDbvtAabbMm : : FromCE ( const btVector3 & c , const btVector3 & e )
2017-08-01 20:30:58 +08:00
{
btDbvtAabbMm box ;
2019-01-03 21:26:51 +08:00
box . mi = c - e ;
box . mx = c + e ;
return ( box ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
inline btDbvtAabbMm btDbvtAabbMm : : FromCR ( const btVector3 & c , btScalar r )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
return ( FromCE ( c , btVector3 ( r , r , r ) ) ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
inline btDbvtAabbMm btDbvtAabbMm : : FromMM ( const btVector3 & mi , const btVector3 & mx )
2017-08-01 20:30:58 +08:00
{
btDbvtAabbMm box ;
2019-01-03 21:26:51 +08:00
box . mi = mi ;
box . mx = mx ;
return ( box ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
inline btDbvtAabbMm btDbvtAabbMm : : FromPoints ( const btVector3 * pts , int n )
2017-08-01 20:30:58 +08:00
{
btDbvtAabbMm box ;
2019-01-03 21:26:51 +08:00
box . mi = box . mx = pts [ 0 ] ;
for ( int i = 1 ; i < n ; + + i )
2017-08-01 20:30:58 +08:00
{
box . mi . setMin ( pts [ i ] ) ;
box . mx . setMax ( pts [ i ] ) ;
}
2019-01-03 21:26:51 +08:00
return ( box ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
inline btDbvtAabbMm btDbvtAabbMm : : FromPoints ( const btVector3 * * ppts , int n )
2017-08-01 20:30:58 +08:00
{
btDbvtAabbMm box ;
2019-01-03 21:26:51 +08:00
box . mi = box . mx = * ppts [ 0 ] ;
for ( int i = 1 ; i < n ; + + i )
2017-08-01 20:30:58 +08:00
{
box . mi . setMin ( * ppts [ i ] ) ;
box . mx . setMax ( * ppts [ i ] ) ;
}
2019-01-03 21:26:51 +08:00
return ( box ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE void btDbvtAabbMm : : Expand ( const btVector3 & e )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
mi - = e ;
mx + = e ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE void btDbvtAabbMm : : SignedExpand ( const btVector3 & e )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( e . x ( ) > 0 )
mx . setX ( mx . x ( ) + e [ 0 ] ) ;
else
mi . setX ( mi . x ( ) + e [ 0 ] ) ;
if ( e . y ( ) > 0 )
mx . setY ( mx . y ( ) + e [ 1 ] ) ;
else
mi . setY ( mi . y ( ) + e [ 1 ] ) ;
if ( e . z ( ) > 0 )
mx . setZ ( mx . z ( ) + e [ 2 ] ) ;
else
mi . setZ ( mi . z ( ) + e [ 2 ] ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE bool btDbvtAabbMm : : Contain ( const btDbvtAabbMm & a ) const
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
return ( ( mi . x ( ) < = a . mi . x ( ) ) & &
( mi . y ( ) < = a . mi . y ( ) ) & &
( mi . z ( ) < = a . mi . z ( ) ) & &
( mx . x ( ) > = a . mx . x ( ) ) & &
( mx . y ( ) > = a . mx . y ( ) ) & &
( mx . z ( ) > = a . mx . z ( ) ) ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE int btDbvtAabbMm : : Classify ( const btVector3 & n , btScalar o , int s ) const
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
btVector3 pi , px ;
switch ( s )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
case ( 0 + 0 + 0 ) :
px = btVector3 ( mi . x ( ) , mi . y ( ) , mi . z ( ) ) ;
pi = btVector3 ( mx . x ( ) , mx . y ( ) , mx . z ( ) ) ;
break ;
case ( 1 + 0 + 0 ) :
px = btVector3 ( mx . x ( ) , mi . y ( ) , mi . z ( ) ) ;
pi = btVector3 ( mi . x ( ) , mx . y ( ) , mx . z ( ) ) ;
break ;
case ( 0 + 2 + 0 ) :
px = btVector3 ( mi . x ( ) , mx . y ( ) , mi . z ( ) ) ;
pi = btVector3 ( mx . x ( ) , mi . y ( ) , mx . z ( ) ) ;
break ;
case ( 1 + 2 + 0 ) :
px = btVector3 ( mx . x ( ) , mx . y ( ) , mi . z ( ) ) ;
pi = btVector3 ( mi . x ( ) , mi . y ( ) , mx . z ( ) ) ;
break ;
case ( 0 + 0 + 4 ) :
px = btVector3 ( mi . x ( ) , mi . y ( ) , mx . z ( ) ) ;
pi = btVector3 ( mx . x ( ) , mx . y ( ) , mi . z ( ) ) ;
break ;
case ( 1 + 0 + 4 ) :
px = btVector3 ( mx . x ( ) , mi . y ( ) , mx . z ( ) ) ;
pi = btVector3 ( mi . x ( ) , mx . y ( ) , mi . z ( ) ) ;
break ;
case ( 0 + 2 + 4 ) :
px = btVector3 ( mi . x ( ) , mx . y ( ) , mx . z ( ) ) ;
pi = btVector3 ( mx . x ( ) , mi . y ( ) , mi . z ( ) ) ;
break ;
case ( 1 + 2 + 4 ) :
px = btVector3 ( mx . x ( ) , mx . y ( ) , mx . z ( ) ) ;
pi = btVector3 ( mi . x ( ) , mi . y ( ) , mi . z ( ) ) ;
break ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
if ( ( btDot ( n , px ) + o ) < 0 ) return ( - 1 ) ;
if ( ( btDot ( n , pi ) + o ) > = 0 ) return ( + 1 ) ;
return ( 0 ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE btScalar btDbvtAabbMm : : ProjectMinimum ( const btVector3 & v , unsigned signs ) const
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btVector3 * b [ ] = { & mx , & mi } ;
const btVector3 p ( b [ ( signs > > 0 ) & 1 ] - > x ( ) ,
b [ ( signs > > 1 ) & 1 ] - > y ( ) ,
b [ ( signs > > 2 ) & 1 ] - > z ( ) ) ;
return ( btDot ( p , v ) ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE void btDbvtAabbMm : : AddSpan ( const btVector3 & d , btScalar & smi , btScalar & smx ) const
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
for ( int i = 0 ; i < 3 ; + + i )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( d [ i ] < 0 )
{
smi + = mx [ i ] * d [ i ] ;
smx + = mi [ i ] * d [ i ] ;
}
2017-08-01 20:30:58 +08:00
else
2019-01-03 21:26:51 +08:00
{
smi + = mi [ i ] * d [ i ] ;
smx + = mx [ i ] * d [ i ] ;
}
2017-08-01 20:30:58 +08:00
}
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE bool Intersect ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
# if DBVT_INT0_IMPL == DBVT_IMPL_SSE
const __m128 rt ( _mm_or_ps ( _mm_cmplt_ps ( _mm_load_ps ( b . mx ) , _mm_load_ps ( a . mi ) ) ,
_mm_cmplt_ps ( _mm_load_ps ( a . mx ) , _mm_load_ps ( b . mi ) ) ) ) ;
# if defined(_WIN32)
const __int32 * pu ( ( const __int32 * ) & rt ) ;
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
const int * pu ( ( const int * ) & rt ) ;
2017-08-01 20:30:58 +08:00
# endif
2019-01-03 21:26:51 +08:00
return ( ( pu [ 0 ] | pu [ 1 ] | pu [ 2 ] ) = = 0 ) ;
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
return ( ( a . mi . x ( ) < = b . mx . x ( ) ) & &
( a . mx . x ( ) > = b . mi . x ( ) ) & &
( a . mi . y ( ) < = b . mx . y ( ) ) & &
( a . mx . y ( ) > = b . mi . y ( ) ) & &
( a . mi . z ( ) < = b . mx . z ( ) ) & &
( a . mx . z ( ) > = b . mi . z ( ) ) ) ;
2017-08-01 20:30:58 +08:00
# endif
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE bool Intersect ( const btDbvtAabbMm & a ,
const btVector3 & b )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
return ( ( b . x ( ) > = a . mi . x ( ) ) & &
( b . y ( ) > = a . mi . y ( ) ) & &
( b . z ( ) > = a . mi . z ( ) ) & &
( b . x ( ) < = a . mx . x ( ) ) & &
( b . y ( ) < = a . mx . y ( ) ) & &
( b . z ( ) < = a . mx . z ( ) ) ) ;
2017-08-01 20:30:58 +08:00
}
//////////////////////////////////////
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE btScalar Proximity ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btVector3 d = ( a . mi + a . mx ) - ( b . mi + b . mx ) ;
return ( btFabs ( d . x ( ) ) + btFabs ( d . y ( ) ) + btFabs ( d . z ( ) ) ) ;
2017-08-01 20:30:58 +08:00
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE int Select ( const btDbvtAabbMm & o ,
const btDbvtAabbMm & a ,
const btDbvtAabbMm & b )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
# if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
# if defined(_WIN32)
static ATTRIBUTE_ALIGNED16 ( const unsigned __int32 ) mask [ ] = { 0x7fffffff , 0x7fffffff , 0x7fffffff , 0x7fffffff } ;
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
static ATTRIBUTE_ALIGNED16 ( const unsigned int ) mask [ ] = { 0x7fffffff , 0x7fffffff , 0x7fffffff , 0x00000000 /*0x7fffffff*/ } ;
2017-08-01 20:30:58 +08:00
# endif
///@todo: the intrinsic version is 11% slower
# if DBVT_USE_INTRINSIC_SSE
2019-01-03 21:26:51 +08:00
union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
__m128 ssereg ;
float floats [ 4 ] ;
int ints [ 4 ] ;
2017-08-01 20:30:58 +08:00
} ;
2019-01-03 21:26:51 +08:00
__m128 omi ( _mm_load_ps ( o . mi ) ) ;
omi = _mm_add_ps ( omi , _mm_load_ps ( o . mx ) ) ;
__m128 ami ( _mm_load_ps ( a . mi ) ) ;
ami = _mm_add_ps ( ami , _mm_load_ps ( a . mx ) ) ;
ami = _mm_sub_ps ( ami , omi ) ;
ami = _mm_and_ps ( ami , _mm_load_ps ( ( const float * ) mask ) ) ;
__m128 bmi ( _mm_load_ps ( b . mi ) ) ;
bmi = _mm_add_ps ( bmi , _mm_load_ps ( b . mx ) ) ;
bmi = _mm_sub_ps ( bmi , omi ) ;
bmi = _mm_and_ps ( bmi , _mm_load_ps ( ( const float * ) mask ) ) ;
__m128 t0 ( _mm_movehl_ps ( ami , ami ) ) ;
ami = _mm_add_ps ( ami , t0 ) ;
ami = _mm_add_ss ( ami , _mm_shuffle_ps ( ami , ami , 1 ) ) ;
__m128 t1 ( _mm_movehl_ps ( bmi , bmi ) ) ;
bmi = _mm_add_ps ( bmi , t1 ) ;
bmi = _mm_add_ss ( bmi , _mm_shuffle_ps ( bmi , bmi , 1 ) ) ;
2017-08-01 20:30:58 +08:00
btSSEUnion tmp ;
2019-01-03 21:26:51 +08:00
tmp . ssereg = _mm_cmple_ss ( bmi , ami ) ;
return tmp . ints [ 0 ] & 1 ;
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
ATTRIBUTE_ALIGNED16 ( __int32 r [ 1 ] ) ;
2017-08-01 20:30:58 +08:00
__asm
{
mov eax , o
mov ecx , a
mov edx , b
movaps xmm0 , [ eax ]
movaps xmm5 , mask
addps xmm0 , [ eax + 16 ]
movaps xmm1 , [ ecx ]
movaps xmm2 , [ edx ]
addps xmm1 , [ ecx + 16 ]
addps xmm2 , [ edx + 16 ]
subps xmm1 , xmm0
subps xmm2 , xmm0
andps xmm1 , xmm5
andps xmm2 , xmm5
movhlps xmm3 , xmm1
movhlps xmm4 , xmm2
addps xmm1 , xmm3
addps xmm2 , xmm4
pshufd xmm3 , xmm1 , 1
pshufd xmm4 , xmm2 , 1
addss xmm1 , xmm3
addss xmm2 , xmm4
cmpless xmm2 , xmm1
movss r , xmm2
}
2019-01-03 21:26:51 +08:00
return ( r [ 0 ] & 1 ) ;
2017-08-01 20:30:58 +08:00
# endif
# else
2019-01-03 21:26:51 +08:00
return ( Proximity ( o , a ) < Proximity ( o , b ) ? 0 : 1 ) ;
2017-08-01 20:30:58 +08:00
# endif
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE void Merge ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b ,
btDbvtAabbMm & r )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
# if DBVT_MERGE_IMPL == DBVT_IMPL_SSE
__m128 ami ( _mm_load_ps ( a . mi ) ) ;
__m128 amx ( _mm_load_ps ( a . mx ) ) ;
__m128 bmi ( _mm_load_ps ( b . mi ) ) ;
__m128 bmx ( _mm_load_ps ( b . mx ) ) ;
ami = _mm_min_ps ( ami , bmi ) ;
amx = _mm_max_ps ( amx , bmx ) ;
_mm_store_ps ( r . mi , ami ) ;
_mm_store_ps ( r . mx , amx ) ;
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
for ( int i = 0 ; i < 3 ; + + i )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( a . mi [ i ] < b . mi [ i ] )
r . mi [ i ] = a . mi [ i ] ;
else
r . mi [ i ] = b . mi [ i ] ;
if ( a . mx [ i ] > b . mx [ i ] )
r . mx [ i ] = a . mx [ i ] ;
else
r . mx [ i ] = b . mx [ i ] ;
2017-08-01 20:30:58 +08:00
}
# endif
}
//
2019-01-03 21:26:51 +08:00
DBVT_INLINE bool NotEqual ( const btDbvtAabbMm & a ,
const btDbvtAabbMm & b )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
return ( ( a . mi . x ( ) ! = b . mi . x ( ) ) | |
( a . mi . y ( ) ! = b . mi . y ( ) ) | |
( a . mi . z ( ) ! = b . mi . z ( ) ) | |
( a . mx . x ( ) ! = b . mx . x ( ) ) | |
( a . mx . y ( ) ! = b . mx . y ( ) ) | |
( a . mx . z ( ) ! = b . mx . z ( ) ) ) ;
2017-08-01 20:30:58 +08:00
}
//
// Inline's
//
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : enumNodes ( const btDbvtNode * root ,
DBVT_IPOLICY )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
policy . Process ( root ) ;
if ( root - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
enumNodes ( root - > childs [ 0 ] , policy ) ;
enumNodes ( root - > childs [ 1 ] , policy ) ;
2017-08-01 20:30:58 +08:00
}
}
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : enumLeaves ( const btDbvtNode * root ,
DBVT_IPOLICY )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root - > isinternal ( ) )
{
enumLeaves ( root - > childs [ 0 ] , policy ) ;
enumLeaves ( root - > childs [ 1 ] , policy ) ;
}
else
{
policy . Process ( root ) ;
}
2017-08-01 20:30:58 +08:00
}
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : collideTT ( const btDbvtNode * root0 ,
const btDbvtNode * root1 ,
DBVT_IPOLICY )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root0 & & root1 )
{
int depth = 1 ;
int treshold = DOUBLE_STACKSIZE - 4 ;
btAlignedObjectArray < sStkNN > stkStack ;
stkStack . resize ( DOUBLE_STACKSIZE ) ;
stkStack [ 0 ] = sStkNN ( root0 , root1 ) ;
do
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
sStkNN p = stkStack [ - - depth ] ;
if ( depth > treshold )
{
stkStack . resize ( stkStack . size ( ) * 2 ) ;
treshold = stkStack . size ( ) - 4 ;
}
if ( p . a = = p . b )
{
if ( p . a - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . a - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . a - > childs [ 1 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . a - > childs [ 1 ] ) ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
}
else if ( Intersect ( p . a - > volume , p . b - > volume ) )
{
if ( p . a - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( p . b - > isinternal ( ) )
{
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 1 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 1 ] ) ;
}
else
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b ) ;
2017-08-01 20:30:58 +08:00
}
}
2019-01-03 21:26:51 +08:00
else
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( p . b - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 1 ] ) ;
2017-08-01 20:30:58 +08:00
}
else
{
2019-01-03 21:26:51 +08:00
policy . Process ( p . a , p . b ) ;
2017-08-01 20:30:58 +08:00
}
}
2019-01-03 21:26:51 +08:00
}
} while ( depth ) ;
}
2017-08-01 20:30:58 +08:00
}
2020-01-09 01:05:43 +08:00
//
DBVT_PREFIX
inline void btDbvt : : selfCollideT ( const btDbvntNode * root ,
DBVT_IPOLICY )
{
DBVT_CHECKTYPE
if ( root )
{
int depth = 1 ;
int treshold = DOUBLE_STACKSIZE - 4 ;
btAlignedObjectArray < sStknNN > stkStack ;
stkStack . resize ( DOUBLE_STACKSIZE ) ;
stkStack [ 0 ] = sStknNN ( root , root ) ;
do
{
sStknNN p = stkStack [ - - depth ] ;
if ( depth > treshold )
{
stkStack . resize ( stkStack . size ( ) * 2 ) ;
treshold = stkStack . size ( ) - 4 ;
}
if ( p . a = = p . b )
{
if ( p . a - > isinternal ( ) & & p . a - > angle > SIMD_PI )
{
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 0 ] , p . a - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 1 ] , p . a - > childs [ 1 ] ) ;
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 0 ] , p . a - > childs [ 1 ] ) ;
}
}
else if ( Intersect ( p . a - > volume , p . b - > volume ) )
{
if ( p . a - > isinternal ( ) )
{
if ( p . b - > isinternal ( ) )
{
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 0 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 1 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 0 ] , p . b - > childs [ 1 ] ) ;
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 1 ] , p . b - > childs [ 1 ] ) ;
}
else
{
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 0 ] , p . b ) ;
stkStack [ depth + + ] = sStknNN ( p . a - > childs [ 1 ] , p . b ) ;
}
}
else
{
if ( p . b - > isinternal ( ) )
{
stkStack [ depth + + ] = sStknNN ( p . a , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStknNN ( p . a , p . b - > childs [ 1 ] ) ;
}
else
{
policy . Process ( p . a , p . b ) ;
}
}
}
} while ( depth ) ;
}
}
//
DBVT_PREFIX
inline void btDbvt : : selfCollideTT ( const btDbvtNode * root ,
DBVT_IPOLICY )
{
DBVT_CHECKTYPE
if ( root )
{
int depth = 1 ;
int treshold = DOUBLE_STACKSIZE - 4 ;
btAlignedObjectArray < sStkNN > stkStack ;
stkStack . resize ( DOUBLE_STACKSIZE ) ;
stkStack [ 0 ] = sStkNN ( root , root ) ;
do
{
sStkNN p = stkStack [ - - depth ] ;
if ( depth > treshold )
{
stkStack . resize ( stkStack . size ( ) * 2 ) ;
treshold = stkStack . size ( ) - 4 ;
}
if ( p . a = = p . b )
{
if ( p . a - > isinternal ( ) )
{
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . a - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . a - > childs [ 1 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . a - > childs [ 1 ] ) ;
}
}
else if ( Intersect ( p . a - > volume , p . b - > volume ) )
{
if ( p . a - > isinternal ( ) )
{
if ( p . b - > isinternal ( ) )
{
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 1 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 1 ] ) ;
}
else
{
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b ) ;
}
}
else
{
if ( p . b - > isinternal ( ) )
{
stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 1 ] ) ;
}
else
{
policy . Process ( p . a , p . b ) ;
}
}
}
} while ( depth ) ;
}
}
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : collideTTpersistentStack ( const btDbvtNode * root0 ,
const btDbvtNode * root1 ,
DBVT_IPOLICY )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root0 & & root1 )
{
int depth = 1 ;
int treshold = DOUBLE_STACKSIZE - 4 ;
m_stkStack . resize ( DOUBLE_STACKSIZE ) ;
m_stkStack [ 0 ] = sStkNN ( root0 , root1 ) ;
do
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
sStkNN p = m_stkStack [ - - depth ] ;
if ( depth > treshold )
{
m_stkStack . resize ( m_stkStack . size ( ) * 2 ) ;
treshold = m_stkStack . size ( ) - 4 ;
}
if ( p . a = = p . b )
{
if ( p . a - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . a - > childs [ 0 ] ) ;
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . a - > childs [ 1 ] ) ;
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . a - > childs [ 1 ] ) ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
}
else if ( Intersect ( p . a - > volume , p . b - > volume ) )
{
if ( p . a - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( p . b - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 0 ] ) ;
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 0 ] ) ;
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 1 ] ) ;
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 1 ] ) ;
}
else
{
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b ) ;
m_stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b ) ;
2017-08-01 20:30:58 +08:00
}
}
2019-01-03 21:26:51 +08:00
else
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( p . b - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
m_stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 0 ] ) ;
m_stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 1 ] ) ;
2017-08-01 20:30:58 +08:00
}
else
{
2019-01-03 21:26:51 +08:00
policy . Process ( p . a , p . b ) ;
2017-08-01 20:30:58 +08:00
}
}
2019-01-03 21:26:51 +08:00
}
} while ( depth ) ;
}
2017-08-01 20:30:58 +08:00
}
#if 0
//
DBVT_PREFIX
inline void btDbvt : : collideTT ( const btDbvtNode * root0 ,
const btDbvtNode * root1 ,
const btTransform & xform ,
DBVT_IPOLICY )
{
DBVT_CHECKTYPE
if ( root0 & & root1 )
{
int depth = 1 ;
int treshold = DOUBLE_STACKSIZE - 4 ;
btAlignedObjectArray < sStkNN > stkStack ;
stkStack . resize ( DOUBLE_STACKSIZE ) ;
stkStack [ 0 ] = sStkNN ( root0 , root1 ) ;
do {
sStkNN p = stkStack [ - - depth ] ;
if ( Intersect ( p . a - > volume , p . b - > volume , xform ) )
{
if ( depth > treshold )
{
stkStack . resize ( stkStack . size ( ) * 2 ) ;
treshold = stkStack . size ( ) - 4 ;
}
if ( p . a - > isinternal ( ) )
{
if ( p . b - > isinternal ( ) )
{
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b - > childs [ 1 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b - > childs [ 1 ] ) ;
}
else
{
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 0 ] , p . b ) ;
stkStack [ depth + + ] = sStkNN ( p . a - > childs [ 1 ] , p . b ) ;
}
}
else
{
if ( p . b - > isinternal ( ) )
{
stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 0 ] ) ;
stkStack [ depth + + ] = sStkNN ( p . a , p . b - > childs [ 1 ] ) ;
}
else
{
policy . Process ( p . a , p . b ) ;
}
}
}
} while ( depth ) ;
}
}
//
DBVT_PREFIX
inline void btDbvt : : collideTT ( const btDbvtNode * root0 ,
const btTransform & xform0 ,
const btDbvtNode * root1 ,
const btTransform & xform1 ,
DBVT_IPOLICY )
{
const btTransform xform = xform0 . inverse ( ) * xform1 ;
collideTT ( root0 , root1 , xform , policy ) ;
}
2019-01-03 21:26:51 +08:00
# endif
2017-08-01 20:30:58 +08:00
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : collideTV ( const btDbvtNode * root ,
const btDbvtVolume & vol ,
DBVT_IPOLICY ) const
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
ATTRIBUTE_ALIGNED16 ( btDbvtVolume )
volume ( vol ) ;
btAlignedObjectArray < const btDbvtNode * > stack ;
2017-08-01 20:30:58 +08:00
stack . resize ( 0 ) ;
# ifndef BT_DISABLE_STACK_TEMP_MEMORY
2019-01-03 21:26:51 +08:00
char tempmemory [ SIMPLE_STACKSIZE * sizeof ( const btDbvtNode * ) ] ;
2017-08-01 20:30:58 +08:00
stack . initializeFromBuffer ( tempmemory , 0 , SIMPLE_STACKSIZE ) ;
# else
stack . reserve ( SIMPLE_STACKSIZE ) ;
2019-01-03 21:26:51 +08:00
# endif //BT_DISABLE_STACK_TEMP_MEMORY
2017-08-01 20:30:58 +08:00
stack . push_back ( root ) ;
2019-01-03 21:26:51 +08:00
do
{
const btDbvtNode * n = stack [ stack . size ( ) - 1 ] ;
2017-08-01 20:30:58 +08:00
stack . pop_back ( ) ;
2019-01-03 21:26:51 +08:00
if ( Intersect ( n - > volume , volume ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( n - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
stack . push_back ( n - > childs [ 0 ] ) ;
stack . push_back ( n - > childs [ 1 ] ) ;
}
else
{
policy . Process ( n ) ;
}
}
2019-01-03 21:26:51 +08:00
} while ( stack . size ( ) > 0 ) ;
2017-08-01 20:30:58 +08:00
}
}
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : collideTVNoStackAlloc ( const btDbvtNode * root ,
const btDbvtVolume & vol ,
btNodeStack & stack ,
DBVT_IPOLICY ) const
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
ATTRIBUTE_ALIGNED16 ( btDbvtVolume )
volume ( vol ) ;
2017-08-01 20:30:58 +08:00
stack . resize ( 0 ) ;
stack . reserve ( SIMPLE_STACKSIZE ) ;
stack . push_back ( root ) ;
2019-01-03 21:26:51 +08:00
do
{
const btDbvtNode * n = stack [ stack . size ( ) - 1 ] ;
2017-08-01 20:30:58 +08:00
stack . pop_back ( ) ;
2019-01-03 21:26:51 +08:00
if ( Intersect ( n - > volume , volume ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( n - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
stack . push_back ( n - > childs [ 0 ] ) ;
stack . push_back ( n - > childs [ 1 ] ) ;
}
else
{
policy . Process ( n ) ;
}
}
2019-01-03 21:26:51 +08:00
} while ( stack . size ( ) > 0 ) ;
2017-08-01 20:30:58 +08:00
}
}
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : rayTestInternal ( const btDbvtNode * root ,
const btVector3 & rayFrom ,
const btVector3 & rayTo ,
const btVector3 & rayDirectionInverse ,
unsigned int signs [ 3 ] ,
btScalar lambda_max ,
const btVector3 & aabbMin ,
const btVector3 & aabbMax ,
btAlignedObjectArray < const btDbvtNode * > & stack ,
DBVT_IPOLICY ) const
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
( void ) rayTo ;
2017-08-01 20:30:58 +08:00
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root )
2017-08-01 20:30:58 +08:00
{
btVector3 resultNormal ;
2019-01-03 21:26:51 +08:00
int depth = 1 ;
int treshold = DOUBLE_STACKSIZE - 2 ;
2017-08-01 20:30:58 +08:00
stack . resize ( DOUBLE_STACKSIZE ) ;
2019-01-03 21:26:51 +08:00
stack [ 0 ] = root ;
2017-08-01 20:30:58 +08:00
btVector3 bounds [ 2 ] ;
2019-01-03 21:26:51 +08:00
do
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btDbvtNode * node = stack [ - - depth ] ;
bounds [ 0 ] = node - > volume . Mins ( ) - aabbMax ;
bounds [ 1 ] = node - > volume . Maxs ( ) - aabbMin ;
btScalar tmin = 1.f , lambda_min = 0.f ;
unsigned int result1 = false ;
result1 = btRayAabb2 ( rayFrom , rayDirectionInverse , signs , bounds , tmin , lambda_min , lambda_max ) ;
if ( result1 )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( node - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( depth > treshold )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stack . resize ( stack . size ( ) * 2 ) ;
treshold = stack . size ( ) - 2 ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
stack [ depth + + ] = node - > childs [ 0 ] ;
stack [ depth + + ] = node - > childs [ 1 ] ;
2017-08-01 20:30:58 +08:00
}
else
{
policy . Process ( node ) ;
}
}
2019-01-03 21:26:51 +08:00
} while ( depth ) ;
2017-08-01 20:30:58 +08:00
}
}
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : rayTest ( const btDbvtNode * root ,
const btVector3 & rayFrom ,
const btVector3 & rayTo ,
DBVT_IPOLICY )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root )
{
btVector3 rayDir = ( rayTo - rayFrom ) ;
rayDir . normalize ( ) ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
btVector3 rayDirectionInverse ;
rayDirectionInverse [ 0 ] = rayDir [ 0 ] = = btScalar ( 0.0 ) ? btScalar ( BT_LARGE_FLOAT ) : btScalar ( 1.0 ) / rayDir [ 0 ] ;
rayDirectionInverse [ 1 ] = rayDir [ 1 ] = = btScalar ( 0.0 ) ? btScalar ( BT_LARGE_FLOAT ) : btScalar ( 1.0 ) / rayDir [ 1 ] ;
rayDirectionInverse [ 2 ] = rayDir [ 2 ] = = btScalar ( 0.0 ) ? btScalar ( BT_LARGE_FLOAT ) : btScalar ( 1.0 ) / rayDir [ 2 ] ;
unsigned int signs [ 3 ] = { rayDirectionInverse [ 0 ] < 0.0 , rayDirectionInverse [ 1 ] < 0.0 , rayDirectionInverse [ 2 ] < 0.0 } ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
btScalar lambda_max = rayDir . dot ( rayTo - rayFrom ) ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
btVector3 resultNormal ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
btAlignedObjectArray < const btDbvtNode * > stack ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
int depth = 1 ;
int treshold = DOUBLE_STACKSIZE - 2 ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
char tempmemory [ DOUBLE_STACKSIZE * sizeof ( const btDbvtNode * ) ] ;
2017-08-01 20:30:58 +08:00
# ifndef BT_DISABLE_STACK_TEMP_MEMORY
2019-01-03 21:26:51 +08:00
stack . initializeFromBuffer ( tempmemory , DOUBLE_STACKSIZE , DOUBLE_STACKSIZE ) ;
# else //BT_DISABLE_STACK_TEMP_MEMORY
stack . resize ( DOUBLE_STACKSIZE ) ;
# endif //BT_DISABLE_STACK_TEMP_MEMORY
stack [ 0 ] = root ;
btVector3 bounds [ 2 ] ;
do
{
const btDbvtNode * node = stack [ - - depth ] ;
bounds [ 0 ] = node - > volume . Mins ( ) ;
bounds [ 1 ] = node - > volume . Maxs ( ) ;
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
btScalar tmin = 1.f , lambda_min = 0.f ;
unsigned int result1 = btRayAabb2 ( rayFrom , rayDirectionInverse , signs , bounds , tmin , lambda_min , lambda_max ) ;
2017-08-01 20:30:58 +08:00
# ifdef COMPARE_BTRAY_AABB2
2019-01-03 21:26:51 +08:00
btScalar param = 1.f ;
bool result2 = btRayAabb ( rayFrom , rayTo , node - > volume . Mins ( ) , node - > volume . Maxs ( ) , param , resultNormal ) ;
btAssert ( result1 = = result2 ) ;
# endif //TEST_BTRAY_AABB2
2017-08-01 20:30:58 +08:00
2019-01-03 21:26:51 +08:00
if ( result1 )
{
if ( node - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( depth > treshold )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stack . resize ( stack . size ( ) * 2 ) ;
treshold = stack . size ( ) - 2 ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
stack [ depth + + ] = node - > childs [ 0 ] ;
stack [ depth + + ] = node - > childs [ 1 ] ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
else
{
policy . Process ( node ) ;
}
}
} while ( depth ) ;
}
2017-08-01 20:30:58 +08:00
}
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : collideKDOP ( const btDbvtNode * root ,
const btVector3 * normals ,
const btScalar * offsets ,
int count ,
DBVT_IPOLICY )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root )
{
const int inside = ( 1 < < count ) - 1 ;
btAlignedObjectArray < sStkNP > stack ;
int signs [ sizeof ( unsigned ) * 8 ] ;
btAssert ( count < int ( sizeof ( signs ) / sizeof ( signs [ 0 ] ) ) ) ;
for ( int i = 0 ; i < count ; + + i )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
signs [ i ] = ( ( normals [ i ] . x ( ) > = 0 ) ? 1 : 0 ) +
( ( normals [ i ] . y ( ) > = 0 ) ? 2 : 0 ) +
( ( normals [ i ] . z ( ) > = 0 ) ? 4 : 0 ) ;
}
stack . reserve ( SIMPLE_STACKSIZE ) ;
stack . push_back ( sStkNP ( root , 0 ) ) ;
do
{
sStkNP se = stack [ stack . size ( ) - 1 ] ;
bool out = false ;
stack . pop_back ( ) ;
for ( int i = 0 , j = 1 ; ( ! out ) & & ( i < count ) ; + + i , j < < = 1 )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( 0 = = ( se . mask & j ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const int side = se . node - > volume . Classify ( normals [ i ] , offsets [ i ] , signs [ i ] ) ;
switch ( side )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
case - 1 :
out = true ;
break ;
case + 1 :
se . mask | = j ;
break ;
2017-08-01 20:30:58 +08:00
}
}
2019-01-03 21:26:51 +08:00
}
if ( ! out )
{
if ( ( se . mask ! = inside ) & & ( se . node - > isinternal ( ) ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stack . push_back ( sStkNP ( se . node - > childs [ 0 ] , se . mask ) ) ;
stack . push_back ( sStkNP ( se . node - > childs [ 1 ] , se . mask ) ) ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
else
{
if ( policy . AllLeaves ( se . node ) ) enumLeaves ( se . node , policy ) ;
}
}
} while ( stack . size ( ) ) ;
}
2017-08-01 20:30:58 +08:00
}
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : collideOCL ( const btDbvtNode * root ,
const btVector3 * normals ,
const btScalar * offsets ,
const btVector3 & sortaxis ,
int count ,
DBVT_IPOLICY ,
bool fsort )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root )
{
const unsigned srtsgns = ( sortaxis [ 0 ] > = 0 ? 1 : 0 ) +
( sortaxis [ 1 ] > = 0 ? 2 : 0 ) +
( sortaxis [ 2 ] > = 0 ? 4 : 0 ) ;
const int inside = ( 1 < < count ) - 1 ;
btAlignedObjectArray < sStkNPS > stock ;
btAlignedObjectArray < int > ifree ;
btAlignedObjectArray < int > stack ;
int signs [ sizeof ( unsigned ) * 8 ] ;
btAssert ( count < int ( sizeof ( signs ) / sizeof ( signs [ 0 ] ) ) ) ;
for ( int i = 0 ; i < count ; + + i )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
signs [ i ] = ( ( normals [ i ] . x ( ) > = 0 ) ? 1 : 0 ) +
( ( normals [ i ] . y ( ) > = 0 ) ? 2 : 0 ) +
( ( normals [ i ] . z ( ) > = 0 ) ? 4 : 0 ) ;
}
stock . reserve ( SIMPLE_STACKSIZE ) ;
stack . reserve ( SIMPLE_STACKSIZE ) ;
ifree . reserve ( SIMPLE_STACKSIZE ) ;
stack . push_back ( allocate ( ifree , stock , sStkNPS ( root , 0 , root - > volume . ProjectMinimum ( sortaxis , srtsgns ) ) ) ) ;
do
{
const int id = stack [ stack . size ( ) - 1 ] ;
sStkNPS se = stock [ id ] ;
stack . pop_back ( ) ;
ifree . push_back ( id ) ;
if ( se . mask ! = inside )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
bool out = false ;
for ( int i = 0 , j = 1 ; ( ! out ) & & ( i < count ) ; + + i , j < < = 1 )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
if ( 0 = = ( se . mask & j ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const int side = se . node - > volume . Classify ( normals [ i ] , offsets [ i ] , signs [ i ] ) ;
switch ( side )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
case - 1 :
out = true ;
break ;
case + 1 :
se . mask | = j ;
break ;
2017-08-01 20:30:58 +08:00
}
}
}
2019-01-03 21:26:51 +08:00
if ( out ) continue ;
}
if ( policy . Descent ( se . node ) )
{
if ( se . node - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btDbvtNode * pns [ ] = { se . node - > childs [ 0 ] , se . node - > childs [ 1 ] } ;
sStkNPS nes [ ] = { sStkNPS ( pns [ 0 ] , se . mask , pns [ 0 ] - > volume . ProjectMinimum ( sortaxis , srtsgns ) ) ,
sStkNPS ( pns [ 1 ] , se . mask , pns [ 1 ] - > volume . ProjectMinimum ( sortaxis , srtsgns ) ) } ;
const int q = nes [ 0 ] . value < nes [ 1 ] . value ? 1 : 0 ;
int j = stack . size ( ) ;
if ( fsort & & ( j > 0 ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
/* Insert 0 */
j = nearest ( & stack [ 0 ] , & stock [ 0 ] , nes [ q ] . value , 0 , stack . size ( ) ) ;
stack . push_back ( 0 ) ;
//void * memmove ( void * destination, const void * source, size_t num );
2017-08-01 20:30:58 +08:00
# if DBVT_USE_MEMMOVE
2019-01-03 21:26:51 +08:00
{
int num_items_to_move = stack . size ( ) - 1 - j ;
if ( num_items_to_move > 0 )
memmove ( & stack [ j + 1 ] , & stack [ j ] , sizeof ( int ) * num_items_to_move ) ;
}
2017-08-01 20:30:58 +08:00
# else
2019-01-03 21:26:51 +08:00
for ( int k = stack . size ( ) - 1 ; k > j ; - - k )
{
stack [ k ] = stack [ k - 1 ] ;
}
2017-08-01 20:30:58 +08:00
# endif
2019-01-03 21:26:51 +08:00
stack [ j ] = allocate ( ifree , stock , nes [ q ] ) ;
/* Insert 1 */
j = nearest ( & stack [ 0 ] , & stock [ 0 ] , nes [ 1 - q ] . value , j , stack . size ( ) ) ;
stack . push_back ( 0 ) ;
2017-08-01 20:30:58 +08:00
# if DBVT_USE_MEMMOVE
2019-01-03 21:26:51 +08:00
{
int num_items_to_move = stack . size ( ) - 1 - j ;
if ( num_items_to_move > 0 )
memmove ( & stack [ j + 1 ] , & stack [ j ] , sizeof ( int ) * num_items_to_move ) ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
# else
for ( int k = stack . size ( ) - 1 ; k > j ; - - k )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stack [ k ] = stack [ k - 1 ] ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
# endif
stack [ j ] = allocate ( ifree , stock , nes [ 1 - q ] ) ;
2017-08-01 20:30:58 +08:00
}
else
{
2019-01-03 21:26:51 +08:00
stack . push_back ( allocate ( ifree , stock , nes [ q ] ) ) ;
stack . push_back ( allocate ( ifree , stock , nes [ 1 - q ] ) ) ;
2017-08-01 20:30:58 +08:00
}
}
2019-01-03 21:26:51 +08:00
else
{
policy . Process ( se . node , se . value ) ;
}
}
} while ( stack . size ( ) ) ;
}
2017-08-01 20:30:58 +08:00
}
//
DBVT_PREFIX
2019-01-03 21:26:51 +08:00
inline void btDbvt : : collideTU ( const btDbvtNode * root ,
DBVT_IPOLICY )
2017-08-01 20:30:58 +08:00
{
DBVT_CHECKTYPE
2019-01-03 21:26:51 +08:00
if ( root )
{
btAlignedObjectArray < const btDbvtNode * > stack ;
stack . reserve ( SIMPLE_STACKSIZE ) ;
stack . push_back ( root ) ;
do
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
const btDbvtNode * n = stack [ stack . size ( ) - 1 ] ;
stack . pop_back ( ) ;
if ( policy . Descent ( n ) )
{
if ( n - > isinternal ( ) )
2017-08-01 20:30:58 +08:00
{
2019-01-03 21:26:51 +08:00
stack . push_back ( n - > childs [ 0 ] ) ;
stack . push_back ( n - > childs [ 1 ] ) ;
2017-08-01 20:30:58 +08:00
}
2019-01-03 21:26:51 +08:00
else
{
policy . Process ( n ) ;
}
}
} while ( stack . size ( ) > 0 ) ;
}
2017-08-01 20:30:58 +08:00
}
//
// PP Cleanup
//
# undef DBVT_USE_MEMMOVE
# undef DBVT_USE_TEMPLATE
# undef DBVT_VIRTUAL_DTOR
# undef DBVT_VIRTUAL
# undef DBVT_PREFIX
# undef DBVT_IPOLICY
# undef DBVT_CHECKTYPE
# undef DBVT_IMPL_GENERIC
# undef DBVT_IMPL_SSE
# undef DBVT_USE_INTRINSIC_SSE
# undef DBVT_SELECT_IMPL
# undef DBVT_MERGE_IMPL
# undef DBVT_INT0_IMPL
# endif