1578 lines
40 KiB
C++
1578 lines
40 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2007 Erwin Coumans https://bulletphysics.org
|
|
|
|
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
|
|
#define DBVT_IMPL_GENERIC 0 // Generic implementation
|
|
#define DBVT_IMPL_SSE 1 // SSE
|
|
|
|
// Template implementation of ICollide
|
|
#ifdef _WIN32
|
|
#if (defined(_MSC_VER) && _MSC_VER >= 1400)
|
|
#define DBVT_USE_TEMPLATE 1
|
|
#else
|
|
#define DBVT_USE_TEMPLATE 0
|
|
#endif
|
|
#else
|
|
#define DBVT_USE_TEMPLATE 0
|
|
#endif
|
|
|
|
// Use only intrinsics instead of inline asm
|
|
#define DBVT_USE_INTRINSIC_SSE 1
|
|
|
|
// Using memmov for collideOCL
|
|
#define DBVT_USE_MEMMOVE 1
|
|
|
|
// Enable benchmarking code
|
|
#define DBVT_ENABLE_BENCHMARK 0
|
|
|
|
// Inlining
|
|
#define DBVT_INLINE SIMD_FORCE_INLINE
|
|
|
|
// Specific methods implementation
|
|
|
|
//SSE gives errors on a MSVC 7.1
|
|
#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
|
|
#else
|
|
#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC
|
|
#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC
|
|
#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC
|
|
#endif
|
|
|
|
#if (DBVT_SELECT_IMPL == DBVT_IMPL_SSE) || \
|
|
(DBVT_MERGE_IMPL == DBVT_IMPL_SSE) || \
|
|
(DBVT_INT0_IMPL == DBVT_IMPL_SSE)
|
|
#include <emmintrin.h>
|
|
#endif
|
|
|
|
//
|
|
// Auto config and checks
|
|
//
|
|
|
|
#if DBVT_USE_TEMPLATE
|
|
#define DBVT_VIRTUAL
|
|
#define DBVT_VIRTUAL_DTOR(a)
|
|
#define DBVT_PREFIX template <typename T>
|
|
#define DBVT_IPOLICY T& policy
|
|
#define DBVT_CHECKTYPE \
|
|
static const ICollide& typechecker = *(T*)1; \
|
|
(void)typechecker;
|
|
#else
|
|
#define DBVT_VIRTUAL_DTOR(a) \
|
|
virtual ~a() {}
|
|
#define DBVT_VIRTUAL virtual
|
|
#define DBVT_PREFIX
|
|
#define DBVT_IPOLICY ICollide& policy
|
|
#define DBVT_CHECKTYPE
|
|
#endif
|
|
|
|
#if DBVT_USE_MEMMOVE
|
|
#if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
|
|
#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
|
|
//
|
|
|
|
/* btDbvtAabbMm */
|
|
struct btDbvtAabbMm
|
|
{
|
|
DBVT_INLINE btDbvtAabbMm(){}
|
|
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); }
|
|
|
|
private:
|
|
DBVT_INLINE void AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const;
|
|
|
|
private:
|
|
btVector3 mi, mx;
|
|
};
|
|
|
|
// Types
|
|
typedef btDbvtAabbMm btDbvtVolume;
|
|
|
|
/* btDbvtNode */
|
|
struct btDbvtNode
|
|
{
|
|
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;
|
|
};
|
|
};
|
|
|
|
/* 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)
|
|
, angle(0)
|
|
, data(n->data)
|
|
{
|
|
childs[0] = 0;
|
|
childs[1] = 0;
|
|
}
|
|
|
|
~btDbvntNode()
|
|
{
|
|
if (childs[0])
|
|
delete childs[0];
|
|
if (childs[1])
|
|
delete childs[1];
|
|
}
|
|
};
|
|
|
|
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.
|
|
struct btDbvt
|
|
{
|
|
/* Stack element */
|
|
struct sStkNN
|
|
{
|
|
const btDbvtNode* a;
|
|
const btDbvtNode* b;
|
|
sStkNN() {}
|
|
sStkNN(const btDbvtNode* na, const btDbvtNode* nb) : a(na), b(nb) {}
|
|
};
|
|
struct sStkNP
|
|
{
|
|
const btDbvtNode* node;
|
|
int mask;
|
|
sStkNP(const btDbvtNode* n, unsigned m) : node(n), mask(m) {}
|
|
};
|
|
struct sStkNPS
|
|
{
|
|
const btDbvtNode* node;
|
|
int mask;
|
|
btScalar value;
|
|
sStkNPS() {}
|
|
sStkNPS(const btDbvtNode* n, unsigned m, btScalar v) : node(n), mask(m), value(v) {}
|
|
};
|
|
struct sStkCLN
|
|
{
|
|
const btDbvtNode* node;
|
|
btDbvtNode* parent;
|
|
sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {}
|
|
};
|
|
|
|
struct sStknNN
|
|
{
|
|
const btDbvntNode* a;
|
|
const btDbvntNode* b;
|
|
sStknNN() {}
|
|
sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {}
|
|
};
|
|
// Policies/Interfaces
|
|
|
|
/* ICollide */
|
|
struct ICollide
|
|
{
|
|
DBVT_VIRTUAL_DTOR(ICollide)
|
|
DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {}
|
|
DBVT_VIRTUAL void Process(const btDbvtNode*) {}
|
|
DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); }
|
|
DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {}
|
|
DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); }
|
|
DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); }
|
|
};
|
|
/* IWriter */
|
|
struct IWriter
|
|
{
|
|
virtual ~IWriter() {}
|
|
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;
|
|
};
|
|
/* IClone */
|
|
struct IClone
|
|
{
|
|
virtual ~IClone() {}
|
|
virtual void CloneLeaf(btDbvtNode*) {}
|
|
};
|
|
|
|
// Constants
|
|
enum
|
|
{
|
|
SIMPLE_STACKSIZE = 64,
|
|
DOUBLE_STACKSIZE = SIMPLE_STACKSIZE * 2
|
|
};
|
|
|
|
// Fields
|
|
btDbvtNode* m_root;
|
|
btDbvtNode* m_free;
|
|
int m_lkhd;
|
|
int m_leaves;
|
|
unsigned m_opath;
|
|
|
|
btAlignedObjectArray<sStkNN> m_stkStack;
|
|
|
|
// Methods
|
|
btDbvt();
|
|
~btDbvt();
|
|
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);
|
|
#if DBVT_ENABLE_BENCHMARK
|
|
static void benchmark();
|
|
#else
|
|
static void benchmark()
|
|
{
|
|
}
|
|
#endif
|
|
// DBVT_IPOLICY must support ICollide policy/interface
|
|
DBVT_PREFIX
|
|
static void enumNodes(const btDbvtNode* root,
|
|
DBVT_IPOLICY);
|
|
DBVT_PREFIX
|
|
static void enumLeaves(const btDbvtNode* root,
|
|
DBVT_IPOLICY);
|
|
DBVT_PREFIX
|
|
void collideTT(const btDbvtNode* root0,
|
|
const btDbvtNode* root1,
|
|
DBVT_IPOLICY);
|
|
DBVT_PREFIX
|
|
void selfCollideT(const btDbvntNode* root,
|
|
DBVT_IPOLICY);
|
|
DBVT_PREFIX
|
|
void selfCollideTT(const btDbvtNode* root,
|
|
DBVT_IPOLICY);
|
|
|
|
DBVT_PREFIX
|
|
void collideTTpersistentStack(const btDbvtNode* root0,
|
|
const btDbvtNode* root1,
|
|
DBVT_IPOLICY);
|
|
#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
|
|
void collideTV(const btDbvtNode* root,
|
|
const btDbvtVolume& volume,
|
|
DBVT_IPOLICY) const;
|
|
|
|
DBVT_PREFIX
|
|
void collideTVNoStackAlloc(const btDbvtNode* root,
|
|
const btDbvtVolume& volume,
|
|
btNodeStack& stack,
|
|
DBVT_IPOLICY) const;
|
|
|
|
///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
|
|
static void rayTest(const btDbvtNode* root,
|
|
const btVector3& rayFrom,
|
|
const btVector3& rayTo,
|
|
DBVT_IPOLICY);
|
|
///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
|
|
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;
|
|
|
|
DBVT_PREFIX
|
|
static void collideKDOP(const btDbvtNode* root,
|
|
const btVector3* normals,
|
|
const btScalar* offsets,
|
|
int count,
|
|
DBVT_IPOLICY);
|
|
DBVT_PREFIX
|
|
static void collideOCL(const btDbvtNode* root,
|
|
const btVector3* normals,
|
|
const btScalar* offsets,
|
|
const btVector3& sortaxis,
|
|
int count,
|
|
DBVT_IPOLICY,
|
|
bool fullsort = true);
|
|
DBVT_PREFIX
|
|
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)
|
|
{
|
|
int m = 0;
|
|
while (l < h)
|
|
{
|
|
m = (l + h) >> 1;
|
|
if (a[i[m]].value >= v)
|
|
l = m + 1;
|
|
else
|
|
h = m;
|
|
}
|
|
return (h);
|
|
}
|
|
static DBVT_INLINE int allocate(btAlignedObjectArray<int>& ifree,
|
|
btAlignedObjectArray<sStkNPS>& stock,
|
|
const sStkNPS& value)
|
|
{
|
|
int i;
|
|
if (ifree.size() > 0)
|
|
{
|
|
i = ifree[ifree.size() - 1];
|
|
ifree.pop_back();
|
|
stock[i] = value;
|
|
}
|
|
else
|
|
{
|
|
i = stock.size();
|
|
stock.push_back(value);
|
|
}
|
|
return (i);
|
|
}
|
|
//
|
|
private:
|
|
btDbvt(const btDbvt&) {}
|
|
};
|
|
|
|
//
|
|
// Inline's
|
|
//
|
|
|
|
//
|
|
inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c, const btVector3& e)
|
|
{
|
|
btDbvtAabbMm box;
|
|
box.mi = c - e;
|
|
box.mx = c + e;
|
|
return (box);
|
|
}
|
|
|
|
//
|
|
inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c, btScalar r)
|
|
{
|
|
return (FromCE(c, btVector3(r, r, r)));
|
|
}
|
|
|
|
//
|
|
inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi, const btVector3& mx)
|
|
{
|
|
btDbvtAabbMm box;
|
|
box.mi = mi;
|
|
box.mx = mx;
|
|
return (box);
|
|
}
|
|
|
|
//
|
|
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts, int n)
|
|
{
|
|
btDbvtAabbMm box;
|
|
box.mi = box.mx = pts[0];
|
|
for (int i = 1; i < n; ++i)
|
|
{
|
|
box.mi.setMin(pts[i]);
|
|
box.mx.setMax(pts[i]);
|
|
}
|
|
return (box);
|
|
}
|
|
|
|
//
|
|
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts, int n)
|
|
{
|
|
btDbvtAabbMm box;
|
|
box.mi = box.mx = *ppts[0];
|
|
for (int i = 1; i < n; ++i)
|
|
{
|
|
box.mi.setMin(*ppts[i]);
|
|
box.mx.setMax(*ppts[i]);
|
|
}
|
|
return (box);
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e)
|
|
{
|
|
mi -= e;
|
|
mx += e;
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e)
|
|
{
|
|
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]);
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
|
|
{
|
|
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()));
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n, btScalar o, int s) const
|
|
{
|
|
btVector3 pi, px;
|
|
switch (s)
|
|
{
|
|
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;
|
|
}
|
|
if ((btDot(n, px) + o) < 0) return (-1);
|
|
if ((btDot(n, pi) + o) >= 0) return (+1);
|
|
return (0);
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v, unsigned signs) const
|
|
{
|
|
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));
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const
|
|
{
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
if (d[i] < 0)
|
|
{
|
|
smi += mx[i] * d[i];
|
|
smx += mi[i] * d[i];
|
|
}
|
|
else
|
|
{
|
|
smi += mi[i] * d[i];
|
|
smx += mx[i] * d[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE bool Intersect(const btDbvtAabbMm& a,
|
|
const btDbvtAabbMm& b)
|
|
{
|
|
#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);
|
|
#else
|
|
const int* pu((const int*)&rt);
|
|
#endif
|
|
return ((pu[0] | pu[1] | pu[2]) == 0);
|
|
#else
|
|
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()));
|
|
#endif
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE bool Intersect(const btDbvtAabbMm& a,
|
|
const btVector3& b)
|
|
{
|
|
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()));
|
|
}
|
|
|
|
//////////////////////////////////////
|
|
|
|
//
|
|
DBVT_INLINE btScalar Proximity(const btDbvtAabbMm& a,
|
|
const btDbvtAabbMm& b)
|
|
{
|
|
const btVector3 d = (a.mi + a.mx) - (b.mi + b.mx);
|
|
return (btFabs(d.x()) + btFabs(d.y()) + btFabs(d.z()));
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE int Select(const btDbvtAabbMm& o,
|
|
const btDbvtAabbMm& a,
|
|
const btDbvtAabbMm& b)
|
|
{
|
|
#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
|
|
|
|
#if defined(_WIN32)
|
|
static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff};
|
|
#else
|
|
static ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/};
|
|
#endif
|
|
///@todo: the intrinsic version is 11% slower
|
|
#if DBVT_USE_INTRINSIC_SSE
|
|
|
|
union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
|
|
{
|
|
__m128 ssereg;
|
|
float floats[4];
|
|
int ints[4];
|
|
};
|
|
|
|
__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));
|
|
|
|
btSSEUnion tmp;
|
|
tmp.ssereg = _mm_cmple_ss(bmi, ami);
|
|
return tmp.ints[0] & 1;
|
|
|
|
#else
|
|
ATTRIBUTE_ALIGNED16(__int32 r[1]);
|
|
__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
|
|
}
|
|
return (r[0] & 1);
|
|
#endif
|
|
#else
|
|
return (Proximity(o, a) < Proximity(o, b) ? 0 : 1);
|
|
#endif
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE void Merge(const btDbvtAabbMm& a,
|
|
const btDbvtAabbMm& b,
|
|
btDbvtAabbMm& r)
|
|
{
|
|
#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);
|
|
#else
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
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];
|
|
}
|
|
#endif
|
|
}
|
|
|
|
//
|
|
DBVT_INLINE bool NotEqual(const btDbvtAabbMm& a,
|
|
const btDbvtAabbMm& b)
|
|
{
|
|
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()));
|
|
}
|
|
|
|
//
|
|
// Inline's
|
|
//
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::enumNodes(const btDbvtNode* root,
|
|
DBVT_IPOLICY)
|
|
{
|
|
DBVT_CHECKTYPE
|
|
policy.Process(root);
|
|
if (root->isinternal())
|
|
{
|
|
enumNodes(root->childs[0], policy);
|
|
enumNodes(root->childs[1], policy);
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::enumLeaves(const btDbvtNode* root,
|
|
DBVT_IPOLICY)
|
|
{
|
|
DBVT_CHECKTYPE
|
|
if (root->isinternal())
|
|
{
|
|
enumLeaves(root->childs[0], policy);
|
|
enumLeaves(root->childs[1], policy);
|
|
}
|
|
else
|
|
{
|
|
policy.Process(root);
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::collideTT(const btDbvtNode* root0,
|
|
const btDbvtNode* root1,
|
|
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 (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);
|
|
}
|
|
}
|
|
|
|
//
|
|
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);
|
|
}
|
|
}
|
|
|
|
|
|
DBVT_PREFIX
|
|
inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0,
|
|
const btDbvtNode* root1,
|
|
DBVT_IPOLICY)
|
|
{
|
|
DBVT_CHECKTYPE
|
|
if (root0 && root1)
|
|
{
|
|
int depth = 1;
|
|
int treshold = DOUBLE_STACKSIZE - 4;
|
|
|
|
m_stkStack.resize(DOUBLE_STACKSIZE);
|
|
m_stkStack[0] = sStkNN(root0, root1);
|
|
do
|
|
{
|
|
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())
|
|
{
|
|
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]);
|
|
}
|
|
}
|
|
else if (Intersect(p.a->volume, p.b->volume))
|
|
{
|
|
if (p.a->isinternal())
|
|
{
|
|
if (p.b->isinternal())
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (p.b->isinternal())
|
|
{
|
|
m_stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
|
|
m_stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
|
|
}
|
|
else
|
|
{
|
|
policy.Process(p.a, p.b);
|
|
}
|
|
}
|
|
}
|
|
} while (depth);
|
|
}
|
|
}
|
|
|
|
#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);
|
|
}
|
|
#endif
|
|
|
|
DBVT_PREFIX
|
|
inline void btDbvt::collideTV(const btDbvtNode* root,
|
|
const btDbvtVolume& vol,
|
|
DBVT_IPOLICY) const
|
|
{
|
|
DBVT_CHECKTYPE
|
|
if (root)
|
|
{
|
|
ATTRIBUTE_ALIGNED16(btDbvtVolume)
|
|
volume(vol);
|
|
btAlignedObjectArray<const btDbvtNode*> stack;
|
|
stack.resize(0);
|
|
#ifndef BT_DISABLE_STACK_TEMP_MEMORY
|
|
char tempmemory[SIMPLE_STACKSIZE * sizeof(const btDbvtNode*)];
|
|
stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE);
|
|
#else
|
|
stack.reserve(SIMPLE_STACKSIZE);
|
|
#endif //BT_DISABLE_STACK_TEMP_MEMORY
|
|
|
|
stack.push_back(root);
|
|
do
|
|
{
|
|
const btDbvtNode* n = stack[stack.size() - 1];
|
|
stack.pop_back();
|
|
if (Intersect(n->volume, volume))
|
|
{
|
|
if (n->isinternal())
|
|
{
|
|
stack.push_back(n->childs[0]);
|
|
stack.push_back(n->childs[1]);
|
|
}
|
|
else
|
|
{
|
|
policy.Process(n);
|
|
}
|
|
}
|
|
} while (stack.size() > 0);
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::collideTVNoStackAlloc(const btDbvtNode* root,
|
|
const btDbvtVolume& vol,
|
|
btNodeStack& stack,
|
|
DBVT_IPOLICY) const
|
|
{
|
|
DBVT_CHECKTYPE
|
|
if (root)
|
|
{
|
|
ATTRIBUTE_ALIGNED16(btDbvtVolume)
|
|
volume(vol);
|
|
stack.resize(0);
|
|
stack.reserve(SIMPLE_STACKSIZE);
|
|
stack.push_back(root);
|
|
do
|
|
{
|
|
const btDbvtNode* n = stack[stack.size() - 1];
|
|
stack.pop_back();
|
|
if (Intersect(n->volume, volume))
|
|
{
|
|
if (n->isinternal())
|
|
{
|
|
stack.push_back(n->childs[0]);
|
|
stack.push_back(n->childs[1]);
|
|
}
|
|
else
|
|
{
|
|
policy.Process(n);
|
|
}
|
|
}
|
|
} while (stack.size() > 0);
|
|
}
|
|
}
|
|
|
|
DBVT_PREFIX
|
|
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
|
|
{
|
|
(void)rayTo;
|
|
DBVT_CHECKTYPE
|
|
if (root)
|
|
{
|
|
btVector3 resultNormal;
|
|
|
|
int depth = 1;
|
|
int treshold = DOUBLE_STACKSIZE - 2;
|
|
stack.resize(DOUBLE_STACKSIZE);
|
|
stack[0] = root;
|
|
btVector3 bounds[2];
|
|
do
|
|
{
|
|
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)
|
|
{
|
|
if (node->isinternal())
|
|
{
|
|
if (depth > treshold)
|
|
{
|
|
stack.resize(stack.size() * 2);
|
|
treshold = stack.size() - 2;
|
|
}
|
|
stack[depth++] = node->childs[0];
|
|
stack[depth++] = node->childs[1];
|
|
}
|
|
else
|
|
{
|
|
policy.Process(node);
|
|
}
|
|
}
|
|
} while (depth);
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::rayTest(const btDbvtNode* root,
|
|
const btVector3& rayFrom,
|
|
const btVector3& rayTo,
|
|
DBVT_IPOLICY)
|
|
{
|
|
DBVT_CHECKTYPE
|
|
if (root)
|
|
{
|
|
btVector3 rayDir = (rayTo - rayFrom);
|
|
rayDir.normalize();
|
|
|
|
///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};
|
|
|
|
btScalar lambda_max = rayDir.dot(rayTo - rayFrom);
|
|
|
|
btVector3 resultNormal;
|
|
|
|
btAlignedObjectArray<const btDbvtNode*> stack;
|
|
|
|
int depth = 1;
|
|
int treshold = DOUBLE_STACKSIZE - 2;
|
|
|
|
char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)];
|
|
#ifndef BT_DISABLE_STACK_TEMP_MEMORY
|
|
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();
|
|
|
|
btScalar tmin = 1.f, lambda_min = 0.f;
|
|
unsigned int result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
|
|
|
|
#ifdef COMPARE_BTRAY_AABB2
|
|
btScalar param = 1.f;
|
|
bool result2 = btRayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal);
|
|
btAssert(result1 == result2);
|
|
#endif //TEST_BTRAY_AABB2
|
|
|
|
if (result1)
|
|
{
|
|
if (node->isinternal())
|
|
{
|
|
if (depth > treshold)
|
|
{
|
|
stack.resize(stack.size() * 2);
|
|
treshold = stack.size() - 2;
|
|
}
|
|
stack[depth++] = node->childs[0];
|
|
stack[depth++] = node->childs[1];
|
|
}
|
|
else
|
|
{
|
|
policy.Process(node);
|
|
}
|
|
}
|
|
} while (depth);
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::collideKDOP(const btDbvtNode* root,
|
|
const btVector3* normals,
|
|
const btScalar* offsets,
|
|
int count,
|
|
DBVT_IPOLICY)
|
|
{
|
|
DBVT_CHECKTYPE
|
|
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)
|
|
{
|
|
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)
|
|
{
|
|
if (0 == (se.mask & j))
|
|
{
|
|
const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
|
|
switch (side)
|
|
{
|
|
case -1:
|
|
out = true;
|
|
break;
|
|
case +1:
|
|
se.mask |= j;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (!out)
|
|
{
|
|
if ((se.mask != inside) && (se.node->isinternal()))
|
|
{
|
|
stack.push_back(sStkNP(se.node->childs[0], se.mask));
|
|
stack.push_back(sStkNP(se.node->childs[1], se.mask));
|
|
}
|
|
else
|
|
{
|
|
if (policy.AllLeaves(se.node)) enumLeaves(se.node, policy);
|
|
}
|
|
}
|
|
} while (stack.size());
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::collideOCL(const btDbvtNode* root,
|
|
const btVector3* normals,
|
|
const btScalar* offsets,
|
|
const btVector3& sortaxis,
|
|
int count,
|
|
DBVT_IPOLICY,
|
|
bool fsort)
|
|
{
|
|
DBVT_CHECKTYPE
|
|
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)
|
|
{
|
|
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)
|
|
{
|
|
bool out = false;
|
|
for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1)
|
|
{
|
|
if (0 == (se.mask & j))
|
|
{
|
|
const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
|
|
switch (side)
|
|
{
|
|
case -1:
|
|
out = true;
|
|
break;
|
|
case +1:
|
|
se.mask |= j;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (out) continue;
|
|
}
|
|
if (policy.Descent(se.node))
|
|
{
|
|
if (se.node->isinternal())
|
|
{
|
|
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))
|
|
{
|
|
/* 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 );
|
|
|
|
#if DBVT_USE_MEMMOVE
|
|
{
|
|
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);
|
|
}
|
|
#else
|
|
for (int k = stack.size() - 1; k > j; --k)
|
|
{
|
|
stack[k] = stack[k - 1];
|
|
}
|
|
#endif
|
|
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);
|
|
#if DBVT_USE_MEMMOVE
|
|
{
|
|
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);
|
|
}
|
|
#else
|
|
for (int k = stack.size() - 1; k > j; --k)
|
|
{
|
|
stack[k] = stack[k - 1];
|
|
}
|
|
#endif
|
|
stack[j] = allocate(ifree, stock, nes[1 - q]);
|
|
}
|
|
else
|
|
{
|
|
stack.push_back(allocate(ifree, stock, nes[q]));
|
|
stack.push_back(allocate(ifree, stock, nes[1 - q]));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
policy.Process(se.node, se.value);
|
|
}
|
|
}
|
|
} while (stack.size());
|
|
}
|
|
}
|
|
|
|
//
|
|
DBVT_PREFIX
|
|
inline void btDbvt::collideTU(const btDbvtNode* root,
|
|
DBVT_IPOLICY)
|
|
{
|
|
DBVT_CHECKTYPE
|
|
if (root)
|
|
{
|
|
btAlignedObjectArray<const btDbvtNode*> stack;
|
|
stack.reserve(SIMPLE_STACKSIZE);
|
|
stack.push_back(root);
|
|
do
|
|
{
|
|
const btDbvtNode* n = stack[stack.size() - 1];
|
|
stack.pop_back();
|
|
if (policy.Descent(n))
|
|
{
|
|
if (n->isinternal())
|
|
{
|
|
stack.push_back(n->childs[0]);
|
|
stack.push_back(n->childs[1]);
|
|
}
|
|
else
|
|
{
|
|
policy.Process(n);
|
|
}
|
|
}
|
|
} while (stack.size() > 0);
|
|
}
|
|
}
|
|
|
|
//
|
|
// 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
|