diff --git a/src/Plugins/OrientationAnalysis/CMakeLists.txt b/src/Plugins/OrientationAnalysis/CMakeLists.txt index 5e92d576a2..298d58260d 100644 --- a/src/Plugins/OrientationAnalysis/CMakeLists.txt +++ b/src/Plugins/OrientationAnalysis/CMakeLists.txt @@ -243,6 +243,10 @@ set(PLUGIN_EXTRA_SOURCES "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities/SIMPLConversion.hpp" "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities/TiffWriter.hpp" "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities/TiffWriter.cpp" + "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities/delaunator.cpp" + "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities/delaunator.h" + "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities/RTree.hpp" + "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities/IntersectionUtilities.hpp" ) target_sources(${PLUGIN_NAME} PRIVATE ${PLUGIN_EXTRA_SOURCES}) source_group(TREE "${${PLUGIN_NAME}_SOURCE_DIR}/src/${PLUGIN_NAME}/utilities" PREFIX ${PLUGIN_NAME} FILES ${PLUGIN_EXTRA_SOURCES}) diff --git a/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/Filters/Algorithms/WritePoleFigure.cpp b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/Filters/Algorithms/WritePoleFigure.cpp index e1f4917077..bbf96c0c8c 100644 --- a/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/Filters/Algorithms/WritePoleFigure.cpp +++ b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/Filters/Algorithms/WritePoleFigure.cpp @@ -2,16 +2,22 @@ #include "OrientationAnalysis/utilities/FiraSansRegular.hpp" #include "OrientationAnalysis/utilities/Fonts.hpp" +#include "OrientationAnalysis/utilities/IntersectionUtilities.hpp" #include "OrientationAnalysis/utilities/LatoBold.hpp" #include "OrientationAnalysis/utilities/LatoRegular.hpp" +#include "OrientationAnalysis/utilities/RTree.hpp" #include "OrientationAnalysis/utilities/TiffWriter.hpp" +#include "OrientationAnalysis/utilities/delaunator.h" #include "simplnx/Common/Constants.hpp" #include "simplnx/Common/RgbColor.hpp" #include "simplnx/DataStructure/DataArray.hpp" #include "simplnx/DataStructure/Geometry/ImageGeom.hpp" +#include "simplnx/DataStructure/Geometry/TriangleGeom.hpp" +#include "simplnx/Pipeline/Pipeline.hpp" #include "simplnx/Utilities/DataArrayUtilities.hpp" #include "simplnx/Utilities/ParallelTaskAlgorithm.hpp" +#include "simplnx/Utilities/Parsing/DREAM3D/Dream3dIO.hpp" #include "simplnx/Utilities/StringUtilities.hpp" #include "EbsdLib/Core/EbsdLibConstants.h" @@ -26,8 +32,13 @@ #include "EbsdLib/LaueOps/TriclinicOps.h" #include "EbsdLib/LaueOps/TrigonalLowOps.h" #include "EbsdLib/LaueOps/TrigonalOps.h" +#include "EbsdLib/Utilities/LambertUtilities.h" #include "EbsdLib/Utilities/ModifiedLambertProjection.h" +#include "H5Support/H5Lite.h" +#include "H5Support/H5ScopedSentinel.h" +#include "H5Support/H5Utilities.h" + #define CANVAS_ITY_IMPLEMENTATION #include @@ -86,7 +97,305 @@ class ComputeIntensityStereographicProjection lambert->normalizeSquaresToMRD(); } lambert->createStereographicProjection(m_Config->imageDim, *m_Intensity); + + // This next function (writeLambertData) is experimental but should be left in to + // make sure it will still compile. At some point I will get back to the development + // of this alternate pole figure generation. + // writeLambertData(lambert); + } + } + + template + void unstructuredGridInterpolator(nx::core::IFilter* filter, nx::core::TriangleGeom* delaunayGeom, std::vector& xPositionsPtr, std::vector& yPositionsPtr, T* xyValues, + typename std::vector& outputValues) const + { + using Vec3f = nx::core::Vec3; + using RTreeType = RTree; + + // filter->notifyStatusMessage(QString("Starting Interpolation....")); + nx::core::IGeometry::SharedFaceList& delTriangles = delaunayGeom->getFacesRef(); + size_t numTriangles = delaunayGeom->getNumberOfFaces(); + int percent = 0; + int counter = xPositionsPtr.size() / 100; + RTreeType m_RTree; + // Populate the RTree + + size_t numTris = delaunayGeom->getNumberOfFaces(); + for(size_t tIndex = 0; tIndex < numTris; tIndex++) + { + std::array boundBox = nx::IntersectionUtilities::GetBoundingBoxAtTri(*delaunayGeom, tIndex); + m_RTree.Insert(boundBox.data(), boundBox.data() + 3, tIndex); // Note, all values including zero are fine in this version + } + + for(size_t vertIndex = 0; vertIndex < xPositionsPtr.size(); vertIndex++) + { + Vec3f rayOrigin(xPositionsPtr[vertIndex], yPositionsPtr[vertIndex], 1.0F); + Vec3f rayDirection(0.0F, 0.0F, -1.0F); + Vec3f barycentricCoord(0.0F, 0.0F, 0.0F); + // int xPos = xPositionsPtr[vertIndex]; + // int yPos = yPositionsPtr[vertIndex]; + if(counter != 0) + { + + if(vertIndex % counter == 0) + { + // QString ss = QObject::tr("Interpolating || %1% Complete").arg(percent); + // filter->notifyStatusMessage(ss); + percent += 1; + } + } + + // Create these reusable variables to save the reallocation each time through the loop + + std::vector hitTriangleIds; + std::function func = [&](size_t id) { + hitTriangleIds.push_back(id); + return true; // keep going + }; + + int nhits = m_RTree.Search(rayOrigin.data(), rayOrigin.data(), func); + for(auto triIndex : hitTriangleIds) + { + barycentricCoord = {0.0F, 0.0F, 0.0F}; + std::array triVertIndices; + // Get the Vertex Coordinates for each of the 3 vertices + std::array verts; + delaunayGeom->getFaceCoordinates(triIndex, verts); + Vec3f v0 = verts[0]; + Vec3f v1 = verts[1]; + Vec3f v2 = verts[2]; + + // Get the vertex Indices from the triangle + delaunayGeom->getFacePointIds(triIndex, triVertIndices); + bool inTriangle = nx::IntersectionUtilities::RayTriangleIntersect2(rayOrigin, rayDirection, v0, v1, v2, barycentricCoord); + if(inTriangle) + { + // Linear Interpolate dx and dy values using the barycentric coordinates + delaunayGeom->getFaceCoordinates(triIndex, verts); + float f0 = xyValues[triVertIndices[0]]; + float f1 = xyValues[triVertIndices[1]]; + float f2 = xyValues[triVertIndices[2]]; + + float interpolatedVal = (barycentricCoord[0] * f0) + (barycentricCoord[1] * f1) + (barycentricCoord[2] * f2); + + outputValues[vertIndex] = interpolatedVal; + + break; + } + } + } + } + + int writeLambertData(ModifiedLambertProjection::Pointer lambert) const + { + int err = -1; + + int m_Dimension = lambert->getDimension(); + EbsdLib::DoubleArrayType::Pointer m_NorthSquare = lambert->getNorthSquare(); + EbsdLib::DoubleArrayType::Pointer m_SouthSquare = lambert->getSouthSquare(); + + // We want half the sphere area for each square because each square represents a hemisphere. + const float32 sphereRadius = 1.0f; + float halfSphereArea = 4.0f * EbsdLib::Constants::k_PiF * sphereRadius * sphereRadius / 2.0f; + // The length of a side of the square is the square root of the area + float squareEdge = std::sqrt(halfSphereArea); + float32 m_StepSize = squareEdge / static_cast(m_Dimension); + + float32 m_MaxCoord = squareEdge / 2.0f; + float32 m_MinCoord = -squareEdge / 2.0f; + std::array vert = {0.0f, 0.0f, 0.0f}; + + std::vector squareCoords(m_Dimension * m_Dimension * 3); + + // Northern Hemisphere Coordinates + std::vector northSphereCoords(m_Dimension * m_Dimension * 3); + std::vector northStereoCoords(m_Dimension * m_Dimension * 3); + + // Southern Hemisphere Coordinates + std::vector southSphereCoords(m_Dimension * m_Dimension * 3); + std::vector southStereoCoords(m_Dimension * m_Dimension * 3); + + size_t index = 0; + + const float origin = m_MinCoord + (m_StepSize / 2.0f); + for(int32_t y = 0; y < m_Dimension; ++y) + { + for(int x = 0; x < m_Dimension; ++x) + { + vert[0] = origin + (static_cast(x) * m_StepSize); + vert[1] = origin + (static_cast(y) * m_StepSize); + + squareCoords[index * 3] = vert[0]; + squareCoords[index * 3 + 1] = vert[1]; + squareCoords[index * 3 + 2] = 0.0f; + + LambertUtilities::LambertSquareVertToSphereVert(vert.data(), LambertUtilities::Hemisphere::North); + + northSphereCoords[index * 3] = vert[0]; + northSphereCoords[index * 3 + 1] = vert[1]; + northSphereCoords[index * 3 + 2] = vert[2]; + + northStereoCoords[index * 3] = vert[0] / (1.0f + vert[2]); + northStereoCoords[index * 3 + 1] = vert[1] / (1.0f + vert[2]); + northStereoCoords[index * 3 + 2] = 0.0f; + + // Reset the Lambert Square Coord + vert[0] = origin + (static_cast(x) * m_StepSize); + vert[1] = origin + (static_cast(y) * m_StepSize); + LambertUtilities::LambertSquareVertToSphereVert(vert.data(), LambertUtilities::Hemisphere::South); + + southSphereCoords[index * 3] = vert[0]; + southSphereCoords[index * 3 + 1] = vert[1]; + southSphereCoords[index * 3 + 2] = vert[2]; + + southStereoCoords[index * 3] = vert[0] / (1.0f + vert[2]); + southStereoCoords[index * 3 + 1] = vert[1] / (1.0f + vert[2]); + southStereoCoords[index * 3 + 2] = 0.0f; + + index++; + } + } + + //********************************************************************************************************************** + // Triangulate the stereo coordinates + + DataStructure dataStructure; + + auto* triangleGeomPtr = TriangleGeom::Create(dataStructure, "Delaunay"); + auto& triangleGeom = *triangleGeomPtr; + + DataPath sharedFaceListPath({"Delaunay", "SharedFaceList"}); + + usize numPts = northStereoCoords.size() / 3; + // Create the default DataArray that will hold the FaceList and Vertices. We + // size these to 1 because the Csv parser will resize them to the appropriate number of tuples + using DimensionType = std::vector; + + DimensionType faceTupleShape = {0}; + Result result = CreateArray(dataStructure, faceTupleShape, {3ULL}, sharedFaceListPath, IDataAction::Mode::Execute); + if(result.invalid()) + { + return -1; + // return MergeResults(result, MakeErrorResult(-5509, fmt::format("{}CreateGeometry2DAction: Could not allocate SharedTriList '{}'", prefix, trianglesPath.toString()))); + } + auto& sharedFaceListRef = dataStructure.getDataRefAs(sharedFaceListPath); + triangleGeom.setFaceList(sharedFaceListRef); + + // Create the Vertex Array with a component size of 3 + DataPath vertexPath({"Delaunay", "SharedVertexList"}); + + DimensionType vertexTupleShape = {0}; + result = CreateArray(dataStructure, vertexTupleShape, {3}, vertexPath, IDataAction::Mode::Execute); + if(result.invalid()) + { + return -2; + // return MergeResults(result, MakeErrorResult(-5510, fmt::format("{}CreateGeometry2DAction: Could not allocate SharedVertList '{}'", prefix, vertexPath.toString()))); } + Float32Array* vertexArray = ArrayFromPath(dataStructure, vertexPath); + triangleGeom.setVertices(*vertexArray); + triangleGeom.resizeVertexList(numPts); + + auto* vertexAttributeMatrix = AttributeMatrix::Create(dataStructure, "VertexData", {numPts}, triangleGeom.getId()); + if(vertexAttributeMatrix == nullptr) + { + return -3; + // return MakeErrorResult(-5512, fmt::format("CreateGeometry2DAction: Failed to create attribute matrix: '{}'", prefix, vertexDataPath.toString())); + } + triangleGeom.setVertexAttributeMatrix(*vertexAttributeMatrix); + + if(nullptr != triangleGeom.getVertexAttributeMatrix()) + { + triangleGeom.getVertexAttributeMatrix()->resizeTuples({numPts}); + } + auto vertexCoordsPtr = triangleGeom.getVerticesRef(); + + // Create Coords for the Delaunator Algorithm + std::vector coords(2 * numPts, 0.0); + for(usize i = 0; i < numPts; i++) + { + coords[i * 2] = northStereoCoords[i * 3]; + coords[i * 2 + 1] = northStereoCoords[i * 3 + 1]; + vertexCoordsPtr[i * 3] = northStereoCoords[i * 3]; + vertexCoordsPtr[i * 3 + 1] = northStereoCoords[i * 3 + 1]; + vertexCoordsPtr[i * 3 + 2] = northStereoCoords[i * 3 + 2]; + } + + // Perform the triangulation + nx::delaunator::Delaunator d(coords); + + usize numTriangles = d.triangles.size(); + triangleGeom.resizeFaceList(numTriangles / 3); + auto sharedTriListPtr = triangleGeom.getFacesRef(); + // usize triangleIndex = 0; + for(usize i = 0; i < numTriangles; i += 3) + { + std::array triIDs = {d.triangles[i], d.triangles[i + 1], d.triangles[i + 2]}; + sharedTriListPtr[i] = d.triangles[i]; + sharedTriListPtr[i + 1] = d.triangles[i + 1]; + sharedTriListPtr[i + 2] = d.triangles[i + 2]; + } + + // Create the vertex and face AttributeMatrix + auto* faceAttributeMatrix = AttributeMatrix::Create(dataStructure, "Face Data", {numTriangles / 3}, triangleGeom.getId()); + if(faceAttributeMatrix == nullptr) + { + return -4; + // return MakeErrorResult(-5511, fmt::format("{}CreateGeometry2DAction: Failed to create attribute matrix: '{}'", prefix, faceDataPath.toString())); + } + triangleGeom.setFaceAttributeMatrix(*faceAttributeMatrix); + + Pipeline pipeline; + const Result<> result2 = DREAM3D::WriteFile(fmt::format("/tmp/delaunay_triangulation.dream3d"), dataStructure, pipeline, true); + //****************************************************************************************************************************** + + //****************************************************************************************************************************** + // Perform a Bi-linear Interpolation + // Generate a regular grid of XY points + size_t numSteps = 1024; + float32 stepInc = 2.0f / static_cast(numSteps); + std::vector xcoords(numSteps * numSteps); + std::vector ycoords(numSteps * numSteps); + for(size_t y = 0; y < numSteps; ++y) + { + for(size_t x = 0; x < numSteps; x++) + { + size_t idx = y * numSteps + x; + xcoords[idx] = -1.0f + static_cast(x) * stepInc; + ycoords[idx] = -1.0f + static_cast(y) * stepInc; + } + } + + std::vector outputValues(numSteps * numSteps); + unstructuredGridInterpolator(nullptr, &triangleGeom, xcoords, ycoords, m_NorthSquare->data(), outputValues); + + //****************************************************************************************************************************** + + //****************************************************************************************************************************** + // Write out all the data + { + hid_t groupId = H5Support::H5Utilities::createFile("/tmp/lambert_data.h5"); + H5Support::H5ScopedFileSentinel fileSentinel(groupId, false); + + std::vector dims = {static_cast(m_Dimension), static_cast(m_Dimension)}; + err = H5Support::H5Lite::writePointerDataset(groupId, m_NorthSquare->getName(), 2, dims.data(), m_NorthSquare->data()); + err = H5Support::H5Lite::writePointerDataset(groupId, m_SouthSquare->getName(), 2, dims.data(), m_SouthSquare->data()); + dims[0] = m_Dimension * m_Dimension; + dims[1] = 3ULL; + + err = H5Support::H5Lite::writePointerDataset(groupId, "Lambert Square Coords", 2, dims.data(), squareCoords.data()); + err = H5Support::H5Lite::writePointerDataset(groupId, "North Sphere Coords", 2, dims.data(), northSphereCoords.data()); + err = H5Support::H5Lite::writePointerDataset(groupId, "North Stereo Coords", 2, dims.data(), northStereoCoords.data()); + + err = H5Support::H5Lite::writePointerDataset(groupId, "South Sphere Coords", 2, dims.data(), southSphereCoords.data()); + err = H5Support::H5Lite::writePointerDataset(groupId, "South Stereo Coords", 2, dims.data(), southStereoCoords.data()); + + dims[0] = numSteps * numSteps; + err = H5Support::H5Lite::writePointerDataset(groupId, "X Coords", 1, dims.data(), xcoords.data()); + err = H5Support::H5Lite::writePointerDataset(groupId, "Y Coords", 1, dims.data(), ycoords.data()); + err = H5Support::H5Lite::writePointerDataset(groupId, "Interpolated Values", 1, dims.data(), outputValues.data()); + } + + return err; } private: diff --git a/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/IntersectionUtilities.hpp b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/IntersectionUtilities.hpp new file mode 100644 index 0000000000..527270fbfc --- /dev/null +++ b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/IntersectionUtilities.hpp @@ -0,0 +1,222 @@ +#pragma once + +#include "simplnx/Common/Constants.hpp" +#include "simplnx/DataStructure/Geometry/IGeometry.hpp" +#include "simplnx/DataStructure/Geometry/TriangleGeom.hpp" + +#include +namespace nx +{ +namespace IntersectionUtilities +{ +constexpr float k_Epsilon = 1e-8; +constexpr uint32_t width = 310; +constexpr uint32_t height = 150; + +/** + *@brief Bilinear Interpolator implements function to execute a 2D interpolation + */ +using Vec3f = nx::core::Vec3; + +inline float deg2rad(const float& deg) +{ + return deg * nx::core::Constants::k_PiOver180; +} + +inline float clamp(const float& lo, const float& hi, const float& v) +{ + return std::max(lo, std::min(hi, v)); +} + +inline std::array GetBoundingBoxAtTri(nx::core::TriangleGeom& tri, size_t triId) +{ + nx::core::IGeometry::SharedTriList& triList = tri.getFacesRef(); + // nx::core::IGeometry::SharedVertexList& vertList = tri.getVerticesRef(); + + nx::core::uint64* Tri = &triList[triId]; + std::array triCoords; + tri.getFaceCoordinates(triId, triCoords); + nx::core::Point3Df vert1 = triCoords[0]; + nx::core::Point3Df vert2 = triCoords[1]; + nx::core::Point3Df vert3 = triCoords[2]; + // nx::core::float32* vert1 = &vertList[Tri[0] * 3]; + // nx::core::float32* vert2 = &vertList[Tri[1]*3+1]; + // nx::core::float32* vert3 = &vertList[Tri[2]*3+2]; + auto xMinMax = std::minmax({vert1[0], vert2[0], vert3[0]}); + auto yMinMax = std::minmax({vert1[1], vert2[1], vert3[1]}); + auto zMinMax = std::minmax({vert1[2], vert2[2], vert3[2]}); + return {xMinMax.first, yMinMax.first, zMinMax.first, xMinMax.second, yMinMax.second, zMinMax.second}; +} + +/** + * @brief This version of the rayTriangleIntersect algorithm uses a more traditional algorithm to + * determine if a point is inside a triangle. This version should be more computationally + * efficient than the other version + * @param rayOrigin + * @param rayDirection + * @param v0 First Vertex of Triangle + * @param v1 Second Vertex of Triangle + * @param v2 Third Vertex of Triangle + * @param t t part of Barycentric Coord + * @param u u part of Barycentric Coord + * @param v v part of Barycentric Coord + * @return + */ +inline bool RayTriangleIntersect(const Vec3f& rayOrigin, const Vec3f& rayDirection, const Vec3f& v0, const Vec3f& v1, const Vec3f& v2, Vec3f& barycentricCoord) +{ + // compute plane's normal + Vec3f v0v1 = v1 - v0; + Vec3f v0v2 = v2 - v0; + // no need to normalize + Vec3f N = v0v1.cross(v0v2); // N + float denom = N.dot(N); + + // Step 1: finding P + + // check if ray and plane are parallel ? + float NdotRayDirection = N.dot(rayDirection); + + if(fabs(NdotRayDirection) < k_Epsilon) // almost 0 + { + return false; + } // they are parallel so they don't intersect ! + + // compute d parameter using equation 2 + float d = -N.dot(v0); + + // compute t (equation 3) + barycentricCoord[2] = -(N.dot(rayOrigin) + d) / NdotRayDirection; + + // check if the triangle is in behind the ray + if(barycentricCoord[2] < 0) + { + return false; + } // the triangle is behind + + // compute the intersection point using equation 1 + Vec3f P = rayOrigin + (rayDirection * barycentricCoord[2]); + + // Step 2: inside-outside test + Vec3f C; // vector perpendicular to triangle's plane + + // edge 0 + Vec3f edge0 = v1 - v0; + Vec3f vp0 = P - v0; + C = edge0.cross(vp0); + if(N.dot(C) < 0) + { + return false; + } // P is on the right side + + // edge 1 + Vec3f edge1 = v2 - v1; + Vec3f vp1 = P - v1; + C = edge1.cross(vp1); + if((barycentricCoord[0] = N.dot(C)) < 0) + { + return false; + } // P is on the right side + + // edge 2 + Vec3f edge2 = v0 - v2; + Vec3f vp2 = P - v2; + C = edge2.cross(vp2); + if((barycentricCoord[1] = N.dot(C)) < 0) + { + return false; // P is on the right side; + } + + barycentricCoord[0] /= denom; + barycentricCoord[1] /= denom; + barycentricCoord[2] = 1 - barycentricCoord[0] - barycentricCoord[1]; + + return true; // this ray hits the triangle +} + +/* Adapted from https://github.com/erich666/jgt-code/blob/master/Volume_02/Number_1/Moller1997a/raytri.c */ +/* code rewritten to do tests on the sign of the determinant */ +/* the division is before the test of the sign of the det */ +/* and one CROSS has been moved out from the if-else if-else + * @param rayOrigin + * @param rayDirection + * @param v0 First Vertex of Triangle + * @param v1 Second Vertex of Triangle + * @param v2 Third Vertex of Triangle + * @param t part of Barycentric Coord + * @param u part of Barycentric Coord + * @param v part of Barycentric Coord + * @return If the point is within the triangle + */ +inline bool RayTriangleIntersect2(const Vec3f& orig, const Vec3f& dir, const Vec3f& vert0, const Vec3f& vert1, const Vec3f& vert2, Vec3f& bcoord) +{ + Vec3f edge1, edge2, tvec, pvec, qvec; + double det, inv_det; + + /* find vectors for two edges sharing vert0 */ + edge1 = vert1 - vert0; + edge2 = vert2 - vert0; + + /* begin calculating determinant - also used to calculate U parameter */ + pvec = dir.cross(edge2); + + /* if determinant is near zero, ray lies in plane of triangle */ + det = edge1.dot(pvec); + + /* calculate distance from vert0 to ray origin */ + tvec = orig - vert0; + + inv_det = 1.0 / det; + + qvec = tvec.cross(edge1); + + if(det > k_Epsilon) + { + bcoord[0] = tvec.dot(pvec); + if(bcoord[0] < 0.0 || bcoord[0] > det) + { + return false; + } + + /* calculate V parameter and test bounds */ + // bcoord[1] = DOT(dir, qvec); + bcoord[1] = dir.dot(qvec); + if(bcoord[1] < 0.0 || bcoord[0] + bcoord[1] > det) + { + return false; + } + } + else if(det < -k_Epsilon) + { + /* calculate U parameter and test bounds */ + bcoord[0] = tvec.dot(pvec); + if(bcoord[0] > 0.0 || bcoord[0] < det) + { + return false; + } + + /* calculate V parameter and test bounds */ + bcoord[1] = dir.dot(qvec); + if(bcoord[1] > 0.0 || bcoord[0] + bcoord[1] < det) + { + return false; + } + } + else + { + return false; + } /* ray is parallel to the plane of the triangle */ + + // float t = edge2.dotProduct(qvec) * inv_det; + float u = bcoord[0] * inv_det; + float v = bcoord[1] * inv_det; + + bcoord[0] = 1 - u - v; + bcoord[1] = u; + bcoord[2] = v; + + return true; +} + +} // namespace IntersectionUtilities + +} // namespace nx diff --git a/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/RTree.hpp b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/RTree.hpp new file mode 100644 index 0000000000..2f6b953b68 --- /dev/null +++ b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/RTree.hpp @@ -0,0 +1,1652 @@ +#ifndef RTREE_H +#define RTREE_H + +// NOTE This file compiles under MSVC 6 SP5 and MSVC .Net 2003 it may not work on other compilers without modification. + +// NOTE These next few lines may be win32 specific, you may need to modify them to compile on other platform +#include +#include +#include +#include + +#include +#include +#include + +#define ASSERT assert // RTree uses ASSERT( condition ) +#ifndef RTREE_MIN +#define RTREE_MIN std::min +#endif // RTREE_MIN +#ifndef RTREE_MAX +#define RTREE_MAX std::max +#endif // RTREE_MAX + +// +// RTree.h +// + +#define RTREE_TEMPLATE template +#define RTREE_QUAL RTree + +#define RTREE_DONT_USE_MEMPOOLS // This version does not contain a fixed memory allocator, fill in lines with EXAMPLE to implement one. +#define RTREE_USE_SPHERICAL_VOLUME // Better split classification, may be slower on some systems + +// Fwd decl +class RTFileStream; // File I/O helper class, look below for implementation and notes. + +/// \class RTree +/// Implementation of RTree, a multidimensional bounding rectangle tree. +/// Example usage: For a 3-dimensional tree use RTree myTree; +/// +/// This modified, templated C++ version by Greg Douglas at Auran (http://www.auran.com) +/// +/// DATATYPE Referenced data, should be int, void*, obj* etc. no larger than sizeof and simple type +/// ELEMTYPE Type of element such as int or float +/// NUMDIMS Number of dimensions such as 2 or 3 +/// ELEMTYPEREAL Type of element that allows fractional and large values such as float or double, for use in volume calcs +/// +/// NOTES: Inserting and removing data requires the knowledge of its constant Minimal Bounding Rectangle. +/// This version uses new/delete for nodes, I recommend using a fixed size allocator for efficiency. +/// Instead of using a callback function for returned results, I recommend and efficient pre-sized, grow-only memory +/// array similar to MFC CArray or STL Vector for returning search query result. +/// +template +class RTree +{ + static_assert(std::numeric_limits::is_iec559, "'ELEMTYPEREAL' accepts floating-point types only"); + +protected: + struct Node; // Fwd decl. Used by other internal structs and iterator + +public: + // These constant must be declared after Branch and before Node struct + // Stuck up here for MSVC 6 compiler. NSVC .NET 2003 is much happier. + enum + { + MAXNODES = TMAXNODES, ///< RTREE_MAX elements in node + MINNODES = TMINNODES, ///< RTREE_MIN elements in node + }; + +public: + RTree(); + RTree(const RTree& other); + virtual ~RTree(); + + /// Insert entry + /// \param a_min RTREE_MIN of bounding rect + /// \param a_max RTREE_MAX of bounding rect + /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers not allowed. + void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId); + + /// Remove entry + /// \param a_min RTREE_MIN of bounding rect + /// \param a_max RTREE_MAX of bounding rect + /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers not allowed. + void Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId); + + /// Find all within search rectangle + /// \param a_min RTREE_MIN of search bounding rect + /// \param a_max RTREE_MAX of search bounding rect + /// \param a_searchResult Search result array. Caller should set grow size. Function will reset, not append to array. + /// \param a_resultCallback Callback function to return result. Callback should return 'true' to continue searching + /// \param a_context User context to pass as parameter to a_resultCallback + /// \return Returns the number of entries found + int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], std::function callback) const; + + /// Remove all entries from tree + void RemoveAll(); + + /// Count the data elements in this container. This is slow as no internal counter is maintained. + int Count(); + + /// Load tree contents from file + bool Load(const char* a_fileName); + /// Load tree contents from stream + bool Load(RTFileStream& a_stream); + + /// Save tree contents to file + bool Save(const char* a_fileName); + /// Save tree contents to stream + bool Save(RTFileStream& a_stream); + + /// Iterator is not remove safe. + class Iterator + { + private: + enum + { + MAX_STACK = 32 + }; // RTREE_MAX stack size. Allows almost n^32 where n is number of branches in node + + struct StackElement + { + Node* m_node; + int m_branchIndex; + }; + + public: + Iterator() + { + Init(); + } + + ~Iterator() + { + } + + /// Is iterator invalid + bool IsNull() + { + return (m_tos <= 0); + } + + /// Is iterator pointing to valid data + bool IsNotNull() + { + return (m_tos > 0); + } + + /// Access the current data element. Caller must be sure iterator is not NULL first. + DATATYPE& operator*() + { + ASSERT(IsNotNull()); + StackElement& curTos = m_stack[m_tos - 1]; + return curTos.m_node->m_branch[curTos.m_branchIndex].m_data; + } + + /// Access the current data element. Caller must be sure iterator is not NULL first. + const DATATYPE& operator*() const + { + ASSERT(IsNotNull()); + StackElement& curTos = m_stack[m_tos - 1]; + return curTos.m_node->m_branch[curTos.m_branchIndex].m_data; + } + + /// Find the next data element + bool operator++() + { + return FindNextData(); + } + + /// Get the bounds for this node + void GetBounds(ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS]) + { + ASSERT(IsNotNull()); + StackElement& curTos = m_stack[m_tos - 1]; + Branch& curBranch = curTos.m_node->m_branch[curTos.m_branchIndex]; + + for(int index = 0; index < NUMDIMS; ++index) + { + a_min[index] = curBranch.m_rect.m_min[index]; + a_max[index] = curBranch.m_rect.m_max[index]; + } + } + + private: + /// Reset iterator + void Init() + { + m_tos = 0; + } + + /// Find the next data element in the tree (For internal use only) + bool FindNextData() + { + for(;;) + { + if(m_tos <= 0) + { + return false; + } + StackElement curTos = Pop(); // Copy stack top cause it may change as we use it + + if(curTos.m_node->IsLeaf()) + { + // Keep walking through data while we can + if(curTos.m_branchIndex + 1 < curTos.m_node->m_count) + { + // There is more data, just point to the next one + Push(curTos.m_node, curTos.m_branchIndex + 1); + return true; + } + // No more data, so it will fall back to previous level + } + else + { + if(curTos.m_branchIndex + 1 < curTos.m_node->m_count) + { + // Push sibling on for future tree walk + // This is the 'fall back' node when we finish with the current level + Push(curTos.m_node, curTos.m_branchIndex + 1); + } + // Since cur node is not a leaf, push first of next level to get deeper into the tree + Node* nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child; + Push(nextLevelnode, 0); + + // If we pushed on a new leaf, exit as the data is ready at TOS + if(nextLevelnode->IsLeaf()) + { + return true; + } + } + } + } + + /// Push node and branch onto iteration stack (For internal use only) + void Push(Node* a_node, int a_branchIndex) + { + m_stack[m_tos].m_node = a_node; + m_stack[m_tos].m_branchIndex = a_branchIndex; + ++m_tos; + ASSERT(m_tos <= MAX_STACK); + } + + /// Pop element off iteration stack (For internal use only) + StackElement& Pop() + { + ASSERT(m_tos > 0); + --m_tos; + return m_stack[m_tos]; + } + + StackElement m_stack[MAX_STACK]; ///< Stack as we are doing iteration instead of recursion + int m_tos; ///< Top Of Stack index + + friend class RTree; // Allow hiding of non-public functions while allowing manipulation by logical owner + }; + + /// Get 'first' for iteration + void GetFirst(Iterator& a_it) + { + a_it.Init(); + Node* first = m_root; + while(first) + { + if(first->IsInternalNode() && first->m_count > 1) + { + a_it.Push(first, 1); // Descend sibling branch later + } + else if(first->IsLeaf()) + { + if(first->m_count) + { + a_it.Push(first, 0); + } + break; + } + first = first->m_branch[0].m_child; + } + } + + /// Get Next for iteration + void GetNext(Iterator& a_it) + { + ++a_it; + } + + /// Is iterator NULL, or at end? + bool IsNull(Iterator& a_it) + { + return a_it.IsNull(); + } + + /// Get object at iterator position + DATATYPE& GetAt(Iterator& a_it) + { + return *a_it; + } + +protected: + /// Minimal bounding rectangle (n-dimensional) + struct Rect + { + ELEMTYPE m_min[NUMDIMS]; ///< RTREE_MIN dimensions of bounding box + ELEMTYPE m_max[NUMDIMS]; ///< RTREE_MAX dimensions of bounding box + }; + + /// May be data or may be another subtree + /// The parents level determines this. + /// If the parents level is 0, then this is data + struct Branch + { + Rect m_rect; ///< Bounds + Node* m_child; ///< Child node + DATATYPE m_data; ///< Data Id + }; + + /// Node for each branch level + struct Node + { + bool IsInternalNode() + { + return (m_level > 0); + } // Not a leaf, but a internal node + bool IsLeaf() + { + return (m_level == 0); + } // A leaf, contains data + + int m_count; ///< Count + int m_level; ///< Leaf is zero, others positive + Branch m_branch[MAXNODES]; ///< Branch + }; + + /// A link list of nodes for reinsertion after a delete operation + struct ListNode + { + ListNode* m_next; ///< Next in list + Node* m_node; ///< Node + }; + + /// Variables for finding a split partition + struct PartitionVars + { + enum + { + NOT_TAKEN = -1 + }; // indicates that position + + int m_partition[MAXNODES + 1]; + int m_total; + int m_minFill; + int m_count[2]; + Rect m_cover[2]; + ELEMTYPEREAL m_area[2]; + + Branch m_branchBuf[MAXNODES + 1]; + int m_branchCount; + Rect m_coverSplit; + ELEMTYPEREAL m_coverSplitArea; + }; + + Node* AllocNode(); + void FreeNode(Node* a_node); + void InitNode(Node* a_node); + void InitRect(Rect* a_rect); + bool InsertRectRec(const Branch& a_branch, Node* a_node, Node** a_newNode, int a_level); + bool InsertRect(const Branch& a_branch, Node** a_root, int a_level); + Rect NodeCover(Node* a_node); + bool AddBranch(const Branch* a_branch, Node* a_node, Node** a_newNode); + void DisconnectBranch(Node* a_node, int a_index); + int PickBranch(const Rect* a_rect, Node* a_node); + Rect CombineRect(const Rect* a_rectA, const Rect* a_rectB); + void SplitNode(Node* a_node, const Branch* a_branch, Node** a_newNode); + ELEMTYPEREAL RectSphericalVolume(Rect* a_rect); + ELEMTYPEREAL RectVolume(Rect* a_rect); + ELEMTYPEREAL CalcRectVolume(Rect* a_rect); + void GetBranches(Node* a_node, const Branch* a_branch, PartitionVars* a_parVars); + void ChoosePartition(PartitionVars* a_parVars, int a_minFill); + void LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars); + void InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_minFill); + void PickSeeds(PartitionVars* a_parVars); + void Classify(int a_index, int a_group, PartitionVars* a_parVars); + bool RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root); + bool RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, ListNode** a_listNode); + ListNode* AllocListNode(); + void FreeListNode(ListNode* a_listNode); + bool Overlap(Rect* a_rectA, Rect* a_rectB) const; + void ReInsert(Node* a_node, ListNode** a_listNode); + bool Search(Node* a_node, Rect* a_rect, int& a_foundCount, std::function callback) const; + void RemoveAllRec(Node* a_node); + void Reset(); + void CountRec(Node* a_node, int& a_count); + + bool SaveRec(Node* a_node, RTFileStream& a_stream); + bool LoadRec(Node* a_node, RTFileStream& a_stream); + void CopyRec(Node* current, Node* other); + + Node* m_root; ///< Root of tree + ELEMTYPEREAL m_unitSphereVolume; ///< Unit sphere constant for required number of dimensions + +public: + // return all the AABBs that form the RTree + std::vector ListTree() const; +}; + +// Because there is not stream support, this is a quick and dirty file I/O helper. +// Users will likely replace its usage with a Stream implementation from their favorite API. +class RTFileStream +{ + FILE* m_file; + +public: + RTFileStream() + { + m_file = NULL; + } + + ~RTFileStream() + { + Close(); + } + + bool Open(const char* a_fileName, const char* mode) + { +#if defined(_WIN32) && defined(__STDC_WANT_SECURE_LIB__) + return fopen_s(&m_file, a_fileName, mode) == 0; +#else + m_file = fopen(a_fileName, mode); + return m_file != nullptr; +#endif + } + + bool OpenRead(const char* a_fileName) + { + return this->Open(a_fileName, "rb"); + } + + bool OpenWrite(const char* a_fileName) + { + return this->Open(a_fileName, "wb"); + } + + void Close() + { + if(m_file) + { + fclose(m_file); + m_file = NULL; + } + } + + template + size_t Write(const TYPE& a_value) + { + ASSERT(m_file); + return fwrite((void*)&a_value, sizeof(a_value), 1, m_file); + } + + template + size_t WriteArray(const TYPE* a_array, int a_count) + { + ASSERT(m_file); + return fwrite((void*)a_array, sizeof(TYPE) * a_count, 1, m_file); + } + + template + size_t Read(TYPE& a_value) + { + ASSERT(m_file); + return fread((void*)&a_value, sizeof(a_value), 1, m_file); + } + + template + size_t ReadArray(TYPE* a_array, int a_count) + { + ASSERT(m_file); + return fread((void*)a_array, sizeof(TYPE) * a_count, 1, m_file); + } +}; + +RTREE_TEMPLATE +RTREE_QUAL::RTree() +{ + ASSERT(MAXNODES > MINNODES); + ASSERT(MINNODES > 0); + + // Precomputed volumes of the unit spheres for the first few dimensions + const float UNIT_SPHERE_VOLUMES[] = { + 0.000000f, 2.000000f, 3.141593f, // Dimension 0,1,2 + 4.188790f, 4.934802f, 5.263789f, // Dimension 3,4,5 + 5.167713f, 4.724766f, 4.058712f, // Dimension 6,7,8 + 3.298509f, 2.550164f, 1.884104f, // Dimension 9,10,11 + 1.335263f, 0.910629f, 0.599265f, // Dimension 12,13,14 + 0.381443f, 0.235331f, 0.140981f, // Dimension 15,16,17 + 0.082146f, 0.046622f, 0.025807f, // Dimension 18,19,20 + }; + + m_root = AllocNode(); + m_root->m_level = 0; + m_unitSphereVolume = (ELEMTYPEREAL)UNIT_SPHERE_VOLUMES[NUMDIMS]; +} + +RTREE_TEMPLATE +RTREE_QUAL::RTree(const RTree& other) +: RTree() +{ + CopyRec(m_root, other.m_root); +} + +RTREE_TEMPLATE +RTREE_QUAL::~RTree() +{ + Reset(); // Free, or reset node memory +} + +RTREE_TEMPLATE +void RTREE_QUAL::Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId) +{ +#ifdef _DEBUG + for(int index = 0; index < NUMDIMS; ++index) + { + ASSERT(a_min[index] <= a_max[index]); + } +#endif //_DEBUG + + Branch branch; + branch.m_data = a_dataId; + branch.m_child = NULL; + + for(int axis = 0; axis < NUMDIMS; ++axis) + { + branch.m_rect.m_min[axis] = a_min[axis]; + branch.m_rect.m_max[axis] = a_max[axis]; + } + + InsertRect(branch, &m_root, 0); +} + +RTREE_TEMPLATE +void RTREE_QUAL::Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId) +{ +#ifdef _DEBUG + for(int index = 0; index < NUMDIMS; ++index) + { + ASSERT(a_min[index] <= a_max[index]); + } +#endif //_DEBUG + + Rect rect; + + for(int axis = 0; axis < NUMDIMS; ++axis) + { + rect.m_min[axis] = a_min[axis]; + rect.m_max[axis] = a_max[axis]; + } + + RemoveRect(&rect, a_dataId, &m_root); +} + +RTREE_TEMPLATE +int RTREE_QUAL::Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], std::function callback) const +{ +#ifdef _DEBUG + for(int index = 0; index < NUMDIMS; ++index) + { + ASSERT(a_min[index] <= a_max[index]); + } +#endif //_DEBUG + + Rect rect; + + for(int axis = 0; axis < NUMDIMS; ++axis) + { + rect.m_min[axis] = a_min[axis]; + rect.m_max[axis] = a_max[axis]; + } + + // NOTE: May want to return search result another way, perhaps returning the number of found elements here. + + int foundCount = 0; + Search(m_root, &rect, foundCount, callback); + + return foundCount; +} + +RTREE_TEMPLATE +int RTREE_QUAL::Count() +{ + int count = 0; + CountRec(m_root, count); + + return count; +} + +RTREE_TEMPLATE +void RTREE_QUAL::CountRec(Node* a_node, int& a_count) +{ + if(a_node->IsInternalNode()) // not a leaf node + { + for(int index = 0; index < a_node->m_count; ++index) + { + CountRec(a_node->m_branch[index].m_child, a_count); + } + } + else // A leaf node + { + a_count += a_node->m_count; + } +} + +RTREE_TEMPLATE +bool RTREE_QUAL::Load(const char* a_fileName) +{ + RemoveAll(); // Clear existing tree + + RTFileStream stream; + if(!stream.OpenRead(a_fileName)) + { + return false; + } + + bool result = Load(stream); + + stream.Close(); + + return result; +} + +RTREE_TEMPLATE +bool RTREE_QUAL::Load(RTFileStream& a_stream) +{ + // Write some kind of header + int _dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24); + int _dataSize = sizeof(DATATYPE); + int _dataNumDims = NUMDIMS; + int _dataElemSize = sizeof(ELEMTYPE); + int _dataElemRealSize = sizeof(ELEMTYPEREAL); + int _dataMaxNodes = TMAXNODES; + int _dataMinNodes = TMINNODES; + + int dataFileId = 0; + int dataSize = 0; + int dataNumDims = 0; + int dataElemSize = 0; + int dataElemRealSize = 0; + int dataMaxNodes = 0; + int dataMinNodes = 0; + + a_stream.Read(dataFileId); + a_stream.Read(dataSize); + a_stream.Read(dataNumDims); + a_stream.Read(dataElemSize); + a_stream.Read(dataElemRealSize); + a_stream.Read(dataMaxNodes); + a_stream.Read(dataMinNodes); + + bool result = false; + + // Test if header was valid and compatible + if((dataFileId == _dataFileId) && (dataSize == _dataSize) && (dataNumDims == _dataNumDims) && (dataElemSize == _dataElemSize) && (dataElemRealSize == _dataElemRealSize) && + (dataMaxNodes == _dataMaxNodes) && (dataMinNodes == _dataMinNodes)) + { + // Recursively load tree + result = LoadRec(m_root, a_stream); + } + + return result; +} + +RTREE_TEMPLATE +bool RTREE_QUAL::LoadRec(Node* a_node, RTFileStream& a_stream) +{ + a_stream.Read(a_node->m_level); + a_stream.Read(a_node->m_count); + + if(a_node->IsInternalNode()) // not a leaf node + { + for(int index = 0; index < a_node->m_count; ++index) + { + Branch* curBranch = &a_node->m_branch[index]; + + a_stream.ReadArray(curBranch->m_rect.m_min, NUMDIMS); + a_stream.ReadArray(curBranch->m_rect.m_max, NUMDIMS); + + curBranch->m_child = AllocNode(); + LoadRec(curBranch->m_child, a_stream); + } + } + else // A leaf node + { + for(int index = 0; index < a_node->m_count; ++index) + { + Branch* curBranch = &a_node->m_branch[index]; + + a_stream.ReadArray(curBranch->m_rect.m_min, NUMDIMS); + a_stream.ReadArray(curBranch->m_rect.m_max, NUMDIMS); + + a_stream.Read(curBranch->m_data); + } + } + + return true; // Should do more error checking on I/O operations +} + +RTREE_TEMPLATE +void RTREE_QUAL::CopyRec(Node* current, Node* other) +{ + current->m_level = other->m_level; + current->m_count = other->m_count; + + if(current->IsInternalNode()) // not a leaf node + { + for(int index = 0; index < current->m_count; ++index) + { + Branch* currentBranch = ¤t->m_branch[index]; + Branch* otherBranch = &other->m_branch[index]; + + std::copy(otherBranch->m_rect.m_min, otherBranch->m_rect.m_min + NUMDIMS, currentBranch->m_rect.m_min); + + std::copy(otherBranch->m_rect.m_max, otherBranch->m_rect.m_max + NUMDIMS, currentBranch->m_rect.m_max); + + currentBranch->m_child = AllocNode(); + CopyRec(currentBranch->m_child, otherBranch->m_child); + } + } + else // A leaf node + { + for(int index = 0; index < current->m_count; ++index) + { + Branch* currentBranch = ¤t->m_branch[index]; + Branch* otherBranch = &other->m_branch[index]; + + std::copy(otherBranch->m_rect.m_min, otherBranch->m_rect.m_min + NUMDIMS, currentBranch->m_rect.m_min); + + std::copy(otherBranch->m_rect.m_max, otherBranch->m_rect.m_max + NUMDIMS, currentBranch->m_rect.m_max); + + currentBranch->m_data = otherBranch->m_data; + } + } +} + +RTREE_TEMPLATE +bool RTREE_QUAL::Save(const char* a_fileName) +{ + RTFileStream stream; + if(!stream.OpenWrite(a_fileName)) + { + return false; + } + + bool result = Save(stream); + + stream.Close(); + + return result; +} + +RTREE_TEMPLATE +bool RTREE_QUAL::Save(RTFileStream& a_stream) +{ + // Write some kind of header + int dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24); + int dataSize = sizeof(DATATYPE); + int dataNumDims = NUMDIMS; + int dataElemSize = sizeof(ELEMTYPE); + int dataElemRealSize = sizeof(ELEMTYPEREAL); + int dataMaxNodes = TMAXNODES; + int dataMinNodes = TMINNODES; + + a_stream.Write(dataFileId); + a_stream.Write(dataSize); + a_stream.Write(dataNumDims); + a_stream.Write(dataElemSize); + a_stream.Write(dataElemRealSize); + a_stream.Write(dataMaxNodes); + a_stream.Write(dataMinNodes); + + // Recursively save tree + bool result = SaveRec(m_root, a_stream); + + return result; +} + +RTREE_TEMPLATE +bool RTREE_QUAL::SaveRec(Node* a_node, RTFileStream& a_stream) +{ + a_stream.Write(a_node->m_level); + a_stream.Write(a_node->m_count); + + if(a_node->IsInternalNode()) // not a leaf node + { + for(int index = 0; index < a_node->m_count; ++index) + { + Branch* curBranch = &a_node->m_branch[index]; + + a_stream.WriteArray(curBranch->m_rect.m_min, NUMDIMS); + a_stream.WriteArray(curBranch->m_rect.m_max, NUMDIMS); + + SaveRec(curBranch->m_child, a_stream); + } + } + else // A leaf node + { + for(int index = 0; index < a_node->m_count; ++index) + { + Branch* curBranch = &a_node->m_branch[index]; + + a_stream.WriteArray(curBranch->m_rect.m_min, NUMDIMS); + a_stream.WriteArray(curBranch->m_rect.m_max, NUMDIMS); + + a_stream.Write(curBranch->m_data); + } + } + + return true; // Should do more error checking on I/O operations +} + +RTREE_TEMPLATE +void RTREE_QUAL::RemoveAll() +{ + // Delete all existing nodes + Reset(); + + m_root = AllocNode(); + m_root->m_level = 0; +} + +RTREE_TEMPLATE +void RTREE_QUAL::Reset() +{ +#ifdef RTREE_DONT_USE_MEMPOOLS + // Delete all existing nodes + RemoveAllRec(m_root); +#else // RTREE_DONT_USE_MEMPOOLS + // Just reset memory pools. We are not using simplnx types + // EXAMPLE +#endif // RTREE_DONT_USE_MEMPOOLS +} + +RTREE_TEMPLATE +void RTREE_QUAL::RemoveAllRec(Node* a_node) +{ + ASSERT(a_node); + ASSERT(a_node->m_level >= 0); + + if(a_node->IsInternalNode()) // This is an internal node in the tree + { + for(int index = 0; index < a_node->m_count; ++index) + { + RemoveAllRec(a_node->m_branch[index].m_child); + } + } + FreeNode(a_node); +} + +RTREE_TEMPLATE +typename RTREE_QUAL::Node* RTREE_QUAL::AllocNode() +{ + Node* newNode; +#ifdef RTREE_DONT_USE_MEMPOOLS + newNode = new Node; +#else // RTREE_DONT_USE_MEMPOOLS + // EXAMPLE +#endif // RTREE_DONT_USE_MEMPOOLS + InitNode(newNode); + return newNode; +} + +RTREE_TEMPLATE +void RTREE_QUAL::FreeNode(Node* a_node) +{ + ASSERT(a_node); + +#ifdef RTREE_DONT_USE_MEMPOOLS + delete a_node; +#else // RTREE_DONT_USE_MEMPOOLS + // EXAMPLE +#endif // RTREE_DONT_USE_MEMPOOLS +} + +// Allocate space for a node in the list used in DeletRect to +// store Nodes that are too empty. +RTREE_TEMPLATE +typename RTREE_QUAL::ListNode* RTREE_QUAL::AllocListNode() +{ +#ifdef RTREE_DONT_USE_MEMPOOLS + return new ListNode; +#else // RTREE_DONT_USE_MEMPOOLS + // EXAMPLE +#endif // RTREE_DONT_USE_MEMPOOLS +} + +RTREE_TEMPLATE +void RTREE_QUAL::FreeListNode(ListNode* a_listNode) +{ +#ifdef RTREE_DONT_USE_MEMPOOLS + delete a_listNode; +#else // RTREE_DONT_USE_MEMPOOLS + // EXAMPLE +#endif // RTREE_DONT_USE_MEMPOOLS +} + +RTREE_TEMPLATE +void RTREE_QUAL::InitNode(Node* a_node) +{ + a_node->m_count = 0; + a_node->m_level = -1; +} + +RTREE_TEMPLATE +void RTREE_QUAL::InitRect(Rect* a_rect) +{ + for(int index = 0; index < NUMDIMS; ++index) + { + a_rect->m_min[index] = (ELEMTYPE)0; + a_rect->m_max[index] = (ELEMTYPE)0; + } +} + +// Inserts a new data rectangle into the index structure. +// Recursively descends tree, propagates splits back up. +// Returns 0 if node was not split. Old node updated. +// If node was split, returns 1 and sets the pointer pointed to by +// new_node to point to the new node. Old node updated to become one of two. +// The level argument specifies the number of steps up from the leaf +// level to insert; e.g. a data rectangle goes in at level = 0. +RTREE_TEMPLATE +bool RTREE_QUAL::InsertRectRec(const Branch& a_branch, Node* a_node, Node** a_newNode, int a_level) +{ + ASSERT(a_node && a_newNode); + ASSERT(a_level >= 0 && a_level <= a_node->m_level); + + // recurse until we reach the correct level for the new record. data records + // will always be called with a_level == 0 (leaf) + if(a_node->m_level > a_level) + { + // Still above level for insertion, go down tree recursively + Node* otherNode; + + // find the optimal branch for this record + int index = PickBranch(&a_branch.m_rect, a_node); + + // recursively insert this record into the picked branch + bool childWasSplit = InsertRectRec(a_branch, a_node->m_branch[index].m_child, &otherNode, a_level); + + if(!childWasSplit) + { + // Child was not split. Merge the bounding box of the new record with the + // existing bounding box + a_node->m_branch[index].m_rect = CombineRect(&a_branch.m_rect, &(a_node->m_branch[index].m_rect)); + return false; + } + else + { + // Child was split. The old branches are now re-partitioned to two nodes + // so we have to re-calculate the bounding boxes of each node + a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child); + Branch branch; + branch.m_child = otherNode; + branch.m_rect = NodeCover(otherNode); + + // The old node is already a child of a_node. Now add the newly-created + // node to a_node as well. a_node might be split because of that. + return AddBranch(&branch, a_node, a_newNode); + } + } + else if(a_node->m_level == a_level) + { + // We have reached level for insertion. Add rect, split if necessary + return AddBranch(&a_branch, a_node, a_newNode); + } + else + { + // Should never occur + ASSERT(0); + return false; + } +} + +// Insert a data rectangle into an index structure. +// InsertRect provides for splitting the root; +// returns 1 if root was split, 0 if it was not. +// The level argument specifies the number of steps up from the leaf +// level to insert; e.g. a data rectangle goes in at level = 0. +// InsertRect2 does the recursion. +// +RTREE_TEMPLATE +bool RTREE_QUAL::InsertRect(const Branch& a_branch, Node** a_root, int a_level) +{ + ASSERT(a_root); + ASSERT(a_level >= 0 && a_level <= (*a_root)->m_level); +#ifdef _DEBUG + for(int index = 0; index < NUMDIMS; ++index) + { + ASSERT(a_branch.m_rect.m_min[index] <= a_branch.m_rect.m_max[index]); + } +#endif //_DEBUG + + Node* newNode; + + if(InsertRectRec(a_branch, *a_root, &newNode, a_level)) // Root split + { + // Grow tree taller and new root + Node* newRoot = AllocNode(); + newRoot->m_level = (*a_root)->m_level + 1; + + Branch branch; + + // add old root node as a child of the new root + branch.m_rect = NodeCover(*a_root); + branch.m_child = *a_root; + AddBranch(&branch, newRoot, NULL); + + // add the split node as a child of the new root + branch.m_rect = NodeCover(newNode); + branch.m_child = newNode; + AddBranch(&branch, newRoot, NULL); + + // set the new root as the root node + *a_root = newRoot; + + return true; + } + + return false; +} + +// Find the smallest rectangle that includes all rectangles in branches of a node. +RTREE_TEMPLATE +typename RTREE_QUAL::Rect RTREE_QUAL::NodeCover(Node* a_node) +{ + ASSERT(a_node); + + Rect rect = a_node->m_branch[0].m_rect; + for(int index = 1; index < a_node->m_count; ++index) + { + rect = CombineRect(&rect, &(a_node->m_branch[index].m_rect)); + } + + return rect; +} + +// Add a branch to a node. Split the node if necessary. +// Returns 0 if node not split. Old node updated. +// Returns 1 if node split, sets *new_node to address of new node. +// Old node updated, becomes one of two. +RTREE_TEMPLATE +bool RTREE_QUAL::AddBranch(const Branch* a_branch, Node* a_node, Node** a_newNode) +{ + ASSERT(a_branch); + ASSERT(a_node); + + if(a_node->m_count < MAXNODES) // Split won't be necessary + { + a_node->m_branch[a_node->m_count] = *a_branch; + ++a_node->m_count; + + return false; + } + else + { + ASSERT(a_newNode); + + SplitNode(a_node, a_branch, a_newNode); + return true; + } +} + +// Disconnect a dependent node. +// Caller must return (or stop using iteration index) after this as count has changed +RTREE_TEMPLATE +void RTREE_QUAL::DisconnectBranch(Node* a_node, int a_index) +{ + ASSERT(a_node && (a_index >= 0) && (a_index < MAXNODES)); + ASSERT(a_node->m_count > 0); + + // Remove element by swapping with the last element to prevent gaps in array + a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1]; + + --a_node->m_count; +} + +// Pick a branch. Pick the one that will need the smallest increase +// in area to accomodate the new rectangle. This will result in the +// least total area for the covering rectangles in the current node. +// In case of a tie, pick the one which was smaller before, to get +// the best resolution when searching. +RTREE_TEMPLATE +int RTREE_QUAL::PickBranch(const Rect* a_rect, Node* a_node) +{ + ASSERT(a_rect && a_node); + + bool firstTime = true; + ELEMTYPEREAL increase; + ELEMTYPEREAL bestIncr = (ELEMTYPEREAL)-1; + ELEMTYPEREAL area; + ELEMTYPEREAL bestArea; + int best = 0; + Rect tempRect; + + for(int index = 0; index < a_node->m_count; ++index) + { + Rect* curRect = &a_node->m_branch[index].m_rect; + area = CalcRectVolume(curRect); + tempRect = CombineRect(a_rect, curRect); + increase = CalcRectVolume(&tempRect) - area; + if((increase < bestIncr) || firstTime) + { + best = index; + bestArea = area; + bestIncr = increase; + firstTime = false; + } + else if((increase == bestIncr) && (area < bestArea)) + { + best = index; + bestArea = area; + bestIncr = increase; + } + } + return best; +} + +// Combine two rectangles into larger one containing both +RTREE_TEMPLATE +typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect(const Rect* a_rectA, const Rect* a_rectB) +{ + ASSERT(a_rectA && a_rectB); + + Rect newRect; + + for(int index = 0; index < NUMDIMS; ++index) + { + newRect.m_min[index] = RTREE_MIN(a_rectA->m_min[index], a_rectB->m_min[index]); + newRect.m_max[index] = RTREE_MAX(a_rectA->m_max[index], a_rectB->m_max[index]); + } + + return newRect; +} + +// Split a node. +// Divides the nodes branches and the extra one between two nodes. +// Old node is one of the new ones, and one really new one is created. +// Tries more than one method for choosing a partition, uses best result. +RTREE_TEMPLATE +void RTREE_QUAL::SplitNode(Node* a_node, const Branch* a_branch, Node** a_newNode) +{ + ASSERT(a_node); + ASSERT(a_branch); + + // Could just use local here, but member or external is faster since it is reused + PartitionVars localVars; + PartitionVars* parVars = &localVars; + + // Load all the branches into a buffer, initialize old node + GetBranches(a_node, a_branch, parVars); + + // Find partition + ChoosePartition(parVars, MINNODES); + + // Create a new node to hold (about) half of the branches + *a_newNode = AllocNode(); + (*a_newNode)->m_level = a_node->m_level; + + // Put branches from buffer into 2 nodes according to the chosen partition + a_node->m_count = 0; + LoadNodes(a_node, *a_newNode, parVars); + + ASSERT((a_node->m_count + (*a_newNode)->m_count) == parVars->m_total); +} + +// Calculate the n-dimensional volume of a rectangle +RTREE_TEMPLATE +ELEMTYPEREAL RTREE_QUAL::RectVolume(Rect* a_rect) +{ + ASSERT(a_rect); + + ELEMTYPEREAL volume = (ELEMTYPEREAL)1; + + for(int index = 0; index < NUMDIMS; ++index) + { + volume *= a_rect->m_max[index] - a_rect->m_min[index]; + } + + ASSERT(volume >= (ELEMTYPEREAL)0); + + return volume; +} + +// The exact volume of the bounding sphere for the given Rect +RTREE_TEMPLATE +ELEMTYPEREAL RTREE_QUAL::RectSphericalVolume(Rect* a_rect) +{ + ASSERT(a_rect); + + ELEMTYPEREAL sumOfSquares = (ELEMTYPEREAL)0; + ELEMTYPEREAL radius; + + for(int index = 0; index < NUMDIMS; ++index) + { + ELEMTYPEREAL halfExtent = ((ELEMTYPEREAL)a_rect->m_max[index] - (ELEMTYPEREAL)a_rect->m_min[index]) * (ELEMTYPEREAL)0.5; + sumOfSquares += halfExtent * halfExtent; + } + + radius = (ELEMTYPEREAL)sqrt(sumOfSquares); + + // Pow maybe slow, so test for common dims like 2,3 and just use x*x, x*x*x. + if(NUMDIMS == 3) + { + return (radius * radius * radius * m_unitSphereVolume); + } + else if(NUMDIMS == 2) + { + return (radius * radius * m_unitSphereVolume); + } + else + { + return (ELEMTYPEREAL)(pow(radius, NUMDIMS) * m_unitSphereVolume); + } +} + +// Use one of the methods to calculate retangle volume +RTREE_TEMPLATE +ELEMTYPEREAL RTREE_QUAL::CalcRectVolume(Rect* a_rect) +{ +#ifdef RTREE_USE_SPHERICAL_VOLUME + return RectSphericalVolume(a_rect); // Slower but helps certain merge cases +#else // RTREE_USE_SPHERICAL_VOLUME + return RectVolume(a_rect); // Faster but can cause poor merges +#endif // RTREE_USE_SPHERICAL_VOLUME +} + +// Load branch buffer with branches from full node plus the extra branch. +RTREE_TEMPLATE +void RTREE_QUAL::GetBranches(Node* a_node, const Branch* a_branch, PartitionVars* a_parVars) +{ + ASSERT(a_node); + ASSERT(a_branch); + + ASSERT(a_node->m_count == MAXNODES); + + // Load the branch buffer + for(int index = 0; index < MAXNODES; ++index) + { + a_parVars->m_branchBuf[index] = a_node->m_branch[index]; + } + a_parVars->m_branchBuf[MAXNODES] = *a_branch; + a_parVars->m_branchCount = MAXNODES + 1; + + // Calculate rect containing all in the set + a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect; + for(int index = 1; index < MAXNODES + 1; ++index) + { + a_parVars->m_coverSplit = CombineRect(&a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect); + } + a_parVars->m_coverSplitArea = CalcRectVolume(&a_parVars->m_coverSplit); +} + +// Method #0 for choosing a partition: +// As the seeds for the two groups, pick the two rects that would waste the +// most area if covered by a single rectangle, i.e. evidently the worst pair +// to have in the same group. +// Of the remaining, one at a time is chosen to be put in one of the two groups. +// The one chosen is the one with the greatest difference in area expansion +// depending on which group - the rect most strongly attracted to one group +// and repelled from the other. +// If one group gets too full (more would force other group to violate min +// fill requirement) then other group gets the rest. +// These last are the ones that can go in either group most easily. +RTREE_TEMPLATE +void RTREE_QUAL::ChoosePartition(PartitionVars* a_parVars, int a_minFill) +{ + ASSERT(a_parVars); + + ELEMTYPEREAL biggestDiff; + int group, chosen = 0, betterGroup = 0; + + InitParVars(a_parVars, a_parVars->m_branchCount, a_minFill); + PickSeeds(a_parVars); + + while(((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total) && (a_parVars->m_count[0] < (a_parVars->m_total - a_parVars->m_minFill)) && + (a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill))) + { + biggestDiff = (ELEMTYPEREAL)-1; + for(int index = 0; index < a_parVars->m_total; ++index) + { + if(PartitionVars::NOT_TAKEN == a_parVars->m_partition[index]) + { + Rect* curRect = &a_parVars->m_branchBuf[index].m_rect; + Rect rect0 = CombineRect(curRect, &a_parVars->m_cover[0]); + Rect rect1 = CombineRect(curRect, &a_parVars->m_cover[1]); + ELEMTYPEREAL growth0 = CalcRectVolume(&rect0) - a_parVars->m_area[0]; + ELEMTYPEREAL growth1 = CalcRectVolume(&rect1) - a_parVars->m_area[1]; + ELEMTYPEREAL diff = growth1 - growth0; + if(diff >= 0) + { + group = 0; + } + else + { + group = 1; + diff = -diff; + } + + if(diff > biggestDiff) + { + biggestDiff = diff; + chosen = index; + betterGroup = group; + } + else if((diff == biggestDiff) && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup])) + { + chosen = index; + betterGroup = group; + } + } + } + Classify(chosen, betterGroup, a_parVars); + } + + // If one group too full, put remaining rects in the other + if((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total) + { + if(a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill) + { + group = 1; + } + else + { + group = 0; + } + for(int index = 0; index < a_parVars->m_total; ++index) + { + if(PartitionVars::NOT_TAKEN == a_parVars->m_partition[index]) + { + Classify(index, group, a_parVars); + } + } + } + + ASSERT((a_parVars->m_count[0] + a_parVars->m_count[1]) == a_parVars->m_total); + ASSERT((a_parVars->m_count[0] >= a_parVars->m_minFill) && (a_parVars->m_count[1] >= a_parVars->m_minFill)); +} + +// Copy branches from the buffer into two nodes according to the partition. +RTREE_TEMPLATE +void RTREE_QUAL::LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars) +{ + ASSERT(a_nodeA); + ASSERT(a_nodeB); + ASSERT(a_parVars); + + for(int index = 0; index < a_parVars->m_total; ++index) + { + ASSERT(a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1); + + int targetNodeIndex = a_parVars->m_partition[index]; + Node* targetNodes[] = {a_nodeA, a_nodeB}; + + // It is assured that AddBranch here will not cause a node split. + bool nodeWasSplit = AddBranch(&a_parVars->m_branchBuf[index], targetNodes[targetNodeIndex], NULL); + ASSERT(!nodeWasSplit); + } +} + +// Initialize a PartitionVars structure. +RTREE_TEMPLATE +void RTREE_QUAL::InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_minFill) +{ + ASSERT(a_parVars); + + a_parVars->m_count[0] = a_parVars->m_count[1] = 0; + a_parVars->m_area[0] = a_parVars->m_area[1] = (ELEMTYPEREAL)0; + a_parVars->m_total = a_maxRects; + a_parVars->m_minFill = a_minFill; + for(int index = 0; index < a_maxRects; ++index) + { + a_parVars->m_partition[index] = PartitionVars::NOT_TAKEN; + } +} + +RTREE_TEMPLATE +void RTREE_QUAL::PickSeeds(PartitionVars* a_parVars) +{ + int seed0 = 0, seed1 = 0; + ELEMTYPEREAL worst, waste; + ELEMTYPEREAL area[MAXNODES + 1]; + + for(int index = 0; index < a_parVars->m_total; ++index) + { + area[index] = CalcRectVolume(&a_parVars->m_branchBuf[index].m_rect); + } + + worst = -a_parVars->m_coverSplitArea - 1; + for(int indexA = 0; indexA < a_parVars->m_total - 1; ++indexA) + { + for(int indexB = indexA + 1; indexB < a_parVars->m_total; ++indexB) + { + Rect oneRect = CombineRect(&a_parVars->m_branchBuf[indexA].m_rect, &a_parVars->m_branchBuf[indexB].m_rect); + waste = CalcRectVolume(&oneRect) - area[indexA] - area[indexB]; + if(waste > worst) + { + worst = waste; + seed0 = indexA; + seed1 = indexB; + } + } + } + + Classify(seed0, 0, a_parVars); + Classify(seed1, 1, a_parVars); +} + +// Put a branch in one of the groups. +RTREE_TEMPLATE +void RTREE_QUAL::Classify(int a_index, int a_group, PartitionVars* a_parVars) +{ + ASSERT(a_parVars); + ASSERT(PartitionVars::NOT_TAKEN == a_parVars->m_partition[a_index]); + + a_parVars->m_partition[a_index] = a_group; + + // Calculate combined rect + if(a_parVars->m_count[a_group] == 0) + { + a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect; + } + else + { + a_parVars->m_cover[a_group] = CombineRect(&a_parVars->m_branchBuf[a_index].m_rect, &a_parVars->m_cover[a_group]); + } + + // Calculate volume of combined rect + a_parVars->m_area[a_group] = CalcRectVolume(&a_parVars->m_cover[a_group]); + + ++a_parVars->m_count[a_group]; +} + +// Delete a data rectangle from an index structure. +// Pass in a pointer to a Rect, the tid of the record, ptr to ptr to root node. +// Returns 1 if record not found, 0 if success. +// RemoveRect provides for eliminating the root. +RTREE_TEMPLATE +bool RTREE_QUAL::RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root) +{ + ASSERT(a_rect && a_root); + ASSERT(*a_root); + + ListNode* reInsertList = NULL; + + if(!RemoveRectRec(a_rect, a_id, *a_root, &reInsertList)) + { + // Found and deleted a data item + // Reinsert any branches from eliminated nodes + while(reInsertList) + { + Node* tempNode = reInsertList->m_node; + + for(int index = 0; index < tempNode->m_count; ++index) + { + // TODO go over this code. should I use (tempNode->m_level - 1)? + InsertRect(tempNode->m_branch[index], a_root, tempNode->m_level); + } + + ListNode* remLNode = reInsertList; + reInsertList = reInsertList->m_next; + + FreeNode(remLNode->m_node); + FreeListNode(remLNode); + } + + // Check for redundant root (not leaf, 1 child) and eliminate TODO replace + // if with while? In case there is a whole branch of redundant roots... + if((*a_root)->m_count == 1 && (*a_root)->IsInternalNode()) + { + Node* tempNode = (*a_root)->m_branch[0].m_child; + + ASSERT(tempNode); + FreeNode(*a_root); + *a_root = tempNode; + } + return false; + } + else + { + return true; + } +} + +// Delete a rectangle from non-root part of an index structure. +// Called by RemoveRect. Descends tree recursively, +// merges branches on the way back up. +// Returns 1 if record not found, 0 if success. +RTREE_TEMPLATE +bool RTREE_QUAL::RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, ListNode** a_listNode) +{ + ASSERT(a_rect && a_node && a_listNode); + ASSERT(a_node->m_level >= 0); + + if(a_node->IsInternalNode()) // not a leaf node + { + for(int index = 0; index < a_node->m_count; ++index) + { + if(Overlap(a_rect, &(a_node->m_branch[index].m_rect))) + { + if(!RemoveRectRec(a_rect, a_id, a_node->m_branch[index].m_child, a_listNode)) + { + if(a_node->m_branch[index].m_child->m_count >= MINNODES) + { + // child removed, just resize parent rect + a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child); + } + else + { + // child removed, not enough entries in node, eliminate node + ReInsert(a_node->m_branch[index].m_child, a_listNode); + DisconnectBranch(a_node, index); // Must return after this call as count has changed + } + return false; + } + } + } + return true; + } + else // A leaf node + { + for(int index = 0; index < a_node->m_count; ++index) + { + if(a_node->m_branch[index].m_data == a_id) + { + DisconnectBranch(a_node, index); // Must return after this call as count has changed + return false; + } + } + return true; + } +} + +// Decide whether two rectangles overlap. +RTREE_TEMPLATE +bool RTREE_QUAL::Overlap(Rect* a_rectA, Rect* a_rectB) const +{ + ASSERT(a_rectA && a_rectB); + + for(int index = 0; index < NUMDIMS; ++index) + { + if(a_rectA->m_min[index] > a_rectB->m_max[index] || a_rectB->m_min[index] > a_rectA->m_max[index]) + { + return false; + } + } + return true; +} + +// Add a node to the reinsertion list. All its branches will later +// be reinserted into the index structure. +RTREE_TEMPLATE +void RTREE_QUAL::ReInsert(Node* a_node, ListNode** a_listNode) +{ + ListNode* newListNode; + + newListNode = AllocListNode(); + newListNode->m_node = a_node; + newListNode->m_next = *a_listNode; + *a_listNode = newListNode; +} + +// Search in an index tree or subtree for all data retangles that overlap the argument rectangle. +RTREE_TEMPLATE +bool RTREE_QUAL::Search(Node* a_node, Rect* a_rect, int& a_foundCount, std::function callback) const +{ + ASSERT(a_node); + ASSERT(a_node->m_level >= 0); + ASSERT(a_rect); + + if(a_node->IsInternalNode()) + { + // This is an internal node in the tree + for(int index = 0; index < a_node->m_count; ++index) + { + if(Overlap(a_rect, &a_node->m_branch[index].m_rect)) + { + if(!Search(a_node->m_branch[index].m_child, a_rect, a_foundCount, callback)) + { + // The callback indicated to stop searching + return false; + } + } + } + } + else + { + // This is a leaf node + for(int index = 0; index < a_node->m_count; ++index) + { + if(Overlap(a_rect, &a_node->m_branch[index].m_rect)) + { + DATATYPE& id = a_node->m_branch[index].m_data; + ++a_foundCount; + + if(callback && !callback(id)) + { + return false; // Don't continue searching + } + } + } + } + + return true; // Continue searching +} + +RTREE_TEMPLATE +std::vector RTREE_QUAL::ListTree() const +{ + ASSERT(m_root); + ASSERT(m_root->m_level >= 0); + + std::vector treeList; + + std::vector toVisit; + toVisit.push_back(m_root); + + while(!toVisit.empty()) + { + Node* a_node = toVisit.back(); + toVisit.pop_back(); + if(a_node->IsInternalNode()) + { + // This is an internal node in the tree + for(int index = 0; index < a_node->m_count; ++index) + { + treeList.push_back(a_node->m_branch[index].m_rect); + toVisit.push_back(a_node->m_branch[index].m_child); + } + } + else + { + // This is a leaf node + for(int index = 0; index < a_node->m_count; ++index) + { + treeList.push_back(a_node->m_branch[index].m_rect); + } + } + } + + return treeList; +} + +#undef RTREE_TEMPLATE +#undef RTREE_QUAL + +#endif // RTREE_H diff --git a/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/delaunator.cpp b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/delaunator.cpp new file mode 100644 index 0000000000..3be9c96596 --- /dev/null +++ b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/delaunator.cpp @@ -0,0 +1,662 @@ +/* +* MIT License + +Copyright (c) 2018 Volodymyr Bilonenko + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ +#include "delaunator.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nx::delaunator +{ + +//@see https://stackoverflow.com/questions/33333363/built-in-mod-vs-custom-mod-function-improve-the-performance-of-modulus-op/33333636#33333636 +inline size_t fast_mod(const size_t i, const size_t c) +{ + return i >= c ? i % c : i; +} + +// Kahan and Babuska summation, Neumaier variant; accumulates less FP error +inline double sum(const std::vector& x) +{ + double sum = x[0]; + double err = 0.0; + + for(size_t i = 1; i < x.size(); i++) + { + const double k = x[i]; + const double m = sum + k; + err += std::fabs(sum) >= std::fabs(k) ? sum - m + k : k - m + sum; + sum = m; + } + return sum + err; +} + +inline double dist(const double ax, const double ay, const double bx, const double by) +{ + const double dx = ax - bx; + const double dy = ay - by; + return dx * dx + dy * dy; +} + +inline double circumradius(const Point& p1, const Point& p2, const Point& p3) +{ + Point d = Point::vector(p1, p2); + Point e = Point::vector(p1, p3); + + const double bl = d.magnitude2(); + const double cl = e.magnitude2(); + const double det = Point::determinant(d, e); + + Point radius((e.y() * bl - d.y() * cl) * 0.5 / det, (d.x() * cl - e.x() * bl) * 0.5 / det); + + if((bl > 0.0 || bl < 0.0) && (cl > 0.0 || cl < 0.0) && (det > 0.0 || det < 0.0)) + return radius.magnitude2(); + return (std::numeric_limits::max)(); +} + +inline double circumradius(const double ax, const double ay, const double bx, const double by, const double cx, const double cy) +{ + const double dx = bx - ax; + const double dy = by - ay; + const double ex = cx - ax; + const double ey = cy - ay; + + const double bl = dx * dx + dy * dy; + const double cl = ex * ex + ey * ey; + const double d = dx * ey - dy * ex; + + const double x = (ey * bl - dy * cl) * 0.5 / d; + const double y = (dx * cl - ex * bl) * 0.5 / d; + + if((bl > 0.0 || bl < 0.0) && (cl > 0.0 || cl < 0.0) && (d > 0.0 || d < 0.0)) + { + return x * x + y * y; + } + else + { + return (std::numeric_limits::max)(); + } +} + +inline bool clockwise(const Point& p0, const Point& p1, const Point& p2) +{ + Point v0 = Point::vector(p0, p1); + Point v1 = Point::vector(p0, p2); + double det = Point::determinant(v0, v1); + double dist = v0.magnitude2() + v1.magnitude2(); + double dist2 = Point::dist2(v0, v1); + if(det == 0) + { + return false; + } + double reldet = std::abs(dist / det); + if(reldet > 1e14) + return false; + return det < 0; +} + +inline bool clockwise(double px, double py, double qx, double qy, double rx, double ry) +{ + Point p0(px, py); + Point p1(qx, qy); + Point p2(rx, ry); + return clockwise(p0, p1, p2); +} + +inline bool counterclockwise(const Point& p0, const Point& p1, const Point& p2) +{ + Point v0 = Point::vector(p0, p1); + Point v1 = Point::vector(p0, p2); + double det = Point::determinant(v0, v1); + double dist = v0.magnitude2() + v1.magnitude2(); + double dist2 = Point::dist2(v0, v1); + if(det == 0) + return false; + double reldet = std::abs(dist / det); + if(reldet > 1e14) + return false; + return det > 0; +} + +inline bool counterclockwise(double px, double py, double qx, double qy, double rx, double ry) +{ + Point p0(px, py); + Point p1(qx, qy); + Point p2(rx, ry); + return counterclockwise(p0, p1, p2); +} + +inline Point circumcenter(const double ax, const double ay, const double bx, const double by, const double cx, const double cy) +{ + const double dx = bx - ax; + const double dy = by - ay; + const double ex = cx - ax; + const double ey = cy - ay; + + const double bl = dx * dx + dy * dy; + const double cl = ex * ex + ey * ey; + // ABELL - This is suspect for div-by-0. + const double d = dx * ey - dy * ex; + + const double x = ax + (ey * bl - dy * cl) * 0.5 / d; + const double y = ay + (dx * cl - ex * bl) * 0.5 / d; + + return Point(x, y); +} + +inline bool in_circle(const double ax, const double ay, const double bx, const double by, const double cx, const double cy, const double px, const double py) +{ + const double dx = ax - px; + const double dy = ay - py; + const double ex = bx - px; + const double ey = by - py; + const double fx = cx - px; + const double fy = cy - py; + + const double ap = dx * dx + dy * dy; + const double bp = ex * ex + ey * ey; + const double cp = fx * fx + fy * fy; + + return (dx * (ey * cp - bp * fy) - dy * (ex * cp - bp * fx) + ap * (ex * fy - ey * fx)) < 0.0; +} + +constexpr double EPSILON = std::numeric_limits::epsilon(); + +inline bool check_pts_equal(double x1, double y1, double x2, double y2) +{ + return std::fabs(x1 - x2) <= EPSILON && std::fabs(y1 - y2) <= EPSILON; +} + +// monotonically increases with real angle, but doesn't need expensive trigonometry +inline double pseudo_angle(const double dx, const double dy) +{ + const double p = dx / (std::abs(dx) + std::abs(dy)); + return (dy > 0.0 ? 3.0 - p : 1.0 + p) / 4.0; // [0..1) +} + +Delaunator::Delaunator(std::vector const& in_coords) +: coords(in_coords) +, m_points(in_coords) +{ + std::size_t n = coords.size() >> 1; + + std::vector ids(n); + std::iota(ids.begin(), ids.end(), 0); + + double max_x = std::numeric_limits::lowest(); + double max_y = std::numeric_limits::lowest(); + double min_x = (std::numeric_limits::max)(); + double min_y = (std::numeric_limits::max)(); + for(const Point& p : m_points) + { + min_x = std::min(p.x(), min_x); + min_y = std::min(p.y(), min_y); + max_x = std::max(p.x(), max_x); + max_y = std::max(p.y(), max_y); + } + double width = max_x - min_x; + double height = max_y - min_y; + double span = width * width + height * height; // Everything is square dist. + + Point center((min_x + max_x) / 2, (min_y + max_y) / 2); + + std::size_t i0 = INVALID_INDEX; + std::size_t i1 = INVALID_INDEX; + std::size_t i2 = INVALID_INDEX; + + // pick a seed point close to the centroid + double min_dist = (std::numeric_limits::max)(); + for(size_t i = 0; i < m_points.size(); ++i) + { + const Point& p = m_points[i]; + const double d = Point::dist2(center, p); + if(d < min_dist) + { + i0 = i; + min_dist = d; + } + } + + const Point& p0 = m_points[i0]; + + min_dist = (std::numeric_limits::max)(); + + // find the point closest to the seed + for(std::size_t i = 0; i < n; i++) + { + if(i == i0) + continue; + const double d = Point::dist2(p0, m_points[i]); + if(d < min_dist && d > 0.0) + { + i1 = i; + min_dist = d; + } + } + + const Point& p1 = m_points[i1]; + + double min_radius = (std::numeric_limits::max)(); + + // find the third point which forms the smallest circumcircle + // with the first two + for(std::size_t i = 0; i < n; i++) + { + if(i == i0 || i == i1) + continue; + + const double r = circumradius(p0, p1, m_points[i]); + if(r < min_radius) + { + i2 = i; + min_radius = r; + } + } + + if(!(min_radius < (std::numeric_limits::max)())) + { + throw std::runtime_error("not triangulation"); + } + + const Point& p2 = m_points[i2]; + + if(counterclockwise(p0, p1, p2)) + std::swap(i1, i2); + + double i0x = p0.x(); + double i0y = p0.y(); + double i1x = m_points[i1].x(); + double i1y = m_points[i1].y(); + double i2x = m_points[i2].x(); + double i2y = m_points[i2].y(); + + m_center = circumcenter(i0x, i0y, i1x, i1y, i2x, i2y); + + // Calculate the distances from the center once to avoid having to + // calculate for each compare. This used to be done in the comparator, + // but GCC 7.5+ would copy the comparator to iterators used in the + // sort, and this was excruciatingly slow when there were many points + // because you had to copy the vector of distances. + std::vector dists; + dists.reserve(m_points.size()); + for(const Point& p : m_points) + dists.push_back(dist(p.x(), p.y(), m_center.x(), m_center.y())); + + // sort the points by distance from the seed triangle circumcenter + std::sort(ids.begin(), ids.end(), [&dists](std::size_t i, std::size_t j) { return dists[i] < dists[j]; }); + + // initialize a hash table for storing edges of the advancing convex hull + m_hash_size = static_cast(std::ceil(std::sqrt(n))); + m_hash.resize(m_hash_size); + std::fill(m_hash.begin(), m_hash.end(), INVALID_INDEX); + + // initialize arrays for tracking the edges of the advancing convex hull + hull_prev.resize(n); + hull_next.resize(n); + hull_tri.resize(n); + + hull_start = i0; + + size_t hull_size = 3; + + hull_next[i0] = hull_prev[i2] = i1; + hull_next[i1] = hull_prev[i0] = i2; + hull_next[i2] = hull_prev[i1] = i0; + + hull_tri[i0] = 0; + hull_tri[i1] = 1; + hull_tri[i2] = 2; + + m_hash[hash_key(i0x, i0y)] = i0; + m_hash[hash_key(i1x, i1y)] = i1; + m_hash[hash_key(i2x, i2y)] = i2; + + // ABELL - Why are we doing this is n < 3? There is no triangulation if + // there is no triangle. + + std::size_t max_triangles = n < 3 ? 1 : 2 * n - 5; + triangles.reserve(max_triangles * 3); + halfedges.reserve(max_triangles * 3); + add_triangle(i0, i1, i2, INVALID_INDEX, INVALID_INDEX, INVALID_INDEX); + double xp = std::numeric_limits::quiet_NaN(); + double yp = std::numeric_limits::quiet_NaN(); + + // Go through points based on distance from the center. + for(std::size_t k = 0; k < n; k++) + { + const std::size_t i = ids[k]; + const double x = coords[2 * i]; + const double y = coords[2 * i + 1]; + + // skip near-duplicate points + if(k > 0 && check_pts_equal(x, y, xp, yp)) + continue; + xp = x; + yp = y; + + // ABELL - This is dumb. We have the indices. Use them. + // skip seed triangle points + if(check_pts_equal(x, y, i0x, i0y) || check_pts_equal(x, y, i1x, i1y) || check_pts_equal(x, y, i2x, i2y)) + continue; + + // find a visible edge on the convex hull using edge hash + std::size_t start = 0; + + size_t key = hash_key(x, y); + for(size_t j = 0; j < m_hash_size; j++) + { + start = m_hash[fast_mod(key + j, m_hash_size)]; + + // ABELL - Not sure how hull_next[start] could ever equal start + // I *think* hull_next is just a representation of the hull in one + // direction. + if(start != INVALID_INDEX && start != hull_next[start]) + break; + } + + // ABELL + // Make sure what we found is on the hull. + assert(hull_prev[start] != start); + assert(hull_prev[start] != INVALID_INDEX); + + start = hull_prev[start]; + size_t e = start; + size_t q; + + // Advance until we find a place in the hull where our current point + // can be added. + while(true) + { + q = hull_next[e]; + if(Point::equal(m_points[i], m_points[e], span) || Point::equal(m_points[i], m_points[q], span)) + { + e = INVALID_INDEX; + break; + } + if(counterclockwise(x, y, coords[2 * e], coords[2 * e + 1], coords[2 * q], coords[2 * q + 1])) + break; + e = q; + if(e == start) + { + e = INVALID_INDEX; + break; + } + } + + // ABELL + // This seems wrong. Perhaps we should check what's going on? + if(e == INVALID_INDEX) // likely a near-duplicate point; skip it + continue; + + // add the first triangle from the point + std::size_t t = add_triangle(e, i, hull_next[e], INVALID_INDEX, INVALID_INDEX, hull_tri[e]); + + hull_tri[i] = legalize(t + 2); // Legalize the triangle we just added. + hull_tri[e] = t; + hull_size++; + + // walk forward through the hull, adding more triangles and + // flipping recursively + std::size_t next = hull_next[e]; + while(true) + { + q = hull_next[next]; + if(!counterclockwise(x, y, coords[2 * next], coords[2 * next + 1], coords[2 * q], coords[2 * q + 1])) + break; + t = add_triangle(next, i, q, hull_tri[i], INVALID_INDEX, hull_tri[next]); + hull_tri[i] = legalize(t + 2); + hull_next[next] = next; // mark as removed + hull_size--; + next = q; + } + + // walk backward from the other side, adding more triangles and flipping + if(e == start) + { + while(true) + { + q = hull_prev[e]; + if(!counterclockwise(x, y, coords[2 * q], coords[2 * q + 1], coords[2 * e], coords[2 * e + 1])) + break; + t = add_triangle(q, i, e, INVALID_INDEX, hull_tri[e], hull_tri[q]); + legalize(t + 2); + hull_tri[q] = t; + hull_next[e] = e; // mark as removed + hull_size--; + e = q; + } + } + + // update the hull indices + hull_prev[i] = e; + hull_start = e; + hull_prev[next] = i; + hull_next[e] = i; + hull_next[i] = next; + + m_hash[hash_key(x, y)] = i; + m_hash[hash_key(coords[2 * e], coords[2 * e + 1])] = e; + } +} + +double Delaunator::get_hull_area() +{ + std::vector hull_area; + size_t e = hull_start; + size_t cnt = 1; + do + { + hull_area.push_back((coords[2 * e] - coords[2 * hull_prev[e]]) * (coords[2 * e + 1] + coords[2 * hull_prev[e] + 1])); + cnt++; + e = hull_next[e]; + } while(e != hull_start); + return sum(hull_area); +} + +double Delaunator::get_triangle_area() +{ + std::vector vals; + for(size_t i = 0; i < triangles.size(); i += 3) + { + const double ax = coords[2 * triangles[i]]; + const double ay = coords[2 * triangles[i] + 1]; + const double bx = coords[2 * triangles[i + 1]]; + const double by = coords[2 * triangles[i + 1] + 1]; + const double cx = coords[2 * triangles[i + 2]]; + const double cy = coords[2 * triangles[i + 2] + 1]; + double val = std::fabs((by - ay) * (cx - bx) - (bx - ax) * (cy - by)); + vals.push_back(val); + } + return sum(vals); +} + +std::size_t Delaunator::legalize(std::size_t a) +{ + std::size_t i = 0; + std::size_t ar = 0; + m_edge_stack.clear(); + + // recursion eliminated with a fixed-size stack + while(true) + { + const size_t b = halfedges[a]; + + /* if the pair of triangles doesn't satisfy the Delaunay condition + * (p1 is inside the circumcircle of [p0, pl, pr]), flip them, + * then do the same check/flip recursively for the new pair of triangles + * + * pl pl + * /||\ / \ + * al/ || \bl al/ \a + * / || \ / \ + * / a||b \ flip /___ar___\ + * p0\ || /p1 => p0\---bl---/p1 + * \ || / \ / + * ar\ || /br b\ /br + * \||/ \ / + * pr pr + */ + const size_t a0 = 3 * (a / 3); + ar = a0 + (a + 2) % 3; + + if(b == INVALID_INDEX) + { + if(i > 0) + { + i--; + a = m_edge_stack[i]; + continue; + } + else + { + // i = INVALID_INDEX; + break; + } + } + + const size_t b0 = 3 * (b / 3); + const size_t al = a0 + (a + 1) % 3; + const size_t bl = b0 + (b + 2) % 3; + + const std::size_t p0 = triangles[ar]; + const std::size_t pr = triangles[a]; + const std::size_t pl = triangles[al]; + const std::size_t p1 = triangles[bl]; + + const bool illegal = in_circle(coords[2 * p0], coords[2 * p0 + 1], coords[2 * pr], coords[2 * pr + 1], coords[2 * pl], coords[2 * pl + 1], coords[2 * p1], coords[2 * p1 + 1]); + + if(illegal) + { + triangles[a] = p1; + triangles[b] = p0; + + auto hbl = halfedges[bl]; + + // Edge swapped on the other side of the hull (rare). + // Fix the halfedge reference + if(hbl == INVALID_INDEX) + { + std::size_t e = hull_start; + do + { + if(hull_tri[e] == bl) + { + hull_tri[e] = a; + break; + } + e = hull_prev[e]; + } while(e != hull_start); + } + link(a, hbl); + link(b, halfedges[ar]); + link(ar, bl); + std::size_t br = b0 + (b + 1) % 3; + + if(i < m_edge_stack.size()) + { + m_edge_stack[i] = br; + } + else + { + m_edge_stack.push_back(br); + } + i++; + } + else + { + if(i > 0) + { + i--; + a = m_edge_stack[i]; + continue; + } + else + { + break; + } + } + } + return ar; +} + +std::size_t Delaunator::hash_key(const double x, const double y) const +{ + const double dx = x - m_center.x(); + const double dy = y - m_center.y(); + return fast_mod(static_cast(std::llround(std::floor(pseudo_angle(dx, dy) * static_cast(m_hash_size)))), m_hash_size); +} + +std::size_t Delaunator::add_triangle(std::size_t i0, std::size_t i1, std::size_t i2, std::size_t a, std::size_t b, std::size_t c) +{ + std::size_t t = triangles.size(); + triangles.push_back(i0); + triangles.push_back(i1); + triangles.push_back(i2); + link(t, a); + link(t + 1, b); + link(t + 2, c); + return t; +} + +void Delaunator::link(const std::size_t a, const std::size_t b) +{ + std::size_t s = halfedges.size(); + if(a == s) + { + halfedges.push_back(b); + } + else if(a < s) + { + halfedges[a] = b; + } + else + { + throw std::runtime_error("Cannot link edge"); + } + if(b != INVALID_INDEX) + { + std::size_t s2 = halfedges.size(); + if(b == s2) + { + halfedges.push_back(a); + } + else if(b < s2) + { + halfedges[b] = a; + } + else + { + throw std::runtime_error("Cannot link edge"); + } + } +} + +} // namespace nx::delaunator diff --git a/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/delaunator.h b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/delaunator.h new file mode 100644 index 0000000000..5b7fbbddd9 --- /dev/null +++ b/src/Plugins/OrientationAnalysis/src/OrientationAnalysis/utilities/delaunator.h @@ -0,0 +1,188 @@ +/* +* MIT License + +Copyright (c) 2018 Volodymyr Bilonenko + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + */ + +#pragma once + +#ifdef DELAUNATOR_HEADER_ONLY +#define INLINE inline +#else +#define INLINE +#endif + +#include +#include +#include + +namespace nx +{ + +namespace delaunator +{ + +constexpr std::size_t INVALID_INDEX = (std::numeric_limits::max)(); + +class Point +{ +public: + Point(double x, double y) + : m_x(x) + , m_y(y) + { + } + Point() + : m_x(0) + , m_y(0) + { + } + + double x() const + { + return m_x; + } + + double y() const + { + return m_y; + } + + double magnitude2() const + { + return m_x * m_x + m_y * m_y; + } + + static double determinant(const Point& p1, const Point& p2) + { + return p1.m_x * p2.m_y - p1.m_y * p2.m_x; + } + + static Point vector(const Point& p1, const Point& p2) + { + return Point(p2.m_x - p1.m_x, p2.m_y - p1.m_y); + } + + static double dist2(const Point& p1, const Point& p2) + { + Point vec = vector(p1, p2); + return vec.m_x * vec.m_x + vec.m_y * vec.m_y; + } + + static bool equal(const Point& p1, const Point& p2, double span) + { + double dist = dist2(p1, p2) / span; + + // ABELL - This number should be examined to figure how how + // it correlates with the breakdown of calculating determinants. + return dist < 1e-20; + } + +private: + double m_x; + double m_y; +}; + +inline std::ostream& operator<<(std::ostream& out, const Point& p) +{ + out << p.x() << "/" << p.y(); + return out; +} + +class Points +{ +public: + using const_iterator = Point const*; + + Points(const std::vector& coords) + : m_coords(coords) + { + } + + const Point& operator[](size_t offset) + { + return reinterpret_cast(*(m_coords.data() + (offset * 2))); + }; + + Points::const_iterator begin() const + { + return reinterpret_cast(m_coords.data()); + } + Points::const_iterator end() const + { + return reinterpret_cast(m_coords.data() + m_coords.size()); + } + size_t size() const + { + return m_coords.size() / 2; + } + +private: + const std::vector& m_coords; +}; + +class Delaunator +{ + +public: + std::vector const& coords; + Points m_points; + + // 'triangles' stores the indices to the 'X's of the input + // 'coords'. + std::vector triangles; + + // 'halfedges' store indices into 'triangles'. If halfedges[X] = Y, + // It says that there's an edge from X to Y where a) X and Y are + // both indices into triangles and b) X and Y are indices into different + // triangles in the array. This allows you to get from a triangle to + // its adjacent triangle. If the a triangle edge has no adjacent triangle, + // its half edge will be INVALID_INDEX. + std::vector halfedges; + + std::vector hull_prev; + std::vector hull_next; + + // This contains indexes into the triangles array. + std::vector hull_tri; + std::size_t hull_start; + + INLINE Delaunator(std::vector const& in_coords); + INLINE double get_hull_area(); + INLINE double get_triangle_area(); + +private: + std::vector m_hash; + Point m_center; + std::size_t m_hash_size; + std::vector m_edge_stack; + + INLINE std::size_t legalize(std::size_t a); + INLINE std::size_t hash_key(double x, double y) const; + INLINE std::size_t add_triangle(std::size_t i0, std::size_t i1, std::size_t i2, std::size_t a, std::size_t b, std::size_t c); + INLINE void link(std::size_t a, std::size_t b); +}; + +} // namespace delaunator + +} // namespace nx + +#undef INLINE