lx/watrbx-game-engine
1
0
Fork 0
mirror of https://github.com/copyrighttxt/watrbx-game-engine.git synced 2026-09-04 20:57:49 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
master
watrbx-game-engine/App/v8world/SpatialHashMultiRes.inl
T
watrabi c93494f795 fahhh
2025-10-28 14:05:46 -04:00

1737 lines
52 KiB
C++
Raw Permalink Blame History

#include "V8World/World.h"
#include "V8World/ContactManager.h"
#include "Util/Math.h"
#include "rbx/Debug.h"
#include "RbxAssert.h"
#include "G3D/CollisionDetection.h"
#include <vector>
namespace RBX {
float SpatialHashStatic::hashGridSize(int level) {return (float)(cellMinSize << level);}
float SpatialHashStatic::hashGridRecip(int level) {return 1.0f / SpatialHashStatic::hashGridSize(level);}
size_t SpatialHashStatic::numBuckets(int level) {return 65536;}
Vector3int32 SpatialHashStatic::realToHashGrid(int level, const Vector3& realPoint)
{
Vector3 gridPoint = realPoint * SpatialHashStatic::hashGridRecip(level);
Vector3int32 hashGrid = Vector3int32::floor(gridPoint); // 4 grids per hash bucket
return hashGrid;
}
ExtentsInt32 SpatialHashStatic::scaleExtents(int smallLevel, int bigLevel, const ExtentsInt32& smallExtents)
{
RBXASSERT_SLOW(smallLevel < bigLevel);
int delta = bigLevel - smallLevel;
return smallExtents.shiftRight(delta);
}
Extents SpatialHashStatic::hashGridToRealExtents(int level, const Vector3int32& hashGrid)
{
Extents answer( hashGridToReal(level, hashGrid),
hashGridToReal(level, hashGrid + Vector3int32::one())
);
RBXASSERT_VERY_FAST(Math::isIntegerVector3(answer.min()));
RBXASSERT_VERY_FAST(Math::isIntegerVector3(answer.max()));
return answer;
}
Vector3 SpatialHashStatic::hashGridToReal(int level, const Vector3int32& hashGrid)
{
return hashGrid.toVector3() * SpatialHashStatic::hashGridSize(level);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
#define SHP template<class Primitive, class Contact, class ContactManager, int MAX_LEVELS>
// statics - templated SpatialHash
SHP const int SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::rootLevel = MAX_LEVELS-1;
SHP SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::SpatialHash(World* world, ContactManager* contactManager, int maxCellsPerPrimitive)
: world(world)
, contactManager(contactManager)
, maxCellsPerPrimitive(maxCellsPerPrimitive)
, nodesOut(0)
, maxBucket(0)
, numTreeNodesTotal(0)
{
setup();
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::setup()
{
for (int i=0; i<MAX_LEVELS; i++) {
hashTables[i].resize(SpatialHashStatic::numBuckets(i));
memset(&hashTables[i][0], 0, sizeof(hashTables[i][0])*hashTables[i].size());
}
}
SHP SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::~SpatialHash()
{
cleanup();
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::cleanup()
{
// nb: make all checks match with fastClear as well.
RBXASSERT_SPATIAL_HASH(validateTallyTreeNodes());
RBXASSERT_SPATIAL_HASH(validateNoNodesOut());
RBXASSERT(nodesOut == 0);
}
SHP const Extents SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::calcNewExtents(Primitive* p)
{
return SpatialHashStatic::safeExtents(p->getFastFuzzyExtents());
}
SHP typename SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::SpatialNode* SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::newNode(int level, int hash, const Vector3int32& grid)
{
SpatialNode* answer;
answer = new SpatialNode(level, hash, grid);
if (level == 0) {
answer->treeNode = NULL;
}
else {
answer->treeNode = findTreeNode(level, hash, grid);
if (! answer->treeNode)
answer->treeNode = createTreeNode(level, hash, grid);
answer->treeNode->refByPrimitives++;
RBXASSERT(level == answer->treeNode->level);
}
return answer;
}
SHP typename SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::TreeNode *SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::findTreeNode(
int level, int hash, const Vector3int32 &gridCoord)
{
TreeNode * tn = hashTables[level][hash].treeNodes;
while (tn) {
RBXASSERT(tn->hashId == hash);
RBXASSERT(tn->level == level);
if (tn->gridId == gridCoord) {
return tn;
}
tn = tn->next;
}
return NULL;
}
SHP typename SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::TreeNode *SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::createTreeNode(
int level, int hash, const Vector3int32 &gridCoord)
{
TreeNode *tn;
tn = new TreeNode();
numTreeNodesTotal ++;
tn->refByPrimitives = 0;
tn->level = level;
tn->hashId = hash;
tn->gridId = gridCoord;
RBXASSERT_SPATIAL_HASH(validateTreeNodeNotHere(tn, level, hash));
tn->next = hashTables[level][hash].treeNodes;
hashTables[level][hash].treeNodes = tn;
return tn;
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::_retireTreeNode(TreeNode* tn) {
TreeNode** p = &hashTables[tn->level][tn->hashId].treeNodes;
while (*p != tn) {
p = &((*p)->next);
}
*p = tn->next;
tn->next = NULL;
delete tn;
numTreeNodesTotal --;
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::retireTreeNode(TreeNode* tn)
{
if (--tn->refByPrimitives) {
// still being used, do nothing
return;
}
if (tn->childMask) {
// This tree node still has children, i.e. it's part of the tree.
// Therefore, even though no SpatialNode is using this treenode (which
// in turn means this treenode corresponds to no primitives), it
// needs to be kept.
return;
}
// this treenode's refcount is 0 AND has no children
//retire it and adjust hierarchy
int tnlevel = tn->level;
Vector3int32 tngridId(tn->gridId);
_retireTreeNode(tn);
// notify ancestors that this has been removed
removeTreeNodeChild(tnlevel, tngridId);
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::insertNodeToPrimitive(SpatialNode* node,
Primitive* p,
const Vector3int32& grid,
int hash)
{
RBXASSERT(node->level == p->getSpatialNodeLevel());
node->primitive = p;
node->gridId = grid;
node->hashId = hash;
RBXASSERT_SPATIAL_HASH(validateInsertNodeToPrimitive(node, p, grid, hash));
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::returnNode(SpatialNode* node)
{
delete node;
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::removeNodeFromHash(SpatialNode* remove)
{
SpatialNode** nodePtr = &hashTables[remove->getLevel()][remove->hashId].nodes;
while (*nodePtr != remove) {
nodePtr = &((*nodePtr)->nextHashLink);
}
*nodePtr = remove->nextHashLink;
}
SHP typename SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::SpatialNode* SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::findNode(Primitive* p, const Vector3int32& grid)
{
const int l = p->getSpatialNodeLevel();
RBXASSERT_SPATIAL_HASH(static_cast<SpatialNode*>(p->spatialNodes)->getLevel() == l);
RBXASSERT(SpatialHashStatic::numBuckets(l) == hashTables[l].size());
int hash = SpatialHashStatic::getHash(l, grid);
SpatialNode* node = hashTables[l][hash].nodes;
if (node == NULL)
return NULL;
while ((node->primitive != p) || (node->gridId != grid)) {
node = node->nextHashLink;
if (! node) {
break;
}
}
return node;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::oldExtentsOverlap(Primitive* me, Primitive* other)
{
const int myLevel = me->getSpatialNodeLevel();
const int otherLevel = other->getSpatialNodeLevel();
RBXASSERT(myLevel >= 0);
RBXASSERT(otherLevel >= 0);
bool answer = false;
if (myLevel == otherLevel) {
answer = ExtentsInt32::overlapsOrTouches(me->getOldSpatialExtents(), other->getOldSpatialExtents());
} else {
Primitive* big = (myLevel > otherLevel) ? me : other;
Primitive* smallPrim = (big == me) ? other : me;
ExtentsInt32 smallInBig = SpatialHashStatic::scaleExtents(smallPrim->getSpatialNodeLevel(), big->getSpatialNodeLevel(), smallPrim->getOldSpatialExtents());
answer = ExtentsInt32::overlapsOrTouches(big->getOldSpatialExtents(), smallInBig);
}
return answer;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::findOtherNodesInLevel0Cell(SpatialNode* destroy)
{
RBXASSERT(destroy->getLevel() == 0);
Vector3int32 g = destroy->gridId;
int hash = destroy->hashId;
SpatialNode* node_sameHash = hashTables[0][hash].nodes;
while (node_sameHash) {
if (node_sameHash->gridId == g)
{
#ifdef _DEBUG
Primitive* other = node_sameHash->primitive;
RBXASSERT(other != destroy->primitive);
#endif
return true;
}
node_sameHash = node_sameHash->nextHashLink;
}
return false;
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::destroyNode(SpatialNode* destroy)
{
RBXASSERT_SPATIAL_HASH(validateTallyTreeNodes());
RBXASSERT_SPATIAL_HASH(validateRemoveNodeFromPrimitive(destroy));
RBXASSERT_SPATIAL_HASH(destroy->getLevel() == destroy->primitive->getSpatialNodeLevel());
// This is insignificant for timing
removeNodeFromHash(destroy);
// for all lower level nodes
if (destroy->getLevel() > 0)
{
RBXASSERT(destroy->treeNode);
retireTreeNode(destroy->treeNode);
}
else
{
if (!findOtherNodesInLevel0Cell(destroy)) { // this (conceptual) leaf cell is empty; maintain hierachy
removeTreeNodeChild(0, destroy->gridId);
}
}
returnNode(destroy);
nodesOut--;
RBXASSERT_SPATIAL_HASH(validateTallyTreeNodes());
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::removeTreeNodeChild(int childLevel, Vector3int32 &childGridCoord)
{
int childHash;
RBXASSERT((childHash = SpatialHashStatic::getHash(childLevel, childGridCoord), 1));
for (int l=childLevel+1; l < MAX_LEVELS; l++) {
Vector3int32 g;
g.x = childGridCoord.x>>(l-childLevel); g.y = childGridCoord.y>>(l-childLevel); g.z = childGridCoord.z>>(l-childLevel);
int hash = SpatialHashStatic::getHash(l, g);
TreeNode *tn = findTreeNode(l, hash, g);
RBXASSERT(tn);
int offset = ((childGridCoord.x>>(l-childLevel-1)) & 1)+
(((childGridCoord.y>>(l-childLevel-1)) & 1)<<1) + (((childGridCoord.z>>(l-childLevel-1)) & 1) << 2);
RBXASSERT(tn->children[offset] == childHash);
// remove child
tn->removeChild(offset);
// if there's still children
if (tn->refByPrimitives==0 && !tn->childMask) {
// this means the child that we've just removed is the only
// reason why this treenode existed. Now it's removed
_retireTreeNode(tn);
} else {
// if this treenode is not deleted, no need to check parent
break;
}
childHash = hash;
}
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::hashHasPrimitive(int level, Primitive* p, int hash, const Vector3int32& grid)
{
SpatialNode* test = hashTables[level][hash].nodes;
while (test) {
if ((test->primitive == p) && (test->gridId == grid)) {
return true;
}
test = test->nextHashLink;
}
return false;
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::addContactFromChildren(TreeNode *tn, Primitive *p)
{
unsigned short *children = tn->children;
Vector3int32 baseGrid;
baseGrid.x = tn->gridId.x << 1;
baseGrid.y = tn->gridId.y << 1;
baseGrid.z = tn->gridId.z << 1;
for (int i=0; i<8; i++) {
if (tn->hasChild(i)) {
Vector3int32 g;
g.x = baseGrid.x + (i & 1);
g.y = baseGrid.y + ((i & 2)>>1);
g.z = baseGrid.z + ((i & 4)>>2);
RBXASSERT(tn->level - 1 >= 0);
SpatialNode *tryNode = hashTables[ tn->level - 1 ][ children[i] ].nodes;
while (tryNode) {
Primitive* other = tryNode->primitive;
if ((other != p) && (tryNode->gridId == g )) {
if (Primitive::getContact(p, other) == NULL)
{
contactManager->onNewPair(p, other);
}
}
else {
RBXASSERT(g != tryNode->gridId);
}
tryNode = tryNode->nextHashLink;
}
//recursive
if (tn->level > 1) {
TreeNode *c = findTreeNode(tn->level - 1, children[i], g);
RBXASSERT(c);
addContactFromChildren(c, p);
}
}
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::addNode(Primitive* p, const Vector3int32& grid, bool addContact)
{
RBXASSERT(p->getSpatialNodeLevel() != -1);
int level = p->getSpatialNodeLevel();
int hash = SpatialHashStatic::getHash(level, grid);
RBXASSERT_SPATIAL_HASH(validateTallyTreeNodes());
RBXASSERT_VERY_FAST(!hashHasPrimitive(level, p, hash, grid));
nodesOut++;
SpatialNode* addedNode = newNode(level, hash, grid);
#ifdef _DEBUG
if (level > 0) {
TreeNode *tn_save = addedNode->treeNode;
RBXASSERT(addedNode->getLevel() == tn_save->level);
}
#endif
// 1. Put in the primitive's linked list
insertNodeToPrimitive(addedNode, p, grid, hash);
// 2. This hash's linked list of nodes
SpatialNode* tryNode = hashTables[level][hash].nodes;
addedNode->nextHashLink = tryNode;
hashTables[level][hash].nodes = addedNode;
// 3. Cycle through the pre-existing nodes - see if any are hit
int numNodes = 1; // start with 1 - this one
// For nodes at the same level, and higher levels nodes containing this node
Vector3int32 g = grid;
int prevHash = -1;
bool needHiearchyUpdate = true;
for (int l=level; l < MAX_LEVELS; l++) {
if (l > level) {
g.x >>= 1; g.y >>= 1; g.z >>= 1;
hash = SpatialHashStatic::getHash(l, g);
tryNode = hashTables[l][hash].nodes;
}
if (Primitive::hasGetFirstContact && addContact)
{
while (tryNode) {
RBXASSERT( l < MAX_LEVELS);
numNodes++;
Primitive* other = tryNode->primitive;
if ((other != p) && (tryNode->gridId == g)) {
if (Primitive::getContact(p, other) == NULL)
{
contactManager->onNewPair(p, other);
}
}
else {
RBXASSERT(g != tryNode->gridId);
}
tryNode = tryNode->nextHashLink;
}
}
// maintain hiearchy information
if (l > level && needHiearchyUpdate) {
TreeNode *tn = findTreeNode(l, hash, g);
if (!tn)
tn = createTreeNode(l, hash, g);
int offset = ((grid.x>>(l-level-1)) & 1)+
(((grid.y>>(l-level-1)) & 1)<<1) + (((grid.z>>(l-level-1)) & 1) << 2);
RBXASSERT(0<=offset && offset<=7 && prevHash >= 0);
// establish child "pointer"
RBXASSERT(!tn->hasChild(offset) || tn->children[offset]==prevHash);
if (tn->hasChild(offset))
needHiearchyUpdate = false; // this and upper levels tree-nodes has already been set
else
tn->setChild(offset, prevHash);
if (level > 0)
{
RBXASSERT(addedNode->treeNode && addedNode->getLevel() == addedNode->treeNode->level);
}
}
// save immediately-lower-level hashId
prevHash = hash;
}
// add contacts from children
if (Primitive::hasGetFirstContact && addContact && addedNode->getLevel() > 0) {
RBXASSERT(addedNode && addedNode->treeNode->level == addedNode->getLevel());
addContactFromChildren(addedNode->treeNode, p);
}
maxBucket = std::max(maxBucket, numNodes);
RBXASSERT_SPATIAL_HASH(validateTallyTreeNodes());
}
SHP int SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::computeLevel(const Primitive* p, const Extents& extents)
{
const float extra = static_cast<float>(SpatialHashStatic::cellMinSize * 2.0); // extra buffer for thin objects
Vector3 size = extents.size() + Vector3(extra, extra, extra);
float volume = size.x * size.y * size.z;
int maxLevel = p->requestFixed() ? SpatialHashStatic::maxLevelForAnchored : MAX_LEVELS - 1;
float maxVolumeThisLevel = static_cast<float>((SpatialHashStatic::cellMinSize * SpatialHashStatic::cellMinSize * SpatialHashStatic::cellMinSize)* maxCellsPerPrimitive);
for (int answerLevel = 0; answerLevel < maxLevel; ++answerLevel)
{
if (volume < maxVolumeThisLevel) {
return answerLevel;
}
maxVolumeThisLevel *= 8;
}
return maxLevel;
}
/*
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::computeMinMax(const Primitive* p, const Extents& extents, Vector3int32& min, Vector3int32& max)
{
RBXASSERT(p->getSpatialNodeLevel() != -1);
computeMinMax(p->getSpatialNodeLevel(), extents, min, max);
}
*/
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::onPrimitiveAdded(Primitive* p, bool addContact) {
if( contactManager->primitiveIsExcludedFromSpatialHash(p) )
return;
primitiveAdded(p, addContact);
if (!coarseMovementCallbacks.empty()) {
typename CoarseMovementCallback::UpdateInfo info;
info.updateType = CoarseMovementCallback::UpdateInfo::UPDATE_TYPE_Insert;
info.newLevel = p->getSpatialNodeLevel();
info.newSpatialExtents = p->getOldSpatialExtents();
for (size_t i = 0; i < coarseMovementCallbacks.size(); ++i) {
coarseMovementCallbacks[i]->coarsePrimitiveMovement(p, info);
}
}
}
// Note -use FastFuzzyExtents when not in the middle of a simulation step
// Use FastFuzzyExtentsNoCompute when in a step
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::primitiveAdded(Primitive* p, bool addContact)
{
WriteValidator writeValidator(concurrencyValidator);
RBXASSERT_SPATIAL_HASH(validateContacts(p));
RBXASSERT_SPATIAL_HASH(p->spatialNodes == NULL);
RBXASSERT(p->getSpatialNodeLevel() == -1);
Extents newExtentsFloat = calcNewExtents(p);
int level = computeLevel(p, newExtentsFloat);
Vector3int32 newMin, newMax;
SpatialHashStatic::computeMinMax(level, newExtentsFloat, newMin, newMax);
p->setSpatialNodeLevel(level);
ExtentsInt32 newExtents(newMin, newMax);
p->setOldSpatialExtents(newExtents);
changeMinMax(p, &newExtents, NULL, &newExtents, addContact);
RBXASSERT(p->getSpatialNodeLevel() == level);
RBXASSERT_SPATIAL_HASH(validateContacts(p));
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::changeMinMax( Primitive* p,
const ExtentsInt32* change,
const ExtentsInt32* oldBox,
const ExtentsInt32* newBox,
bool addContact)
{
bool newEqualsChange = (newBox == change);
bool oldEqualsChange = (oldBox == change);
for (int i = change->low.x; i <= change->high.x; ++i) {
for (int j = change->low.y; j <= change->high.y; ++j) {
for (int k = change->low.z; k <= change->high.z; ++k) {
Vector3int32 v(i, j, k);
const bool inNew = newEqualsChange || (newBox && newBox->contains(v));
const bool inOld = oldEqualsChange || (oldBox && oldBox->contains(v));
if (inNew && !inOld) {
addNode(p, v, addContact); // only update bucket counts when moving to avoid
} // big counts around 0,0,0
else if (inOld && !inNew) {
SpatialNode* destroyMe = findNode(p, v);
RBXASSERT(destroyMe);
destroyNode(destroyMe);
}
}
}
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::fastClear()
{
FastClearSpatialNode fastClearSpatialNode(this);
FastClearTreeNode fastClearTreeNode(this);
for (int level = 0; level < MAX_LEVELS; level ++)
{
for (int hashId=0; hashId<(int)SpatialHashStatic::numBuckets(level); hashId++)
{
SpatialNode *sn = hashTables[level][hashId].nodes;
while (sn)
{
SpatialNode *nodeToFree = sn;
sn = nodeToFree->nextHashLink;
fastClearSpatialNode(nodeToFree);
delete nodeToFree;
}
TreeNode *tn = hashTables[level][hashId].treeNodes;
while (tn)
{
TreeNode *nodeToFree = tn;
tn = nodeToFree->next;
RBXASSERT((fastClearTreeNode(nodeToFree), true)); // only run this if asserts are on.
delete nodeToFree;
};
}
}
Allocator<SpatialNode>::releaseMemory();
Allocator<TreeNode>::releaseMemory();
maxBucket = 0;
cleanup();
setup();
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::checkAndReleaseContacts(Primitive *p)
{
if (Primitive::hasGetFirstContact )
{
outOfContact.clear();
Contact * contact = p->getFirstContact();
while (contact) {
Primitive *other = contact->otherPrimitive(p);
contact = p->getNextContact(contact);
if( contactManager->primitiveIsExcludedFromSpatialHash(other) )
continue;
bool overlap = oldExtentsOverlap(p, other);
RBXASSERT_SPATIAL_HASH(overlap == validateNodesOverlap(p, other));
if (!overlap) {
outOfContact.append(other);
}
}
for (int i=0; i<(int)outOfContact.size(); i++)
contactManager->releasePair(p, outOfContact[i]);
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::primitiveExtentsChanged(Primitive* p, const Extents& extents)
{
WriteValidator writeValidator(concurrencyValidator);
RBXASSERT_SPATIAL_HASH(validateContacts(p));
RBXASSERT_SPATIAL_HASH(p->spatialNodes != NULL);
RBXASSERT(p->getSpatialNodeLevel() != -1);
Vector3int32 newMin, newMax;
// For now, never change a primitive's level in the hierarchy, once it
// has been determined at creation time
SpatialHashStatic::computeMinMax(p->getSpatialNodeLevel(), extents, newMin, newMax);
if ( (newMin == p->getOldSpatialMin())
&& (newMax == p->getOldSpatialMax())) {
return;
}
ExtentsInt32 oldBox(p->getOldSpatialExtents());
ExtentsInt32 newBox(newMin, newMax);
p->setOldSpatialExtents(newBox);
if (ExtentsInt32::overlapsOrTouches(oldBox, newBox)) {
ExtentsInt32 unionBox = ExtentsInt32::unionExtents(oldBox, newBox);
changeMinMax(p, &unionBox, &oldBox, &newBox);
}
else {
changeMinMax(p, &oldBox, &oldBox, NULL);
RBXASSERT_SPATIAL_HASH(p->spatialNodeCount == 0);
changeMinMax(p, &newBox, NULL, &newBox);
}
checkAndReleaseContacts(p);
RBXASSERT_SPATIAL_HASH(validateContacts(p));
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::onPrimitiveRemoved(Primitive* p) {
primitiveRemoved(p);
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::primitiveRemoved(Primitive* p)
{
WriteValidator writeValidator(concurrencyValidator);
RBXASSERT_SPATIAL_HASH(validateContacts(p));
RBXASSERT(p->getSpatialNodeLevel() != -1 || contactManager->primitiveIsExcludedFromSpatialHash(p));
changeMinMax(p, &p->getOldSpatialExtents(), &p->getOldSpatialExtents(), NULL);
p->setSpatialNodeLevel(-1);
p->setOldSpatialExtents(ExtentsInt32::empty());
RBXASSERT_SPATIAL_HASH(p->spatialNodeCount == 0);
if (Primitive::hasGetFirstContact )
{
outOfContact.clear();
for (int i = 0; i < p->getNumContacts(); i++)
outOfContact.append(p->getContactOther(i));
for (int i = 0; i < (int)outOfContact.size(); i++)
contactManager->releasePair(p, outOfContact[i]);
}
RBXASSERT_SPATIAL_HASH(validateContacts(p));
}
// Note -use FastFuzzyExtents when not in the middle of a simulation step
// Use FastFuzzyExtentsNoCompute when in a step
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::onPrimitiveExtentsChanged(Primitive* p)
{
if( contactManager->primitiveIsExcludedFromSpatialHash(p) )
return;
contactManager->checkTerrainContact(p);
Extents newExtentsFloat = calcNewExtents(p);
int newLevel = computeLevel(p, newExtentsFloat);
int oldLevel = p->getSpatialNodeLevel();
int delta = newLevel - oldLevel;
ExtentsInt32 preUpdateSpatialExtents = p->getOldSpatialExtents();
if ((delta > 0) || (delta < -1)) // grow always, shrink only if 2 steps down
{
primitiveRemoved(p);
primitiveAdded(p, true);
}
else
{
primitiveExtentsChanged(p, newExtentsFloat);
}
ExtentsInt32 postUpdateSpatialExtents = p->getOldSpatialExtents();
if (!coarseMovementCallbacks.empty() &&
(newLevel != oldLevel || preUpdateSpatialExtents != postUpdateSpatialExtents)) {
typename CoarseMovementCallback::UpdateInfo info;
info.updateType = CoarseMovementCallback::UpdateInfo::UPDATE_TYPE_Change;
info.oldLevel = oldLevel;
info.oldSpatialExtents = preUpdateSpatialExtents;
info.newLevel = newLevel;
info.newSpatialExtents = postUpdateSpatialExtents;
for (size_t i = 0; i < coarseMovementCallbacks.size(); ++i) {
coarseMovementCallbacks[i]->coarsePrimitiveMovement(p, info);
}
}
}
// Now that primitives are assembled into mechanisms we know the full topology to filter
// internal contacts. So let's query the spatial hash to create the contacts and rely on
// onNewPair() to do the filtering
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::onPrimitiveAssembled(Primitive* p)
{
if( contactManager->primitiveIsExcludedFromSpatialHash(p) )
return;
RBXASSERT_SPATIAL_HASH(validateContacts(p));
RBXASSERT_SPATIAL_HASH(p->spatialNodes != NULL);
RBXASSERT(p->getSpatialNodeLevel() != -1);
WriteValidator writeValidator(concurrencyValidator);
Extents newExtents = calcNewExtents(p);
int level = computeLevel(p, newExtents);
Vector3int32 newMin, newMax;
SpatialHashStatic::computeMinMax(level, newExtents, newMin, newMax);
for (int i = newMin.x; i <= newMax.x; ++i) {
for (int j = newMin.y; j <= newMax.y; ++j) {
for (int k = newMin.z; k <= newMax.z; ++k) {
Vector3int32 grid(i, j, k);
SpatialNode* thisNode = findNode(p, grid);
if (!thisNode)
continue;
int hash = SpatialHashStatic::getHash(level, grid);
// This hash's linked list of nodes
SpatialNode* tryNode = hashTables[level][hash].nodes;
// For nodes at the same level, and higher levels nodes containing this node
Vector3int32 g = grid;
for (int l=level; l < MAX_LEVELS; l++) {
if (l > level) {
g.x >>= 1; g.y >>= 1; g.z >>= 1;
hash = SpatialHashStatic::getHash(l, g);
tryNode = hashTables[l][hash].nodes;
}
while (tryNode) {
RBXASSERT( l < MAX_LEVELS);
Primitive* other = tryNode->primitive;
if ((other != p) && (tryNode->gridId == g)) {
if (Primitive::getContact(p, other) == NULL)
{
contactManager->onNewPair(p, other);
}
}
tryNode = tryNode->nextHashLink;
}
}
// add contacts from children
if (thisNode->getLevel() > 0) {
RBXASSERT(thisNode && thisNode->treeNode->level == thisNode->getLevel());
addContactFromChildren(thisNode->treeNode, p);
}
}
}
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesTouchingGrids(const Extents& extents,
const Primitive* ignore,
std::size_t maxCount,
boost::unordered_set<Primitive*>& answer)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
RBXASSERT(answer.size() == 0);
Vector3int32 min, max;
G3D::Array<Primitive*> foundThisGrid; // prevent allocations
for (int level=0; level<MAX_LEVELS; level++) {
if (! hashTables[level].size())
continue;
SpatialHashStatic::computeMinMax(level, extents, min, max);
for (int i = min.x; i <= max.x; ++i) {
for (int j = min.y; j <= max.y; ++j) {
for (int k = min.z; k <= max.z; ++k) {
foundThisGrid.fastClear();
getPrimitivesInGrid(level, Vector3int32(i, j, k), foundThisGrid);
for (int ff = 0; ff < foundThisGrid.size(); ++ff) {
Primitive* p = foundThisGrid[ff];
if ( (p != ignore)
&& (answer.find(p) == answer.end())
&& (extents.overlapsOrTouches(calcNewExtents(p))))
{
answer.insert(p);
//Break out early if we've hit our limit
if(maxCount > 0 && answer.size() >= maxCount)
return;
}
}
}
}
}
}
RBXASSERT(foundThisGrid.size() < 200);
}
// same as above function, but we use a set of all primitives to-be-ignored for use with an ancestor check
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesTouchingGrids(const Extents& extents,
const boost::unordered_set<const Primitive*>& ignoreSet,
std::size_t maxCount,
boost::unordered_set<Primitive*>& answer)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
RBXASSERT(answer.size() == 0);
Vector3int32 min, max;
G3D::Array<Primitive*> foundThisGrid; // prevent allocations
for (int level=0; level<MAX_LEVELS; level++) {
if (! hashTables[level].size())
continue;
SpatialHashStatic::computeMinMax(level, extents, min, max);
for (int i = min.x; i <= max.x; ++i) {
for (int j = min.y; j <= max.y; ++j) {
for (int k = min.z; k <= max.z; ++k) {
foundThisGrid.fastClear();
getPrimitivesInGrid(level, Vector3int32(i, j, k), foundThisGrid);
for (int ff = 0; ff < foundThisGrid.size(); ++ff) {
Primitive* p = foundThisGrid[ff];
if (
(answer.find(p) == answer.end())
&& (extents.overlapsOrTouches(calcNewExtents(p))))
{
// ancestor check here
if ( ignoreSet.find(p) == ignoreSet.end() ) {
answer.insert(p);
//Break out early if we've hit our limit
if(maxCount > 0 && answer.size() >= maxCount)
return;
}
}
}
}
}
}
}
RBXASSERT(foundThisGrid.size() < 200);
}
SHP template <typename Set> void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesOverlapping(const Extents& extents, Set& answer)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
for (int level=0; level<MAX_LEVELS; level++) {
if (!hashTables[level].size())
continue;
// A small negative offset is added to extentsMax before computeMinMax to prevent querying extra layer of cells for perfectly aligned/sized extents
Extents adjustedExtents(extents.min(), (extents.max() - Vector3(0.01f, 0.01f, 0.01f)).max(extents.min()));
Vector3int32 min, max;
SpatialHashStatic::computeMinMax(level, adjustedExtents, min, max);
Vector3 minReal = SpatialHashStatic::hashGridToReal(level, min);
Vector3 maxReal = SpatialHashStatic::hashGridToReal(level, max + Vector3int32(1, 1, 1));
bool skipOverlapTest = extents.contains(minReal) && extents.contains(maxReal);
for (int i = min.x; i <= max.x; ++i) {
for (int j = min.y; j <= max.y; ++j) {
for (int k = min.z; k <= max.z; ++k) {
Vector3int32 grid(i, j, k);
int hash = SpatialHashStatic::getHash(level, grid);
SpatialNode* node = hashTables[level][hash].nodes;
while (node) {
if (node->gridId == grid) {
Primitive* p = node->primitive;
if (skipOverlapTest || extents.overlapsOrTouches(calcNewExtents(p))) {
answer.insert(p);
}
}
node = node->nextHashLink;
}
}
}
}
}
}
SHP template <typename Set> void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesOverlappingRec(const Extents& extents, Set& answer)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
// A small negative offset is added to extentsMax before computeMinMax to prevent querying extra layer of cells for perfectly aligned/sized extents
Extents adjustedExtents(extents.min(), (extents.max() - Vector3(0.01f, 0.01f, 0.01f)).max(extents.min()));
int level = MAX_LEVELS - 1;
Vector3int32 min, max;
SpatialHashStatic::computeMinMax(level, adjustedExtents, min, max);
// If the tested region is contained within the extents we don't have to perform precise overlap tests
// This is very important since tests require reading Primitive memory which leads to extra cache misses.
Vector3 minReal = SpatialHashStatic::hashGridToReal(level, min);
Vector3 maxReal = SpatialHashStatic::hashGridToReal(level, max + Vector3int32(1, 1, 1));
bool skipOverlapTest = extents.contains(minReal) && extents.contains(maxReal);
for (int i = min.x; i <= max.x; ++i) {
for (int j = min.y; j <= max.y; ++j) {
for (int k = min.z; k <= max.z; ++k) {
Vector3int32 grid(i, j, k);
int hash = SpatialHashStatic::getHash(level, grid);
getPrimitivesOverlappingRec(skipOverlapTest ? NULL : &extents, answer, level, hash, grid);
}
}
}
}
SHP template <typename Set> void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesOverlappingRec(const Extents* extents, Set& answer, int level, int hash, const Vector3int32& gridCoord)
{
// Look for nodes at current level
SpatialNode* node = hashTables[level][hash].nodes;
while (node)
{
if (node->gridId == gridCoord)
{
Primitive* p = node->primitive;
if (!extents || extents->overlapsOrTouches(calcNewExtents(p)))
answer.insert(p);
}
node = node->nextHashLink;
}
// Look for nodes at the next level with smaller cells
if (level > 0)
{
TreeNode* treeNode = hashTables[level][hash].treeNodes;
while (treeNode)
{
if (treeNode->gridId == gridCoord)
{
for (int child = 0; child < 8; ++child)
if (treeNode->hasChild(child))
{
int childHash = treeNode->children[child];
Vector3int32 childGrid = getChildGrid(gridCoord, child);
getPrimitivesOverlappingRec(extents, answer, level - 1, childHash, childGrid);
}
// Just one tree node for every cell, no need to look further
return;
}
treeNode = treeNode->next;
}
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesInGrid(int level, const Vector3int32& grid, G3D::Array<Primitive*>& found)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
int hash = SpatialHashStatic::getHash(level, grid);
SpatialNode* node = hashTables[level][hash].nodes;
while (node) {
if (node->gridId == grid) {
RBXASSERT_IF_VALIDATING(!found.contains(node->primitive));
found.append(node->primitive);
}
node = node->nextHashLink;
}
}
// find primitives at all levels--the input grid coord is for level 0
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesInGrid(const Vector3int32& grid, G3D::Array<Primitive*>& found)
{
RBXASSERT(found.size() == 0);
Vector3int32 g = grid;
for (int level = 0; level < MAX_LEVELS;
level++, g.x>>=1, g.y>>=1, g.z>>=1) {
getPrimitivesInGrid(level, g, found);
}
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getNextGrid(Vector3int32& grid,
const RbxRay& unitRay,
float maxDistance)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
RBXASSERT_VERY_FAST(unitRay.direction().isUnit());
int low[3], high[3];
for (int i = 0; i < 3; ++i) {
low[i] = (unitRay.direction()[i] < 0.0) ? -1 : 0;
high[i] = (unitRay.direction()[i] > 0.0) ? 1 : 0;
}
maxDistance += SpatialHashStatic::hashGridSize(0) * 2.0f; // collision detection returns the first hit in a grid box - this could
// be farther away than the actual ultimate hit point
float maxDistanceSquared = maxDistance * maxDistance;
for (int nz = 1; nz <= 3; ++ nz) { // number of non zeros - 1: adjacent face (6), edge (12), corner (8)
for (int i = low[0]; i <= high[0]; ++i) {
for (int j = low[1]; j <= high[1]; ++j) {
for (int k = low[2]; k <= high[2]; ++k) {
if ((std::abs(i) + std::abs(j) + std::abs(k)) == nz) { // start with this number of nonzeros
Vector3int32 offset(i, j, k);
Extents extents = SpatialHashStatic::hashGridToRealExtents(0, grid + offset);
AABox box(extents.min(), extents.max());
Vector3 location;
bool inside;
bool result = G3D::CollisionDetection::collisionLocationForMovingPointFixedAABox( unitRay.origin(), unitRay.direction(), box, location, inside );
if( inside || result )
{
if ((location - unitRay.origin()).squaredMagnitude() < maxDistanceSquared) {
grid = grid + offset;
return true;
}
}
}
}}}}
return false;
}
SHP typename SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::TreeNode* SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getFirstRoot()
{
TreeNode * tn = NULL;
for(size_t id = 0; id < SpatialHashStatic::numBuckets(rootLevel) && tn == NULL; id++)
{
tn = hashTables[rootLevel][id].treeNodes;
}
return tn;
}
SHP typename SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::TreeNode* SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getNextRoot(TreeNode* prevRoot)
{
if(prevRoot)
{
if(prevRoot->next)
{
return prevRoot->next;
}
else
{
// go to next non-empty hash grid
TreeNode * tn = NULL;
for(size_t id = prevRoot->hashId+1; id < SpatialHashStatic::numBuckets(rootLevel) && tn == NULL; id++)
{
tn = hashTables[rootLevel][id].treeNodes;
}
return tn;
}
}
else
{
return NULL;
}
}
SHP typename SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::TreeNode* SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getChild(TreeNode* parent, int octant)
{
RBXASSERT(parent->hasChild(octant));
if(parent->level > 0)
{
return findTreeNode(parent->level-1, parent->children[octant], getChildGrid(parent->gridId, octant));
}
else
{
return NULL;
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::getPrimitivesInTreeNode(TreeNode* tn, G3D::Array<Primitive*>& primitives)
{
SpatialNode* node = hashTables[tn->level][tn->hashId].nodes;
while (node) {
if (node->gridId == tn->gridId) {
RBXASSERT_IF_VALIDATING(!primitives.contains(node->primitive));
primitives.append(node->primitive);
}
node = node->nextHashLink;
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::visitPrimitivesInSpace(SpaceFilter* filter, const Vector3& visitDir)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
int visitOrder[8];
SpatialHashStatic::makeVisitOrder(visitOrder, visitDir);
typedef std::pair<TreeNode*, IntersectResult> TreeNodePair;
std::vector<TreeNodePair> roots;
// get all the root nodes, sort by visitDir. (after testing for intersect)
for(TreeNode* root = getFirstRoot(); root; root = getNextRoot(root))
{
IntersectResult childIntersect = filter->Intersects(SpatialHashStatic::hashGridToRealExtents(root->level, root->gridId));
if(childIntersect == irNone)
{
continue; // no interesct at all from this node and all child nodes.
}
roots.push_back(TreeNodePair(root, childIntersect));
}
if(visitDir != Vector3::zero())
{
SortOffsetByVisitDir sortPred(visitDir);
std::sort(roots.begin(), roots.end(), sortPred);
}
for(size_t i = 0; i < roots.size(); ++i)
{
TreeNode* tn = roots[i].first;
visitPrimitivesInSpaceWorker(tn, tn->level, tn->hashId, tn->gridId, visitOrder, roots[i].second /*childintersect*/, filter, visitDir);
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::visitPrimitivesInSpace(SpaceFilter* filter)
{
ReadOnlyValidator readOnlyValidator(concurrencyValidator);
std::priority_queue<NodeInfo> nodestovisit;
bool bContinueIterating = true; // becomes false when onPrimitives returns false
// get all the root nodes, sort by visitDir. (after testing for intersect)
for(TreeNode* root = getFirstRoot(); root; root = getNextRoot(root))
{
Extents extents = SpatialHashStatic::hashGridToRealExtents(root->level, root->gridId);
IntersectResult childIntersect = filter->Intersects(extents);
if(childIntersect == irNone)
{
continue; // no interesct at all from this node and all child nodes.
}
nodestovisit.push(NodeInfo(root, aRecurseTreeNode, childIntersect, filter->Distance(extents)));
}
while(!nodestovisit.empty() && bContinueIterating)
{
NodeInfo nodeinfo(nodestovisit.top());
nodestovisit.pop();
NodeBase* node = nodeinfo.node;
int level = node->level;
int hashId = node->hashId;
const RBX::Vector3int32& gridId = node->gridId;
// check the children
if(nodeinfo.action == aRecurseTreeNode)
{
// all pushed NodeBases are TreeNode if they are not level 0.
TreeNode* tn = static_cast<TreeNode*>(node);
int childLevel = level-1;
for(int childoffset = 0; childoffset < 8; ++childoffset)
{
if(!tn->hasChild(childoffset))
{
continue; // no child, skip.
}
Vector3int32 childGridId = getChildGrid(tn->gridId, childoffset);
int childHashId = tn->children[childoffset];
IntersectResult childIntersect = irFull;
Extents childExtents = SpatialHashStatic::hashGridToRealExtents(childLevel, childGridId);
if(nodeinfo.intersectResult == irPartial) // must keep checking bounds
{
childIntersect = filter->Intersects(childExtents);
if(childIntersect == irNone)
{
continue; // no interesct at all from this node and all child nodes.
}
}
TreeNode* childTreeNode = NULL;
if(childLevel > 0)
{
childTreeNode = findTreeNode(childLevel, childHashId, childGridId);
RBXASSERT(childTreeNode);
RBXASSERT(childTreeNode->gridId == childGridId);
RBXASSERT(childTreeNode->level == childLevel);
RBXASSERT(tn->children[childoffset] == childTreeNode->hashId);
// we have childTreeNode;
nodestovisit.push(NodeInfo(childTreeNode, aRecurseTreeNode, childIntersect, filter->Distance(childExtents)));
}
else // since we don't have treenodes at the leaf level, we must "reach down" from the above level, instead of "recursing".
{
// no childTreeNode, must pass a spatialNode;
SpatialNode* snode = hashTables[childLevel][childHashId].nodes;
bool extentsNeedCalc = true;
float extentsDistance = 0;
while (snode) {
if (snode->gridId == childGridId)
{
if(extentsNeedCalc) // only calculate distance for this extents 0 or 1 times.
{
extentsDistance = filter->Distance(childExtents);
}
#ifndef PRECISE_SORTING
nodestovisit.push(NodeInfo(snode, aVisitAllSiblingsSpatialNodes, childIntersect, extentsDistance));
break;
#else
// precise sorting.
nodestovisit.push(NodeInfo(snode, aVisitSingleSpatialNode, childIntersect, extentsDistance)));
#endif
}
snode = snode->nextHashLink;
}
}
}
}
if (nodeinfo.action != aVisitSingleSpatialNode )
{
// this treenode could have sibling spatial nodes.
SpatialNode* snode = hashTables[level][hashId].nodes;
while (snode && bContinueIterating) {
if (snode->gridId == gridId)
{
#ifndef PRECISE_SORTING
bContinueIterating = filter->onPrimitive(snode->primitive, nodeinfo.intersectResult, nodeinfo.distance);
#else
nodestovisit.push(NodeInfo(snode, aVisitSingleSpatialNode, nodeinfo.intersectResult, filter->Distance(snode->primitive->getFastFuzzyExtents())));
#endif
}
snode = snode->nextHashLink;
}
}
else //if (nodeinfo.action == aVisitSingleSpatialNode)
{
bContinueIterating = filter->onPrimitive(static_cast<SpatialNode*>(node)->primitive, nodeinfo.intersectResult, nodeinfo.distance);
}
}
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::visitPrimitivesInSpaceWorker(TreeNode* tn, int level, int hashId, const RBX::Vector3int32& gridId, int* visitOrder, IntersectResult intersectResult, SpaceFilter* filter, const Vector3& visitDir)
{
RBXASSERT(intersectResult == irFull || intersectResult == irPartial);
// check the children
if(tn)
{
int childLevel = level-1;
for(int offseti = 0; offseti < 8; ++offseti)
{
int childoffset = visitOrder[offseti];
if(!tn->hasChild(childoffset))
{
continue; // no child, skip.
}
Vector3int32 childGridId = getChildGrid(tn->gridId, childoffset);
int childHashId = tn->children[childoffset];
IntersectResult childIntersect = irFull;
if(intersectResult == irPartial) // must keep checking bounds
{
childIntersect = filter->Intersects(SpatialHashStatic::hashGridToRealExtents(childLevel, childGridId));
if(childIntersect == irNone)
{
continue; // no interesct at all from this node and all child nodes.
}
}
TreeNode* childTreeNode = NULL;
if(childLevel > 0)
{
childTreeNode = findTreeNode(childLevel, childHashId, childGridId);
RBXASSERT(childTreeNode);
RBXASSERT(childTreeNode->gridId == childGridId);
RBXASSERT(childTreeNode->level == childLevel);
RBXASSERT(tn->children[childoffset] == childTreeNode->hashId);
}
visitPrimitivesInSpaceWorker(childTreeNode, childLevel, childHashId, childGridId, visitOrder, childIntersect, filter, visitDir);
}
}
// visit the primitives
SpatialNode* node = hashTables[level][hashId].nodes;
bool bContinue = true;
while (node && bContinue) {
if (node->gridId == gridId) {
bContinue = filter->onPrimitive(node->primitive, intersectResult, 0);
}
node = node->nextHashLink;
}
}
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
//
// Debugging - all of these should RBXASSERT(assertingSpatialHash)
//
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::validateInsertNodeToPrimitive(SpatialNode* node,
Primitive* p,
const Vector3int32& grid,
int hash)
{
RBXASSERT(assertingSpatialHash);
#ifdef _RBX_DEBUGGING_SPATIAL_HASH
// Primitive P's linked list of nodes
SpatialNode* oldFirst = static_cast<SpatialNode*>(p->spatialNodes);
p->spatialNodes = node;
node->nextPrimitiveLink = oldFirst;
node->prevPrimitiveLink = NULL;
if (oldFirst) {
oldFirst->prevPrimitiveLink = node;
}
p->spatialNodeCount++;
#endif
return true;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::validateRemoveNodeFromPrimitive(SpatialNode* node)
{
RBXASSERT(assertingSpatialHash);
#ifdef _RBX_DEBUGGING_SPATIAL_HASH
SpatialNode* prev = node->prevPrimitiveLink;
SpatialNode* next = node->nextPrimitiveLink;
if (next) {
next->prevPrimitiveLink = prev;
}
if (prev) {
prev->nextPrimitiveLink = next;
}
else { // !prev
node->primitive->spatialNodes = next;
}
--node->primitive->spatialNodeCount;
#endif
return true;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::validateNodesOverlap(Primitive* me, Primitive* other)
{
RBXASSERT(assertingSpatialHash);
#ifdef _RBX_DEBUGGING_SPATIAL_HASH
SpatialNode* myNode = static_cast<SpatialNode*>(me->spatialNodes);
SpatialNode* otherNode = static_cast<SpatialNode*>(other->spatialNodes);
if (!myNode || !otherNode)
return false;
if (myNode->getLevel() == otherNode->getLevel()) {
while (myNode) {
SpatialNode* hashNode = hashTables[myNode->getLevel()][myNode->hashId].nodes;
while (hashNode) {
if ((hashNode->primitive == other) && (hashNode->gridId == myNode->gridId)) {
return true;
}
hashNode = hashNode->nextHashLink;
}
myNode = myNode->nextPrimitiveLink;
}
} else {
SpatialNode *lower, *higher;
Primitive *primHigher;
if (myNode->getLevel() > otherNode->getLevel()) {
lower = otherNode;
higher = myNode;
primHigher = me;
} else {
lower = myNode;
higher = otherNode;
primHigher = other;
}
int levelDiff = higher->getLevel() - lower->getLevel();
while (lower) {
Vector3int32 gridCoordHigher;
gridCoordHigher.x = (lower->gridId.x >> levelDiff);
gridCoordHigher.y = (lower->gridId.y >> levelDiff);
gridCoordHigher.z = (lower->gridId.z >> levelDiff);
int hashHigher = getHash(higher->getLevel(), gridCoordHigher);
SpatialNode* node = hashTables[higher->getLevel()][hashHigher].nodes;
while (node) {
if ((node->primitive == primHigher) && (node->gridId == gridCoordHigher)) {
return true;
}
node = node->nextHashLink;
}
lower = lower->nextPrimitiveLink;
}
}
#endif
return false;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::validateTallyTreeNodes()
{
RBXASSERT(assertingSpatialHash);
int numTreeNodesInUse=0, numTreeNodesInPool=0;
for (int level = 0; level < MAX_LEVELS; level ++) {
for (int hashId=0; hashId<(int)SpatialHashStatic::numBuckets(level); hashId++) {
TreeNode *tn = hashTables[level][hashId].treeNodes;
while (tn) {
numTreeNodesInUse ++;
RBXASSERT(tn->childMask || tn->refByPrimitives);
tn = tn->next;
}
}
}
return numTreeNodesInUse + numTreeNodesInPool == numTreeNodesTotal;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::validateTreeNodeNotHere(TreeNode* tn, int level, int hash)
{
RBXASSERT(assertingSpatialHash);
TreeNode *p = hashTables[level][hash].treeNodes;
while (p) {
RBXASSERT(p != tn);
p = p->next;
}
return true;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::validateNoNodesOut()
{
RBXASSERT(assertingSpatialHash);
for (int l=0; l < MAX_LEVELS; l++)
{
RBXASSERT(hashTables[l].size() == SpatialHashStatic::numBuckets(l));
for (size_t i = 0; i < SpatialHashStatic::numBuckets(l); ++i)
{
RBXASSERT(hashTables[l][i].nodes == NULL);
}
}
return true;
}
SHP bool SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::validateContacts(Primitive* p)
{
RBXASSERT(assertingSpatialHash);
#ifdef _RBX_DEBUGGING_SPATIAL_HASH
__if_exists(Primitive::getFirstContact)
{
Contact* c = p->getFirstContact();
// 1. For each contact, confirm there is a hash collision
while (c) {
Primitive* other = c->otherPrimitive(p);
RBXASSERT_SPATIAL_HASH(validateNodesOverlap(p, other));
RBXASSERT_SPATIAL_HASH(validateNodesOverlap(p, other));
c = p->getNextContact(c);
}
// 2. For each node with other, confirm there is a contact
SpatialNode* myNode = static_cast<SpatialNode*>(p->spatialNodes);
while (myNode) {
SpatialNode* hashNode = hashTables[myNode->getLevel()][myNode->hashId].nodes;
while (hashNode) {
if ((hashNode->primitive != p) && (hashNode->gridId == myNode->gridId)) {
RBXASSERT_SPATIAL_HASH(Primitive::getContact(p, hashNode->primitive));
}
hashNode = hashNode->nextHashLink;
}
myNode = myNode->nextPrimitiveLink;
}
}
#endif
return true;
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::doStats() const
{
#if 0
bool operator<(const Vector3int32& a, const Vector3int32& b) {
for (int i = 0; i < 3; ++i)
{
if (a[i] < b[i]) {
return true;
}
else if (a[i] > b[i]) {
return false;
}
}
return false;
}
size_t computeNumNodes(SpatialNode* node)
{
int answer = 0;
while (node)
{
answer++;
node = node->nextHashLink;
}
return answer;
}
size_t computeNumGrids(SpatialNode* node)
{
std::set<Vector3int32> grids;
while (node)
{
grids.insert(node->gridId);
node = node->nextHashLink;
}
return grids.size();
}
// little test of hash function, not normally run
std::set<int> num_list1;
std::set<int> num_list2;
std::set<int> num_list3;
std::set<int> num_list4;
/*
for(int n = 0; n < 100000; n++)
{
// come up with some vectors using the full sample space
Vector3int32 v;
v.x = (int)((rand() << 16) ^ rand());
v.y = (int)((rand() << 16) ^ rand());
v.z = (int)((rand() << 16) ^ rand());
int key = getHash(v);
if(num_list1.find(key) == num_list1.end()) { num_list1.insert(key); continue; }
if(num_list2.find(key) == num_list2.end()) { num_list2.insert(key); continue; }
if(num_list3.find(key) == num_list3.end()) { num_list3.insert(key); continue; }
if(num_list4.find(key) == num_list4.end()) { num_list4.insert(key); continue; }
}
int u1 = num_list1.size();
int u2 = num_list2.size();
int u3 = num_list3.size();
int u4 = num_list4.size();
RBX::StandardOut::singleton()->printf(RBX::MESSAGE_INFO, "Random Hash distribution 1: %d 2: %d 3: %d 4: %d", u1, u2, u3, u4);
*/
std::vector<size_t> counts(numBuckets(), 0);
std::vector<size_t> differentGrids(numBuckets(), 0);
for (size_t i = 0; i < numBuckets(); ++i)
{
counts[i] = computeNumNodes(nodes[i]);
differentGrids[i] = computeNumGrids(nodes[i]);
}
sort(counts.begin(), counts.end());
sort(differentGrids.begin(), differentGrids.end());
RBX::StandardOut::singleton()->printf(RBX::MESSAGE_INFO, "CURRENT HASH DISTRIBUTION");
size_t numSlots = 100;
for (size_t i = 0; i < numSlots; ++i)
{
size_t index = ( (i + 1) * numBuckets() / numSlots ) - 1;
RBX::StandardOut::singleton()->printf(RBX::MESSAGE_INFO, "Slot: %d Count: %d", i, counts[index]);
}
RBX::StandardOut::singleton()->printf(RBX::MESSAGE_INFO, "TOP HASH DISTRIBUTION");
for (size_t i = 0; i < numSlots; ++i)
{
size_t index = numBuckets() - 1 - i;
RBX::StandardOut::singleton()->printf(RBX::MESSAGE_INFO, "Slot: %d Count: %d", i, counts[index]);
}
RBX::StandardOut::singleton()->printf(RBX::MESSAGE_INFO, "TOP GRID COLLISIONS DISTRIBUTION");
for (size_t i = 0; i < numSlots; ++i)
{
size_t index = numBuckets() - 1 - i;
RBX::StandardOut::singleton()->printf(RBX::MESSAGE_INFO, "Slot: %d Count: %d", i, differentGrids[index]);
}
#endif
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::registerCoarseMovementCallback(
CoarseMovementCallback* callback) {
coarseMovementCallbacks.push_back(callback);
}
SHP void SpatialHash<Primitive, Contact, ContactManager, MAX_LEVELS>::unregisterCoarseMovementCallback(
CoarseMovementCallback* callback) {
typename std::vector<CoarseMovementCallback*>::iterator itr =
std::find(coarseMovementCallbacks.begin(), coarseMovementCallbacks.end(), callback);
if (itr != coarseMovementCallbacks.end()) {
coarseMovementCallbacks.erase(itr);
}
}
} // namespace
Reference in New Issue View Git Blame Copy Permalink