From 82300062f81a3a97c1c67e75fd0972c9b101d858 Mon Sep 17 00:00:00 2001 From: gianpaolopalma Date: Wed, 16 Nov 2016 11:24:12 +0100 Subject: [PATCH] Added a new implementation of the kdtree for faces --- vcg/space/index/kdtree/kdtree_face.h | 300 +++++++++++++++++++++++++++ 1 file changed, 300 insertions(+) create mode 100644 vcg/space/index/kdtree/kdtree_face.h diff --git a/vcg/space/index/kdtree/kdtree_face.h b/vcg/space/index/kdtree/kdtree_face.h new file mode 100644 index 00000000..3fc373ee --- /dev/null +++ b/vcg/space/index/kdtree/kdtree_face.h @@ -0,0 +1,300 @@ +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2004-2016 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ + +#ifndef KDTREE_FACE_H +#define KDTREE_FACE_H + +#include +#include + +namespace vcg { + + /** + * This class allows to create a Kd-Tree thought to perform the neighbour query using the mesh faces (closest search). + * The class implemetantion is thread-safe. + */ + template + class KdTreeFace + { + public: + + typedef typename MeshType::ScalarType Scalar; + typedef typename MeshType::CoordType VectorType; + typedef typename MeshType::BoxType AxisAlignedBoxType; + typedef typename MeshType::FacePointer FacePointer; + + class Node + { + public: + Scalar splitValue; + unsigned int firstChildId : 24; + unsigned int dim : 2; + unsigned int leaf : 1; + AxisAlignedBoxType aabb; + std::vector list; + }; + typedef std::vector NodeListPointer; + + public: + + KdTreeFace(MeshType& mesh, unsigned int maxObjPerCell = 64, unsigned int maxDepth = 64) : epsilon(std::numeric_limits::epsilon()) + { + targetCellSize = maxObjPerCell; + targetMaxDepth = maxDepth; + mNodes.resize(1); + Node& node = mNodes.back(); + node.leaf = 0; + node.aabb = mesh.bbox; + node.aabb.Offset(VectorType(epsilon, epsilon, epsilon)); + for (int i = 0; i < mesh.face.size(); i++) + node.list.push_back(&mesh.face[i]); + numLevel = createTree(0, 1); + }; + + ~KdTreeFace() + { + + }; + + + template FacePointer doQueryClosest(const VectorType& queryPoint, VectorType& narestPoint, Scalar& dist, ObjectMarker& marker, Scalar maxDist = std::numeric_limits::max()) + { + if (maxDist < std::numeric_limits::max() && !mNodes[0].aabb.IsIn(queryPoint) && vcg::PointFilledBoxDistance(queryPoint, mNodes[0].aabb) >= maxDist) + { + dist = maxDist; + return NULL; + } + std::vector mNodeStack(numLevel + 1); + mNodeStack[0].nodeId = 0; + mNodeStack[0].sq = 0.f; + unsigned int count = 1; + + Scalar minDist = maxDist; + VectorType p; + FacePointer face = NULL; + while (count) + { + QueryNode& qnode = mNodeStack[count - 1]; + Node& node = mNodes[qnode.nodeId]; + + if (qnode.sq < minDist) + { + if (node.leaf) + { + --count; // pop + for (int i = 0; i < node.list.size(); i++) + { + if (!marker.IsMarked(node.list[i])) + { + marker.Mark(node.list[i]); + Scalar tempDist = minDist; + VectorType tempP; + if (vcg::face::PointDistanceBase(*node.list[i], queryPoint, tempDist, tempP)) + { + if (tempDist < minDist) + { + minDist = tempDist; + p = tempP; + face = node.list[i]; + } + } + } + } + } + else + { + // replace the stack top by the farthest and push the closest + float new_off = queryPoint[node.dim] - node.splitValue; + float abs_off = abs(new_off); + if (abs_off < minDist) + { + if (new_off < 0.) + { + mNodeStack[count].nodeId = node.firstChildId; + qnode.nodeId = node.firstChildId + 1; + new_off = vcg::PointFilledBoxDistance(queryPoint, mNodes[node.firstChildId + 1].aabb); + } + else + { + mNodeStack[count].nodeId = node.firstChildId + 1; + qnode.nodeId = node.firstChildId; + new_off = vcg::PointFilledBoxDistance(queryPoint, mNodes[node.firstChildId].aabb); + } + mNodeStack[count].sq = qnode.sq; + qnode.sq = new_off; + ++count; + } + else + { + if (new_off < 0.) + qnode.nodeId = node.firstChildId; + else + qnode.nodeId = node.firstChildId + 1; + } + } + } + else + { + // pop + --count; + } + } + dist = minDist; + narestPoint = p; + return face; + } + + protected: + + // element of the stack + struct QueryNode + { + QueryNode() {} + QueryNode(unsigned int id) : nodeId(id) {} + unsigned int nodeId; // id of the next node + Scalar sq; // distance to the next node + }; + + + int createTree(unsigned int nodeId, unsigned int level) + { + Node& node = mNodes[nodeId]; + VectorType diag = node.aabb.max - node.aabb.min; + unsigned int dim; + if (diag.X() > diag.Y()) + dim = diag.X() > diag.Z() ? 0 : 2; + else + dim = diag.Y() > diag.Z() ? 1 : 2; + + node.splitValue = Scalar(0.5*(node.aabb.max[dim] + node.aabb.min[dim])); + node.dim = dim; + + AxisAlignedBoxType leftBox, rightBox; + leftBox.Add(node.aabb.min); + rightBox.Add(node.aabb.max); + if (node.dim == 0) + { + leftBox.Add(VectorType(node.splitValue, node.aabb.max[1], node.aabb.max[2])); + rightBox.Add(VectorType(node.splitValue, node.aabb.min[1], node.aabb.min[2])); + } + else if (node.dim == 1) + { + leftBox.Add(VectorType(node.aabb.max[0], node.splitValue, node.aabb.max[2])); + rightBox.Add(VectorType(node.aabb.min[0], node.splitValue, node.aabb.min[2])); + } + else if (node.dim == 2) + { + leftBox.Add(VectorType(node.aabb.max[0], node.aabb.max[1], node.splitValue)); + rightBox.Add(VectorType(node.aabb.min[0], node.aabb.min[1], node.splitValue)); + } + leftBox.Offset(VectorType(epsilon, epsilon, epsilon)); + rightBox.Offset(VectorType(epsilon, epsilon, epsilon)); + + node.firstChildId = mNodes.size(); + int firstChildId = node.firstChildId; + mNodes.resize(mNodes.size() + 2); + Node& parent = mNodes[nodeId]; + Node& leftChild = mNodes[firstChildId]; + Node& rightChild = mNodes[firstChildId + 1]; + leftChild.aabb.SetNull(); + rightChild.aabb.SetNull(); + + for (int i = 0; i < parent.list.size(); i++) + { + unsigned int state = 0; + FacePointer fp = parent.list[i]; + for (int j = 0; j < 3; j++) + { + if (fp->P(j)[dim] < parent.splitValue) + state |= (1 << 0); + else if (fp->P(j)[dim] > parent.splitValue) + state |= (1 << 1); + else + { + state |= (1 << 0); + state |= (1 << 1); + } + } + if (state & (1 << 0)) + { + leftChild.list.push_back(fp); + leftChild.aabb.Add(fp->P(0)); + leftChild.aabb.Add(fp->P(1)); + leftChild.aabb.Add(fp->P(2)); + } + if (state & (1 << 1)) + { + rightChild.list.push_back(fp); + rightChild.aabb.Add(fp->P(0)); + rightChild.aabb.Add(fp->P(1)); + rightChild.aabb.Add(fp->P(2)); + } + } + parent.list.clear(); + leftChild.aabb.Intersect(leftBox); + rightChild.aabb.Intersect(rightBox); + + int leftLevel, rightLevel; + { + if (leftChild.list.size() <= targetCellSize || level >= targetMaxDepth) + { + leftChild.leaf = 1; + leftLevel = level; + } + else + { + leftChild.leaf = 0; + leftLevel = createTree(firstChildId, level + 1); + } + } + + { + Node& rightChild = mNodes[firstChildId + 1]; + if (rightChild.list.size() <= targetCellSize || level >= targetMaxDepth) + { + rightChild.leaf = 1; + rightLevel = level; + } + else + { + rightChild.leaf = 0; + rightLevel = createTree(firstChildId + 1, level + 1); + } + } + if (leftLevel > rightLevel) + return leftLevel; + return rightLevel; + }; + + + protected: + + NodeListPointer mNodes; //kd-tree nodes + unsigned int numLevel; + const Scalar epsilon; + unsigned int targetCellSize; + unsigned int targetMaxDepth; + }; + +} +#endif