yggdrasil75/stupidsimcpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixes

Browse Source
This commit is contained in:
yggdrasil75
2026-03-04 05:41:51 -05:00
parent 494ee931df
commit 83989e955e
1 changed files with 72 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
util/grid/grid3eigen.hpp
View File

@@ -212,21 +212,27 @@ private:
dirtyMeshes_.insert({objectId, subId}); dirtyMeshes_.insert({objectId, subId});
} }
void collectNodesBySubId(OctreeNode* node, int objId, int subId, std::vector<std::shared_ptr<NodeData>>& results) const { void collectNodesBySubIdRecursive(OctreeNode* node, int objId, int subId, std::vector<std::shared_ptr<NodeData>>& results, std::unordered_set<std::shared_ptr<NodeData>>& seen) const {
if (!node) return; if (!node) return;
if (node->isLeaf) { for (const auto& pt : node->points) {
for (const auto& pt : node->points) { if (pt->active && pt->objectId == objId && pt->subId == subId) {
if (pt->active && pt->objectId == objId && pt->subId == subId) { if (seen.insert(pt).second) {
results.push_back(pt); results.push_back(pt);
} }
} }
} else { }
if (!node->isLeaf) {
for (const auto& child : node->children) { for (const auto& child : node->children) {
if (child) collectNodesBySubId(child.get(), objId, subId, results); if (child) collectNodesBySubIdRecursive(child.get(), objId, subId, results, seen);
} }
} }
} }
void collectNodesBySubId(OctreeNode* node, int objId, int subId, std::vector<std::shared_ptr<NodeData>>& results) const {
std::unordered_set<std::shared_ptr<NodeData>> seen;
collectNodesBySubIdRecursive(node, objId, subId, results, seen);
}
float lodFalloffRate_ = 0.1f; // Lower = better, higher = worse. 0-1 float lodFalloffRate_ = 0.1f; // Lower = better, higher = worse. 0-1
float lodMinDistance_ = 100.0f; float lodMinDistance_ = 100.0f;
float maxDistance_ = 4096; float maxDistance_ = 4096;
@@ -319,6 +325,11 @@ private:
BoundingBox cubeBounds = pointData->getCubeBounds(); BoundingBox cubeBounds = pointData->getCubeBounds();
if (!boxIntersectsBox(node->bounds, cubeBounds)) return false; if (!boxIntersectsBox(node->bounds, cubeBounds)) return false;
if (!node->isLeaf && pointData->size >= node->nodeSize) {
node->points.emplace_back(pointData);
return true;
}
if (node->isLeaf) { if (node->isLeaf) {
node->points.emplace_back(pointData); node->points.emplace_back(pointData);
if (node->points.size() > maxPointsPerNode && depth < maxDepth) { if (node->points.size() > maxPointsPerNode && depth < maxDepth) {
@@ -329,7 +340,6 @@ private:
bool inserted = false; bool inserted = false;
for (int i = 0; i < 8; ++i) { for (int i = 0; i < 8; ++i) {
if (node->children[i] && boxIntersectsBox(node->children[i]->bounds, cubeBounds)) { if (node->children[i] && boxIntersectsBox(node->children[i]->bounds, cubeBounds)) {
size++;
inserted |= insertRecursive(node->children[i].get(), pointData, depth + 1); inserted |= insertRecursive(node->children[i].get(), pointData, depth + 1);
} }
} }
@@ -427,17 +437,16 @@ private:
std::shared_ptr<NodeData> findRecursive(OctreeNode* node, const PointType& pos, float tolerance) const { std::shared_ptr<NodeData> findRecursive(OctreeNode* node, const PointType& pos, float tolerance) const {
if (!node->contains(pos)) return nullptr; if (!node->contains(pos)) return nullptr;
if (node->isLeaf) { for (const auto& pointData : node->points) {
for (const auto& pointData : node->points) { if (!pointData->active) continue;
if (!pointData->active) continue;
float distSq = (pointData->position - pos).squaredNorm(); float distSq = (pointData->position - pos).squaredNorm();
if (distSq <= tolerance * tolerance) { if (distSq <= tolerance * tolerance) {
return pointData; return pointData;
}
} }
return nullptr; }
} else {
if (!node->isLeaf) {
int octant = getOctant(pos, node->center); int octant = getOctant(pos, node->center);
if (node->children[octant]) { if (node->children[octant]) {
return findRecursive(node->children[octant].get(), pos, tolerance); return findRecursive(node->children[octant].get(), pos, tolerance);
@@ -446,7 +455,7 @@ private:
return nullptr; return nullptr;
} }
bool removeRecursive(OctreeNode* node, const BoundingBox& bounds, const PointType& pos, float tolerance) { bool removeRecursive(OctreeNode* node, const BoundingBox& bounds, const std::shared_ptr<NodeData>& targetPt) {
{ {
std::lock_guard<std::mutex> lock(node->lodMutex); std::lock_guard<std::mutex> lock(node->lodMutex);
node->lodData = nullptr; node->lodData = nullptr;
@@ -458,21 +467,18 @@ private:
auto it = std::remove_if(node->points.begin(), node->points.end(), auto it = std::remove_if(node->points.begin(), node->points.end(),
[&](const std::shared_ptr<NodeData>& pointData) { [&](const std::shared_ptr<NodeData>& pointData) {
if (!pointData->active) return false; return pointData == targetPt;
float distSq = (pointData->position - pos).squaredNorm();
return (distSq <= tolerance * tolerance);
}); });
if (it != node->points.end()) { if (it != node->points.end()) {
node->points.erase(it, node->points.end()); node->points.erase(it, node->points.end());
size--;
foundAny = true; foundAny = true;
} }
if (!node->isLeaf) { if (!node->isLeaf) {
for (int i = 0; i < 8; ++i) { for (int i = 0; i < 8; ++i) {
if (node->children[i]) { if (node->children[i]) {
foundAny |= removeRecursive(node->children[i].get(), bounds, pos, tolerance); foundAny |= removeRecursive(node->children[i].get(), bounds, targetPt);
} }
} }
} }
@@ -545,10 +551,9 @@ private:
} }
} }
} }
// DDA Traversal // DDA Traversal
PointType center = node->center; PointType center = node->center;
// Calculate plane intersections relative to center
Eigen::Vector3f ttt = (center - ray.origin).cwiseProduct(ray.invDir); Eigen::Vector3f ttt = (center - ray.origin).cwiseProduct(ray.invDir);
int currIdx = 0; int currIdx = 0;
@@ -765,10 +770,11 @@ private:
if (node->lodData) lodGeneratedNodes++; if (node->lodData) lodGeneratedNodes++;
size_t pts = node->points.size();
actualPoints += pts;
if (node->isLeaf) { if (node->isLeaf) {
leafNodes++; leafNodes++;
size_t pts = node->points.size();
actualPoints += pts;
maxPointsInLeaf = std::max(maxPointsInLeaf, pts); maxPointsInLeaf = std::max(maxPointsInLeaf, pts);
minPointsInLeaf = std::min(minPointsInLeaf, pts); minPointsInLeaf = std::min(minPointsInLeaf, pts);
} else { } else {
@@ -851,20 +857,21 @@ private:
void serializeNode(std::ofstream& out, const OctreeNode* node) const { void serializeNode(std::ofstream& out, const OctreeNode* node) const {
writeVal(out, node->isLeaf); writeVal(out, node->isLeaf);
if (node->isLeaf) { // ALWAYS serialize points, unconditionally
size_t pointCount = node->points.size(); size_t pointCount = node->points.size();
writeVal(out, pointCount); writeVal(out, pointCount);
for (const auto& pt : node->points) { for (const auto& pt : node->points) {
writeVal(out, pt->data); writeVal(out, pt->data);
writeVec3(out, pt->position); writeVec3(out, pt->position);
writeVal(out, pt->objectId); writeVal(out, pt->objectId);
writeVal(out, pt->active); writeVal(out, pt->active);
writeVal(out, pt->visible); writeVal(out, pt->visible);
writeVal(out, pt->size); writeVal(out, pt->size);
writeVal(out, pt->colorIdx); writeVal(out, pt->colorIdx);
writeVal(out, pt->materialIdx); writeVal(out, pt->materialIdx);
} }
} else {
if (!node->isLeaf) {
// Write bitmask of active children // Write bitmask of active children
uint8_t childMask = 0; uint8_t childMask = 0;
for (int i = 0; i < 8; ++i) { for (int i = 0; i < 8; ++i) {
@@ -1071,24 +1078,30 @@ private:
return randomDir; return randomDir;
} }
void collectNodesByObjectId(OctreeNode* node, int id, std::vector<std::shared_ptr<NodeData>>& results) const { void collectNodesByObjectIdRecursive(OctreeNode* node, int id, std::vector<std::shared_ptr<NodeData>>& results, std::unordered_set<std::shared_ptr<NodeData>>& seen) const {
if (!node) return; if (!node) return;
if (node->isLeaf) { for (const auto& pt : node->points) {
for (const auto& pt : node->points) { if (pt->active && (id == -1 || pt->objectId == id)) {
if (pt->active && (id == -1 || pt->objectId == id)) { if (seen.insert(pt).second) {
results.push_back(pt); results.push_back(pt);
} }
} }
} else { }
if (!node->isLeaf) {
for (const auto& child : node->children) { for (const auto& child : node->children) {
if (child) { if (child) {
collectNodesByObjectId(child.get(), id, results); collectNodesByObjectIdRecursive(child.get(), id, results, seen);
} }
} }
} }
} }
void collectNodesByObjectId(OctreeNode* node, int id, std::vector<std::shared_ptr<NodeData>>& results) const {
std::unordered_set<std::shared_ptr<NodeData>> seen;
collectNodesByObjectIdRecursive(node, id, results, seen);
}
struct GridCell { struct GridCell {
std::array<PointType, 8> p; std::array<PointType, 8> p;
std::array<float, 8> val; std::array<float, 8> val;
@@ -1264,6 +1277,7 @@ public:
auto pointData = std::make_shared<NodeData>(data, pos, visible, cIdx, size, active, objectId, subId, mIdx); auto pointData = std::make_shared<NodeData>(data, pos, visible, cIdx, size, active, objectId, subId, mIdx);
if (insertRecursive(root_.get(), pointData, 0)) { if (insertRecursive(root_.get(), pointData, 0)) {
this->size++;
return true; return true;
} }
return false; return false;
@@ -1381,7 +1395,11 @@ public:
bool remove(const PointType& pos, float tolerance = EPSILON) { bool remove(const PointType& pos, float tolerance = EPSILON) {
auto pt = find(pos, tolerance); auto pt = find(pos, tolerance);
if (!pt) return false; if (!pt) return false;
return removeRecursive(root_.get(), pt->getCubeBounds(), pos, tolerance); if (removeRecursive(root_.get(), pt->getCubeBounds(), pt)) {
size--;
return true;
}
return false;
} }
std::vector<std::shared_ptr<NodeData>> findInRadius(const PointType& center, float radius, int objectid = -1) const { std::vector<std::shared_ptr<NodeData>> findInRadius(const PointType& center, float radius, int objectid = -1) const {
@@ -1424,7 +1442,7 @@ public:
} }
} }
removeRecursive(root_.get(), pointData->getCubeBounds(), oldPos, tolerance); removeRecursive(root_.get(), pointData->getCubeBounds(), pointData);
pointData->data = newData; pointData->data = newData;
pointData->position = newPos; pointData->position = newPos;
@@ -1465,6 +1483,8 @@ public:
if(res) { if(res) {
invalidateMesh(targetObjId, finalSubId); invalidateMesh(targetObjId, finalSubId);
if(finalSubId != oldSubId) invalidateMesh(targetObjId, oldSubId); if(finalSubId != oldSubId) invalidateMesh(targetObjId, oldSubId);
} else {
size--;
} }
return res; return res;
@@ -1474,12 +1494,13 @@ public:
auto pointData = find(pos); auto pointData = find(pos);
if (!pointData) return false; if (!pointData) return false;
removeRecursive(root_.get(), pointData->getCubeBounds(), pos, EPSILON); removeRecursive(root_.get(), pointData->getCubeBounds(), pointData);
pointData->position = newPos; pointData->position = newPos;
if (insertRecursive(root_.get(), pointData, 0)) { if (insertRecursive(root_.get(), pointData, 0)) {
return true; return true;
} }
size--;
return false; return false;
} }