From e46e4c71df3690ef8c1dcded4e9f80934ee9d417 Mon Sep 17 00:00:00 2001 From: vbd Date: Mon, 30 Jul 2007 07:16:45 +0000 Subject: [PATCH] Staffan : Added files recovered from emacs buffers after deletion on harddrive --- src/INTERP_KERNEL/Interpolation3D.cxx | 317 +++++++ src/INTERP_KERNEL/Interpolation3D.hxx | 19 +- src/INTERP_KERNEL/InterpolationUtils.hxx | 6 +- src/INTERP_KERNEL/Test/CppUnitTest.hxx | 2 +- .../Test/Interpolation3DTest.cxx | 237 +++++ .../Test/Interpolation3DTest.hxx | 59 ++ src/INTERP_KERNEL/TetraAffineTransform.cxx | 1 + src/INTERP_KERNEL/TetraAffineTransform.hxx | 263 ++++++ src/INTERP_KERNEL/TransformedTriangle.cxx | 443 ++++++++- src/INTERP_KERNEL/TransformedTriangle.hxx | 43 +- .../TransformedTriangle_intersect.cxx | 872 ++++++++---------- src/INTERP_KERNEL/VectorUtils.hxx | 61 ++ 12 files changed, 1817 insertions(+), 506 deletions(-) create mode 100644 src/INTERP_KERNEL/Interpolation3D.cxx create mode 100644 src/INTERP_KERNEL/Test/Interpolation3DTest.cxx create mode 100644 src/INTERP_KERNEL/Test/Interpolation3DTest.hxx create mode 100644 src/INTERP_KERNEL/TetraAffineTransform.cxx create mode 100644 src/INTERP_KERNEL/TetraAffineTransform.hxx create mode 100644 src/INTERP_KERNEL/VectorUtils.hxx diff --git a/src/INTERP_KERNEL/Interpolation3D.cxx b/src/INTERP_KERNEL/Interpolation3D.cxx new file mode 100644 index 000000000..6caec3d0f --- /dev/null +++ b/src/INTERP_KERNEL/Interpolation3D.cxx @@ -0,0 +1,317 @@ +#include "Interpolation3D.hxx" + +#include + +#include "MeshElement.hxx" +#include "MeshRegion.hxx" +#include "RegionNode.hxx" +#include "TetraAffineTransform.hxx" +#include "TransformedTriangle.hxx" + +using namespace INTERP_UTILS; +using namespace MEDMEM; +using namespace MED_EN; + +namespace MEDMEM +{ + + /** + * Default constructor + * + */ + Interpolation3D::Interpolation3D() + { + // not implemented + } + + /** + * Destructor + * + */ + Interpolation3D::~Interpolation3D() + { + // not implemented + } + + /** + * Main interface method of the class, which verifies that the meshes are valid for the calculation, + * and then returns the matrix of intersection volumes. + * + * @param srcMesh 3-dimensional source mesh + * @param targetMesh 3-dimesional target mesh, containing only tetraedra + * @returns vector containing for each element i of the source mesh, a map giving for each element j + * of the target mesh which i intersects, the volume of the intersection + */ + IntersectionMatrix Interpolation3D::interpol_maillages(const MEDMEM::MESH& srcMesh, const MEDMEM::MESH& targetMesh) + { + //} it seems wasteful to make a copy here + IntersectionMatrix matrix; + + // we should maybe do more sanity checking here - eliminating meshes that + // are too complicated + + calculateIntersectionVolumes(srcMesh, targetMesh, matrix); + return matrix; + } + + /** + * Performs a depth-first search over srcMesh, using bounding boxes to recursively eliminate the elements of targetMesh + * which cannot intersect smaller and smaller regions of srcMesh. At each level, each region is divided in two, forming + * a binary search tree with leaves consisting of only one element of the source mesh together with the elements of the + * target mesh that can intersect it. The recursion is implemented with a stack of RegionNodes, each one containing a + * source region and a target region. Each region has an associated bounding box and a vector of pointers to the elements + * that belong to it. Each element contains a bounding box, an element type and an index in the MEDMEM ConnectivityIndex array. + * + * @param srcMesh 3-dimensional source mesh + * @param targetMesh 3-dimesional target mesh, containing only tetraedra + * @param matrix vector of maps in which the result is stored + * + */ + void Interpolation3D::calculateIntersectionVolumes(const MEDMEM::MESH& srcMesh, const MEDMEM::MESH& targetMesh, IntersectionMatrix& matrix) + { + // calculate descending connectivities + srcMesh.calculateConnectivity(MED_FULL_INTERLACE, MED_DESCENDING, MED_CELL); + targetMesh.calculateConnectivity(MED_FULL_INTERLACE, MED_DESCENDING, MED_CELL); + + // create MeshElement objects corresponding to each element of the two meshes + + const int numSrcElems = srcMesh.getNumberOfElements(MED_CELL, MED_ALL_ELEMENTS); + const int numTargetElems = targetMesh.getNumberOfElements(MED_CELL, MED_ALL_ELEMENTS); + + // create empty maps for all source elements + matrix.resize(numSrcElems); + + MeshElement* srcElems[numSrcElems]; + MeshElement* targetElems[numTargetElems]; + + std::map indices; + + for(int i = 0 ; i < numSrcElems ; ++i) + { + //const int index = srcMesh.getConnectivityIndex(MED_NODAL, MED_CELL)[i]; + const medGeometryElement type = srcMesh.getElementType(MED_CELL, i + 1); + srcElems[i] = new MeshElement(i + 1, type, srcMesh); + + } + + for(int i = 0 ; i < numTargetElems ; ++i) + { + // const int index = targetMesh.getConnectivityIndex(MED_NODAL, MED_CELL)[i]; + const medGeometryElement type = targetMesh.getElementType(MED_CELL, i + 1); + targetElems[i] = new MeshElement(i + 1, type, targetMesh); + } + + // create initial RegionNode and fill up its source region with all the source mesh elements and + // its target region with all the target mesh elements whose bbox intersects that of the source region + + RegionNode* firstNode = new RegionNode(); + + MeshRegion& srcRegion = firstNode->getSrcRegion(); + + for(int i = 0 ; i < numSrcElems ; ++i) + { + srcRegion.addElement(srcElems[i]); + } + + MeshRegion& targetRegion = firstNode->getTargetRegion(); + + for(int i = 0 ; i < numTargetElems ; ++i) + { + if(!srcRegion.isDisjointWithElementBoundingBox( *(targetElems[i]) )) + { + targetRegion.addElement(targetElems[i]); + } + } + + // using a stack, descend recursively, creating at each step two new RegionNodes having as source region the left and + // right part of the source region of the current node (determined using MeshRegion::split()) and as target region all the + // elements of the target mesh whose bounding box intersects the corresponding part + // Continue until the source region contains only one element, at which point the intersection volumes are + // calculated with all the remaining target mesh elements and stored in the matrix if they are non-zero. + + stack nodes; + nodes.push(firstNode); + + while(!nodes.empty()) + { + RegionNode* currNode = nodes.top(); + nodes.pop(); + // std::cout << "Popping node " << std::endl; + + if(currNode->getSrcRegion().getNumberOfElements() == 1) + { + // std::cout << " - One element" << std::endl; + + // volume calculation + MeshElement* srcElement = *(currNode->getSrcRegion().getBeginElements()); + + // NB : srcElement indices are from 0 .. numSrcElements - 1 + // targetElement indicies from 1 .. numTargetElements + // maybe this is not ideal ... + const int srcIdx = srcElement->getIndex() - 1; + std::map< int, double >* volumes = &(matrix[srcIdx]); + + for(vector::const_iterator iter = currNode->getTargetRegion().getBeginElements() ; + iter != currNode->getTargetRegion().getEndElements() ; ++iter) + { + const double vol = calculateIntersectionVolume(*srcElement, **iter); + if(vol != 0.0) + { + const int targetIdx = (*iter)->getIndex(); + + volumes->insert(make_pair(targetIdx, vol)); + std::cout << "Result : V (" << srcIdx << ", " << targetIdx << ") = " << matrix[srcIdx][targetIdx] << std::endl; + } + } + } + else // recursion + { + typedef MeshElement::ZoneBBoxComparison Cmp; + // std::cout << " - Recursion" << std::endl; + + RegionNode* leftNode = new RegionNode(); + RegionNode* rightNode = new RegionNode(); + + // split current source region + //} decide on axis + static Cmp::BoxAxis axis = Cmp::X; + + currNode->getSrcRegion().split(leftNode->getSrcRegion(), rightNode->getSrcRegion(), axis); + + // ugly hack to avoid problem with enum which does not start at 0 + // I guess I ought to implement ++ for it instead ... + // Anyway, it basically chooses the next axis, circually + axis = (axis != Cmp::Z) ? static_cast(axis + 1) : Cmp::X; + + // add target elements of curr node that overlap the two new nodes + // std::cout << " -- Adding target elements" << std::endl; + int numLeftElements = 0; + int numRightElements = 0; + for(vector::const_iterator iter = currNode->getTargetRegion().getBeginElements() ; + iter != currNode->getTargetRegion().getEndElements() ; ++iter) + { + // std::cout << " --- New target node" << std::endl; + + if(!leftNode->getSrcRegion().isDisjointWithElementBoundingBox(**iter)) + { + leftNode->getTargetRegion().addElement(*iter); + ++numLeftElements; + } + + if(!rightNode->getSrcRegion().isDisjointWithElementBoundingBox(**iter)) + { + rightNode->getTargetRegion().addElement(*iter); + ++numRightElements; + } + + } + + if(numLeftElements != 0) + { + nodes.push(leftNode); + } + else + { + delete leftNode; + } + + if(numRightElements != 0) + { + nodes.push(rightNode); + } + else + { + delete rightNode; + } + + + } + + delete currNode; + // std::cout << "Next iteration. Nodes left : " << nodes.size() << std::endl; + } + + + + // free allocated memory + for(int i = 0 ; i < numSrcElems ; ++i) + { + delete srcElems[i]; + } + for(int i = 0 ; i < numTargetElems ; ++i) + { + delete targetElems[i]; + } + + } + + /** + * Calculates volume of intersection between an element of the source mesh and an element of the target mesh. + * The calculation passes by the following steps : + * a) test if srcElement is in the interior of targetElement -> if yes, return volume of srcElement + * --> test by call to Element::isElementIncludedIn + * b) test if targetElement is in the interior of srcElement -> if yes return volume of targetElement + * --> test by call to Element::isElementIncludedIn + * c) test if srcElement and targetElement are disjoint -> if yes return 0.0 [this test is only possible if srcElement is convex] + * --> test by call to Element::isElementTriviallyDisjointWith + * d) (1) find transformation M that takes the target element (a tetraeder) to the unit tetraeder + * --> call calculateTransform() + * (2) divide srcElement in triangles + * --> call triangulateElement() + * (3) for each triangle in element, + * (i) find polygones --> calculateIntersectionPolygones() + * (ii) calculate volume under polygones --> calculateVolumeUnderPolygone() + * (iii) add volume to sum + * (4) return det(M)*sumVolume (det(M) = 6*vol(targetElement)) + * + * @param srcElement + * @param targetElement + * @returns volume of intersection between srcElement and targetElement + * + */ + double Interpolation3D::calculateIntersectionVolume(const MeshElement& srcElement, const MeshElement& targetElement) + { + + // std::cout << "Intersecting elems " << srcElement.getIndex() << " and " << targetElement.getIndex() << std::endl; + // (a), (b) and (c) not yet implemented + + // (d) : without fine-level filtering (a) - (c) for the time being + + // get array of points of target tetraeder + const double* tetraCorners[4]; + for(int i = 0 ; i < 4 ; ++i) + { + tetraCorners[i] = targetElement.getCoordsOfNode(i + 1); + } + + // create AffineTransform + TetraAffineTransform T( tetraCorners ); + // T.dump(); + + // triangulate source element faces (assumed tetraeder for the time being) + // do nothing + vector triangles; + srcElement.triangulate(triangles, T); + + double volume = 0.0; + + // std::cout << "num triangles = " << triangles.size() << std::endl; + + for(vector::iterator iter = triangles.begin() ; iter != triangles.end(); ++iter) + { + // std::cout << std::endl << "= > Triangle " << ++i << std::endl; + iter->dumpCoords(); + volume += iter->calculateIntersectionVolume(); + } + + std::cout << "Volume = " << volume << ", det= " << T.determinant() << std::endl; + + //? trying without abs to see if the sign of the determinant will always cancel that of the volume + //? but maybe we should take abs( det ( T ) ) or abs ( 1 / det * vol ) + + //? fault in article, Grandy, [8] : it is the determinant of the inverse transformation that + // should be used + return 1.0 / T.determinant() * volume ; + + } +} diff --git a/src/INTERP_KERNEL/Interpolation3D.hxx b/src/INTERP_KERNEL/Interpolation3D.hxx index fba170134..40246802e 100644 --- a/src/INTERP_KERNEL/Interpolation3D.hxx +++ b/src/INTERP_KERNEL/Interpolation3D.hxx @@ -3,7 +3,7 @@ // MEDMEM includes #include "Interpolation.hxx" - +#include "MEDMEM_Mesh.hxx" // standard includes #include @@ -14,6 +14,17 @@ // typedefs typedef std::vector< std::map< int, double > > IntersectionMatrix; + +#ifdef TESTING_INTERP_KERNEL +class Interpolation3DTest; +#endif + +namespace INTERP_UTILS +{ + class MeshElement; + class MeshRegion; +}; + namespace MEDMEM { /** @@ -23,6 +34,10 @@ namespace MEDMEM class Interpolation3D : public Interpolation { +#ifdef TESTING_INTERP_KERNEL + friend class ::Interpolation3DTest; +#endif + public : /** @@ -90,7 +105,7 @@ namespace MEDMEM * @returns volume of intersection between srcElement and targetElement * */ - double calculateIntersectionVolume(const Element srcElement&, const Element targetElement); + double calculateIntersectionVolume(const INTERP_UTILS::MeshElement& srcElement, const INTERP_UTILS::MeshElement& targetElement); }; diff --git a/src/INTERP_KERNEL/InterpolationUtils.hxx b/src/INTERP_KERNEL/InterpolationUtils.hxx index 65fedf9fd..59917a12d 100644 --- a/src/INTERP_KERNEL/InterpolationUtils.hxx +++ b/src/INTERP_KERNEL/InterpolationUtils.hxx @@ -11,7 +11,6 @@ #include #include - namespace INTERP_UTILS { // const double HUGE=1e300; @@ -458,7 +457,7 @@ namespace INTERP_UTILS /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */ - /* fonction pour reconstituer un polygone convexe à partir */ +* /* fonction pour reconstituer un polygone convexe à partir */ /* d'un nuage de point. */ /*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ */ @@ -549,5 +548,8 @@ namespace INTERP_UTILS } + + + }; #endif diff --git a/src/INTERP_KERNEL/Test/CppUnitTest.hxx b/src/INTERP_KERNEL/Test/CppUnitTest.hxx index 3bd1179af..ba0618ddd 100644 --- a/src/INTERP_KERNEL/Test/CppUnitTest.hxx +++ b/src/INTERP_KERNEL/Test/CppUnitTest.hxx @@ -15,7 +15,7 @@ private: double x; }; -// This test has no use + class TestBogusClass : public CppUnit::TestFixture { diff --git a/src/INTERP_KERNEL/Test/Interpolation3DTest.cxx b/src/INTERP_KERNEL/Test/Interpolation3DTest.cxx new file mode 100644 index 000000000..a00738c20 --- /dev/null +++ b/src/INTERP_KERNEL/Test/Interpolation3DTest.cxx @@ -0,0 +1,237 @@ +#include "Interpolation3DTest.hxx" +#include "MEDMEM_Mesh.hxx" + +#include +#include +#include +#include + +using namespace MEDMEM; +using namespace std; + +double Interpolation3DTest::sumVolume(IntersectionMatrix m) +{ + double vol = 0.0; + for(IntersectionMatrix::const_iterator iter = m.begin() ; iter != m.end() ; ++iter) + { + for(map::const_iterator iter2 = iter->begin() ; iter2 != iter->end() ; ++iter2) + { + vol += fabs(iter2->second); + } + } + return vol; +} + +bool Interpolation3DTest::isIntersectionConsistent(IntersectionMatrix m) +{ + bool res = true; + int i = 0; + for(IntersectionMatrix::const_iterator iter = m.begin() ; iter != m.end() ; ++iter) + { + for(map::const_iterator iter2 = iter->begin() ; iter2 != iter->end() ; ++iter2) + { + // volume should be between 0 and 1 / 6 + if(iter2->second < 0.0 - ERR_TOL || iter2->second > 1.0 / 6.0 + ERR_TOL) + { + cout << "Inconsistent intersection volume : V(" << i << ", " << iter2->first << ") = " << iter2->second << endl; + res = false; + } + } + ++i; + } + return res; +} + +void Interpolation3DTest::dumpIntersectionMatrix(IntersectionMatrix m) +{ + int i = 0; + for(IntersectionMatrix::const_iterator iter = m.begin() ; iter != m.end() ; ++iter) + { + for(map::const_iterator iter2 = iter->begin() ; iter2 != iter->end() ; ++iter2) + { + + cout << "V(" << i << ", " << iter2->first << ") = " << iter2->second << endl; + + } + ++i; + } +} + +void Interpolation3DTest::setUp() +{ + interpolator = new Interpolation3D(); +} + +void Interpolation3DTest::tearDown() +{ + delete interpolator; +} + +void Interpolation3DTest::reflexiveTetra() +{ + std::cout << std::endl << std::endl << "=============================" << std::endl; + std::cout << " Reflexive tetra " << endl; + MESH unitMesh(MED_DRIVER, "meshes/tetra1.med", "Mesh_1"); + + std::cout << std::endl << "*** unit tetra" << std::endl; + IntersectionMatrix matrix1 = interpolator->interpol_maillages(unitMesh, unitMesh); + + std::cout << std::endl << "*** non-unit large tetra" << std::endl; + MESH largeMesh(MED_DRIVER, "meshes/tetra2.med", "Mesh_1"); + IntersectionMatrix matrix2 = interpolator->interpol_maillages(largeMesh, largeMesh); + + std::cout << std::endl << "*** non-unit small tetra" << std::endl; + MESH smallMesh(MED_DRIVER, "meshes/tetra2_scaled.med", "Mesh_2"); + IntersectionMatrix matrix3 = interpolator->interpol_maillages(smallMesh, smallMesh); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0 / 6.0, sumVolume(matrix1), ERR_TOL); + CPPUNIT_ASSERT_DOUBLES_EQUAL(48.0, sumVolume(matrix2), ERR_TOL); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.75, sumVolume(matrix3), ERR_TOL); + +} + +void Interpolation3DTest::tetraTetraTransl() +{ + std::cout << std::endl << std::endl << "=============================" << std::endl; + std::cout << " Translated tetra " << endl; + MESH srcMesh(MED_DRIVER, "meshes/tetra1.med", "Mesh_1"); + + MESH targetMesh(MED_DRIVER, "meshes/tetra1_transl_delta.med", "Mesh_3"); + + std::cout << std::endl << "*** src - target" << std::endl; + IntersectionMatrix matrix1 = interpolator->interpol_maillages(srcMesh, targetMesh); + std::cout << std::endl << std::endl << "*** target - src" << std::endl; + IntersectionMatrix matrix2 = interpolator->interpol_maillages(targetMesh, srcMesh); + std::cout << std::endl << std::endl << "*** target - target" << std::endl; + IntersectionMatrix matrix3 = interpolator->interpol_maillages(targetMesh, targetMesh); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.152112, sumVolume(matrix1), 1.0e-6); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.152112, sumVolume(matrix2), 1.0e-6); + CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0 / 6.0, sumVolume(matrix3), ERR_TOL); +} + +void Interpolation3DTest::tetraTetraScale() +{ + std::cout << std::endl << std::endl << "=============================" << std::endl; + std::cout << " Scaled included tetra " << endl; + + MESH srcMesh(MED_DRIVER, "meshes/tetra2.med", "Mesh_1"); + MESH targetMesh(MED_DRIVER, "meshes/tetra2_scaled.med", "Mesh_2"); + + std::cout << "*** src - target" << std::endl; + IntersectionMatrix matrix1 = interpolator->interpol_maillages(srcMesh, targetMesh); + std::cout << std::endl << "*** target - src" << std::endl; + IntersectionMatrix matrix2 = interpolator->interpol_maillages(targetMesh, srcMesh); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.75, sumVolume(matrix1), 1.0e-6); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.75, sumVolume(matrix2), 1.0e-6); +} + + +void Interpolation3DTest::cyl1() +{ + std::cout << std::endl << std::endl << "=============================" << std::endl; + std::cout << " Cylinders " << endl; + + + MESH srcMesh(MED_DRIVER, "meshes/cyl1_fine.med", "Mesh_1"); + MESH targetMesh(MED_DRIVER, "meshes/cyl1_rot_moderate.med", "Mesh_2"); + + IntersectionMatrix matrix1 = interpolator->interpol_maillages(srcMesh, targetMesh); + IntersectionMatrix matrix2 = interpolator->interpol_maillages(targetMesh, srcMesh); + + CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix1)); + CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix2)); + + const double vol1 = sumVolume(matrix1); + const double vol2 = sumVolume(matrix2); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(vol1, vol2, 0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(3.09079e6, vol1, 1.0e2); + CPPUNIT_ASSERT_DOUBLES_EQUAL(3.09079e6, vol2, 1.0e2); +} + + +void Interpolation3DTest::box1() +{ + std::cout << std::endl << std::endl << "=============================" << std::endl; + std::cout << " Boxes " << endl; + + + MESH srcMesh(MED_DRIVER, "meshes/box1_moderate.med", "Mesh_1"); + MESH targetMesh(MED_DRIVER, "meshes/box1_rot_moderate.med", "Mesh_2"); + + IntersectionMatrix matrix1 = interpolator->interpol_maillages(srcMesh, targetMesh); + IntersectionMatrix matrix2 = interpolator->interpol_maillages(targetMesh, srcMesh); + + // CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix1)); + // CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix2)); + + const double vol1 = sumVolume(matrix1); + const double vol2 = sumVolume(matrix2); + + // CPPUNIT_ASSERT_DOUBLES_EQUAL(vol1, vol2, 1.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(750684, vol1, 1.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(750684, vol2, 1.0); + +} + +void Interpolation3DTest::tetra1() +{ + std::cout << std::endl << std::endl << "=============================" << std::endl; + std::cout << " General tetrahedra - 1-element meshes " << endl; + + MESH srcMesh(MED_DRIVER, "meshes/tetra1_gen.med", "Mesh_4"); + MESH targetMesh(MED_DRIVER, "meshes/tetra1_gen_rot.med", "Mesh_5"); +std::cout << "*** src - target" << std::endl; + IntersectionMatrix matrix1 = interpolator->interpol_maillages(srcMesh, targetMesh); + std::cout << "*** target - src" << std::endl; + IntersectionMatrix matrix2 = interpolator->interpol_maillages(targetMesh, srcMesh); + + std::cout << std::endl << std::endl << "--------------------" << std::endl; + std::cout << "src - target" << std::endl; + dumpIntersectionMatrix(matrix1); + std::cout << std::endl << std::endl << "--------------------" << std::endl; + std::cout << "target - src" << std::endl; + dumpIntersectionMatrix(matrix2); + + // CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix1)); + // CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix2)); + + const double vol1 = sumVolume(matrix1); + const double vol2 = sumVolume(matrix2); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(vol1, vol2, 1.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0494584, vol1, 1.0e-7); + CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0494584, vol2, 1.0e-7); +} + +void Interpolation3DTest::tetra3() +{ + std::cout << std::endl << std::endl << "=============================" << std::endl; + std::cout << " General tetrahedra 10-element meshes " << endl; + + MESH srcMesh(MED_DRIVER, "meshes/tetra3_moderate.med", "Mesh_1"); + MESH targetMesh(MED_DRIVER, "meshes/tetra3_rot_moderate.med", "Mesh_3"); + + IntersectionMatrix matrix1 = interpolator->interpol_maillages(srcMesh, targetMesh); + + IntersectionMatrix matrix2 = interpolator->interpol_maillages(targetMesh, srcMesh); + + std::cout << std::endl << std::endl << "--------------------" << std::endl; + std::cout << "src - target" << std::endl; + dumpIntersectionMatrix(matrix1); + std::cout << std::endl << std::endl << "--------------------" << std::endl; + std::cout << "target - src" << std::endl; + dumpIntersectionMatrix(matrix2); + + // CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix1)); + // CPPUNIT_ASSERT_EQUAL(true, isIntersectionConsistent(matrix2)); + + const double vol1 = sumVolume(matrix1); + const double vol2 = sumVolume(matrix2); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(vol1, vol2, 1.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(538.76, vol1, 1.0); + CPPUNIT_ASSERT_DOUBLES_EQUAL(538.76, vol2, 1.0); +} diff --git a/src/INTERP_KERNEL/Test/Interpolation3DTest.hxx b/src/INTERP_KERNEL/Test/Interpolation3DTest.hxx new file mode 100644 index 000000000..79a9d2efb --- /dev/null +++ b/src/INTERP_KERNEL/Test/Interpolation3DTest.hxx @@ -0,0 +1,59 @@ +#ifndef __TU_INTERPOLATION_3D_HXX__ +#define __TU_INTERPOLATION_3D_HXX__ + +#include +#include "../Interpolation3D.hxx" + +#define ERR_TOL 1.0e-8 + +using MEDMEM::Interpolation3D; + +class Interpolation3DTest : public CppUnit::TestFixture +{ + + CPPUNIT_TEST_SUITE( Interpolation3DTest ); + CPPUNIT_TEST( reflexiveTetra ); + CPPUNIT_TEST( tetraTetraTransl ); + CPPUNIT_TEST( tetraTetraScale ); + // CPPUNIT_TEST( box1 ); + // CPPUNIT_TEST( cyl1 ); + CPPUNIT_TEST( tetra1 ); + // CPPUNIT_TEST( tetra3 ); + + CPPUNIT_TEST_SUITE_END(); + + +public: + void setUp(); + + void tearDown(); + + // tests + + void reflexiveTetra(); + + void tetraTetraTransl(); + + void tetraTetraScale(); + + void cyl1(); + + void box1(); + + void tetra1(); + + void tetra3(); + +private: + + Interpolation3D* interpolator; + + double sumVolume(IntersectionMatrix m); + + bool isIntersectionConsistent(IntersectionMatrix m); + + void dumpIntersectionMatrix(IntersectionMatrix m); + +}; + +#endif diff --git a/src/INTERP_KERNEL/TetraAffineTransform.cxx b/src/INTERP_KERNEL/TetraAffineTransform.cxx new file mode 100644 index 000000000..f2f127313 --- /dev/null +++ b/src/INTERP_KERNEL/TetraAffineTransform.cxx @@ -0,0 +1 @@ +#include "TetraAffineTransform.hxx" diff --git a/src/INTERP_KERNEL/TetraAffineTransform.hxx b/src/INTERP_KERNEL/TetraAffineTransform.hxx new file mode 100644 index 000000000..777dd06c2 --- /dev/null +++ b/src/INTERP_KERNEL/TetraAffineTransform.hxx @@ -0,0 +1,263 @@ +#ifndef __TETRA_AFFINE_TRANSFORM_HXX__ +#define __TETRA_AFFINE_TRANSFORM_HXX__ + +#include +#include + +#ifdef TESTING_INTERP_KERNEL +class TetraAffineTransformTest; +#endif + +namespace INTERP_UTILS +{ + + class TetraAffineTransform + { +#ifdef TESTING_INTERP_KERNEL + friend class ::TetraAffineTransformTest; +#endif + + public: + TetraAffineTransform(const double** pts) + { +#if 0 + do { +#endif + // three last points -> linear transform + for(int i = 0; i < 3 ; ++i) + { + for(int j = 0 ; j < 3 ; ++j) + { + // NB we insert columns, not rows + _linearTransform[3*j + i] = (pts[i+1])[j] - (pts[0])[j]; + } + } +#if 0 + calculateDeterminant(); + //assert(_determinant != 0.0); + + if(_determinant < 0.0) + { + // swap two columns + const double* tmp = pts[1]; + pts[1] = pts[2]; + pts[2] = tmp; + } + + } while(_determinant < 0.0); // should do at most one more loop +#endif + + // we need the inverse transform + invertLinearTransform(); + + // first point -> translation + // calculate here because translation takes place in "transformed space", + // or in other words b = -A*O where A is the linear transform + // and O is the position vector of the point that is mapped onto the origin + for(int i = 0 ; i < 3 ; ++i) + { + _translation[i] = -_linearTransform[3*i]*(pts[0])[0] - _linearTransform[3*i+1]*(pts[0])[1] - _linearTransform[3*i+2]*(pts[0])[2] ; + } + + // precalculate determinant (again after inversion of transform) + calculateDeterminant(); + } + + void apply(double* destPt, const double* srcPt) const + { + double* dest = destPt; + + const bool selfAllocation = (destPt == srcPt); + + + + if(selfAllocation) + { + // alloc temporary memory + dest = new double[3]; + + //std::cout << "Oops! self-affectation" << std::endl; + } + + for(int i = 0 ; i < 3 ; ++i) + { + // matrix - vector multiplication + dest[i] = _linearTransform[3*i] * srcPt[0] + _linearTransform[3*i + 1] * srcPt[1] + _linearTransform[3*i + 2] * srcPt[2]; + + // translation + dest[i] += _translation[i]; + } + + if(selfAllocation) + { + // copy result back to destPt + for(int i = 0 ; i < 3 ; ++i) + { + destPt[i] = dest[i]; + } + delete[] dest; + } + } + + double determinant() const + { + return _determinant; + } + + void dump() + { + using namespace std; + + std::cout << "A = " << std::endl << "["; + for(int i = 0; i < 3; ++i) + { + cout << _linearTransform[3*i] << ", " << _linearTransform[3*i + 1] << ", " << _linearTransform[3*i + 1]; + if(i !=2 ) cout << endl; + } + cout << "]" << endl; + + cout << "b = " << "[" << _translation[0] << ", " << _translation[1] << ", " << _translation[2] << "]" << endl; + } + private: + + void invertLinearTransform() + { + //{ we copy the matrix for the lu-factorization + // maybe inefficient + double lu[9]; + for(int i = 0 ; i < 9; ++i) + { + lu[i] = _linearTransform[i]; + } + + // calculate LU factorization + int idx[3]; + factorizeLU(lu, idx); + + // calculate inverse by forward and backward substitution + // store in _linearTransform + // NB _linearTransform cannot be overwritten with lu in the loop + for(int i = 0 ; i < 3 ; ++i) + { + // form standard base vector i + const double b[3] = + { + int(i == 0), + int(i == 1), + int(i == 2) + }; + + //std::cout << "b = [" << b[0] << ", " << b[1] << ", " << b[2] << "]" << std::endl; + + double y[3]; + forwardSubstitution(y, lu, b, idx); + + double x[3]; + backwardSubstitution(x, lu, y, idx); + + // copy to _linearTransform matrix + // NB : this is a column operation, so we cannot + // do this directly when we calculate x + for(int j = 0 ; j < 3 ; j++) + { + _linearTransform[3*j + i] = x[idx[j]]; + } + + } + + } + + void calculateDeterminant() + { + const double subDet[3] = + { + _linearTransform[4] * _linearTransform[8] - _linearTransform[5] * _linearTransform[7], + _linearTransform[3] * _linearTransform[8] - _linearTransform[5] * _linearTransform[6], + _linearTransform[3] * _linearTransform[7] - _linearTransform[4] * _linearTransform[6] + }; + + _determinant = _linearTransform[0] * subDet[0] - _linearTransform[1] * subDet[1] + _linearTransform[2] * subDet[2]; + } + + ///////////////////////////////////////////////// + /// Auxiliary methods for inverse calculation /// + ///////////////////////////////////////////////// + void factorizeLU(double* lu, int* idx) const + { + // 3 x 3 LU factorization + // initialise idx + for(int i = 0 ; i < 3 ; ++i) + { + idx[i] = i; + } + + for(int k = 0; k < 2 ; ++k) + { + + // find pivot + int i = k; + double max = fabs(lu[3*idx[k] + k]); + int row = i; + while(i < 3) + { + if(fabs(lu[3*idx[i] + k]) > max) + { + max = fabs(lu[3*idx[i] + k]); + row = i; + } + ++i; + } + + // swap rows + int tmp = idx[k]; + idx[k] = idx[row]; + idx[row] = tmp; + + // calculate row + for(int j = k + 1 ; j < 3 ; ++j) + { + // l_jk = u_jk / u_kk + lu[3*idx[j] + k] /= lu[3*idx[k] + k]; + for(int s = k + 1; s < 3 ; ++s) + { + // case s = k will always become zero, and then replaced by + // the l-value + // there's no need to calculate this explicitly + + // u_js = u_js - l_jk * u_ks + lu[3*idx[j] + s] -= lu[3*idx[j] + k] * lu[3*idx[k] + s] ; + } + } + } + + + } + + void forwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const + { + x[idx[0]] = ( b[idx[0]] );// / lu[3*idx[0]]; + x[idx[1]] = ( b[idx[1]] - lu[3*idx[1]] * x[idx[0]] ); // / lu[3*idx[1] + 1]; + x[idx[2]] = ( b[idx[2]] - lu[3*idx[2]] * x[idx[0]] - lu[3*idx[2] + 1] * x[idx[1]] ); // / lu[3*idx[2] + 2]; + } + + void backwardSubstitution(double* x, const double* lu, const double* b, const int* idx) const + { + x[idx[2]] = ( b[idx[2]] ) / lu[3*idx[2] + 2]; + x[idx[1]] = ( b[idx[1]] - lu[3*idx[1] + 2] * x[idx[2]] ) / lu[3*idx[1] + 1]; + x[idx[0]] = ( b[idx[0]] - lu[3*idx[0] + 1] * x[idx[1]] - lu[3*idx[0] + 2] * x[idx[2]] ) / lu[3*idx[0]]; + } + + + + double _translation[3]; + double _linearTransform[9]; + + double _determinant; + + }; + + +}; + + +#endif diff --git a/src/INTERP_KERNEL/TransformedTriangle.cxx b/src/INTERP_KERNEL/TransformedTriangle.cxx index 91e751540..56e57a945 100644 --- a/src/INTERP_KERNEL/TransformedTriangle.cxx +++ b/src/INTERP_KERNEL/TransformedTriangle.cxx @@ -2,6 +2,87 @@ #include #include #include +#include +#include +#include "VectorUtils.hxx" +#include + +class CircularSortOrder +{ +public: + + enum CoordType { XY, XZ, YZ }; + + CircularSortOrder(const double* barycenter, const double* pt0, const CoordType type) + : _pt0( pt0 ) + { + // get the indices to use in determinant + _aIdx = (type == YZ) ? 2 : 0; + _bIdx = (type == XY) ? 1 : 2; + + _a = barycenter[_aIdx] - pt0[_aIdx]; + _b = barycenter[_bIdx] - pt0[_bIdx]; + // std::cout << "Creating order of type " << type << " with pt0= " << vToStr(pt0) << std::endl; + // std::cout << "a = " << _a << ", b = " << _b << std::endl; + } + + bool operator()(const double* pt1, const double* pt2) + { + //{ calculations could be optimised here, avoiding intermediary affectations + const double diff[4] = + { + pt1[_aIdx] - _pt0[_aIdx], pt1[_bIdx] - _pt0[_bIdx], // pt1 - pt0 + pt2[_aIdx] - _pt0[_aIdx], pt2[_bIdx] - _pt0[_bIdx], // pt2 - pt0 + }; + + // We need to check if one of the points is equal to pt0, + // since pt0 should always end up at the beginning + // is pt1 == pt0 ? -> if yes, pt1 < pt2 + if(diff[0] == 0.0 && diff[1] == 0.0) + return true; + // is pt2 == pt0 ? -> if yes pt2 < pt1 + if(diff[2] == 0.0 && diff[3] == 0.0) + return false; + + // normal case + const double det1 = _a*diff[1] - _b*diff[0]; + const double det2 = _a*diff[3] - _b*diff[2]; + + return det1 < det2; + } + +private: + int _aIdx, _bIdx; + double _a, _b; + const double* _pt0; +}; + +class ProjectedCentralCircularSortOrder +{ +public: + enum CoordType { XY, XZ, YZ }; + + ProjectedCentralCircularSortOrder(const double* barycenter, const CoordType type) + : _aIdx((type == YZ) ? 2 : 0), + _bIdx((type == XY) ? 1 : 2), + _a(barycenter[_aIdx]), + _b(barycenter[_bIdx]) + { + } + + bool operator()(const double* pt1, const double* pt2) + { + // calculate angles with the axis + const double ang1 = atan2(pt1[_aIdx] - _a, pt1[_bIdx] - _b); + const double ang2 = atan2(pt2[_aIdx] - _a, pt2[_bIdx] - _b); + + return ang1 > ang2; + } + +private: + const int _aIdx, _bIdx; + const double _a, _b; +}; namespace INTERP_UTILS { @@ -49,6 +130,15 @@ namespace INTERP_UTILS TransformedTriangle::~TransformedTriangle() { + // delete elements of polygons + for(std::vector::iterator it = _polygonA.begin() ; it != _polygonA.end() ; ++it) + { + delete[] *it; + } + for(std::vector::iterator it = _polygonB.begin() ; it != _polygonB.end() ; ++it) + { + delete[] *it; + } } /** @@ -61,8 +151,66 @@ namespace INTERP_UTILS */ double TransformedTriangle::calculateIntersectionVolume() { - // not implemented - return 0.0; + // check first that we are not below z - plane + + if(isTriangleBelowTetraeder()) + { + return 0.0; + } + + // get the sign of the volume - equal to the sign of the z-component of the normal + // of the triangle, u_x * v_y - u_y * v_x, where u = q - p and v = r - p + // if it is zero, the triangle is perpendicular to the z - plane and so the volume is zero + const double uv_xy[4] = + { + _coords[5*Q] - _coords[5*P], _coords[5*Q + 1] - _coords[5*P + 1], // u_x, u_y + _coords[5*R] - _coords[5*P], _coords[5*R + 1] - _coords[5*P + 1] // v_x, v_y + }; + + double sign = uv_xy[0] * uv_xy[3] - uv_xy[1] * uv_xy[2]; + + if(sign == 0.0) + { + return 0.0; + } + + // normalize + sign = sign > 0.0 ? 1.0 : -1.0; + + + std::cout << std::endl << "-- Calculating intersection polygons ... " << std::endl; + calculateIntersectionPolygons(); + + // calculate volume under A + std::cout << std::endl << "-- Treating polygon A ... " << std::endl; + double barycenter[3]; + + double volA = 0.0; + + if(_polygonA.size() > 2) + { + calculatePolygonBarycenter(A, barycenter); + sortIntersectionPolygon(A, barycenter); + volA = calculateVolumeUnderPolygon(A, barycenter); + std::cout << "Volume is " << sign * volA << std::endl; + } + + double volB = 0.0; + + // if triangle is not in h = 0 plane, calculate volume under B + if(!isTriangleInPlaneOfFacet(XYZ) && _polygonB.size() > 2) + { + std::cout << std::endl << "-- Treating polygon B ... " << std::endl; + calculatePolygonBarycenter(B, barycenter); + sortIntersectionPolygon(B, barycenter); + volB = calculateVolumeUnderPolygon(B, barycenter); + std::cout << "Volume is " << sign * volB << std::endl; + } + + std::cout << std::endl << "volA + volB = " << sign * (volA + volB) << std::endl << std::endl; + + return sign * (volA + volB); + } //////////////////////////////////////////////////////////////////////////// @@ -82,7 +230,149 @@ namespace INTERP_UTILS */ void TransformedTriangle::calculateIntersectionPolygons() { - // not implemented + assert(_polygonA.size() == 0); + assert(_polygonB.size() == 0); + + // -- surface intersections + // surface - edge + for(TetraEdge edge = OX ; edge <= ZX ; edge = TetraEdge(edge + 1)) + { + if(testSurfaceEdgeIntersection(edge)) + { + //{ we only really need to calculate the point once + double* ptA = new double[3]; + calcIntersectionPtSurfaceEdge(edge, ptA); + _polygonA.push_back(ptA); + std::cout << "Surface-edge : " << vToStr(ptA) << " added to A " << std::endl; + if(edge >= XY) + { + double* ptB = new double[3]; + copyVector3(ptA, ptB); + _polygonB.push_back(ptB); + std::cout << "Surface-edge : " << vToStr(ptB) << " added to B " << std::endl; + } + + } + } + + // surface - ray + for(TetraCorner corner = X ; corner < NO_TET_CORNER ; corner = TetraCorner(corner + 1)) + { + if(testSurfaceRayIntersection(corner)) + { + double* ptB = new double[3]; + copyVector3(&COORDS_TET_CORNER[3 * corner], ptB); + _polygonB.push_back(ptB); + std::cout << "Surface-ray : " << vToStr(ptB) << " added to B" << std::endl; + } + } + + // -- segment intersections + for(TriSegment seg = PQ ; seg < NO_TRI_SEGMENT ; seg = TriSegment(seg + 1)) + { + // segment - facet + for(TetraFacet facet = OYZ ; facet < NO_TET_FACET ; facet = TetraFacet(facet + 1)) + { + if(testSegmentFacetIntersection(seg, facet)) + { + double* ptA = new double[3]; + calcIntersectionPtSegmentFacet(seg, facet, ptA); + _polygonA.push_back(ptA); + std::cout << "Segment-facet : " << vToStr(ptA) << " added to A" << std::endl; + if(facet == XYZ) + { + double* ptB = new double[3]; + copyVector3(ptA, ptB); + _polygonB.push_back(ptB); + std::cout << "Segment-facet : " << vToStr(ptB) << " added to B" << std::endl; + } + } + } + + // segment - edge + for(TetraEdge edge = OX ; edge <= ZX ; edge = TetraEdge(edge + 1)) + { + if(testSegmentEdgeIntersection(seg, edge)) + { + double* ptA = new double[3]; + calcIntersectionPtSegmentEdge(seg, edge, ptA); + _polygonA.push_back(ptA); + std::cout << "Segment-edge : " << vToStr(ptA) << " added to A" << std::endl; + if(edge >= XY) + { + double* ptB = new double[3]; + copyVector3(ptA, ptB); + _polygonB.push_back(ptB); + } + } + } + + // segment - corner + for(TetraCorner corner = O ; corner < NO_TET_CORNER ; corner = TetraCorner(corner + 1)) + { + if(testSegmentCornerIntersection(seg, corner)) + { + double* ptA = new double[3]; + copyVector3(&COORDS_TET_CORNER[3 * corner], ptA); + _polygonA.push_back(ptA); + std::cout << "Segment-corner : " << vToStr(ptA) << " added to A" << std::endl; + if(corner != O) + { + double* ptB = new double[3]; + _polygonB.push_back(ptB); + copyVector3(&COORDS_TET_CORNER[3 * corner], ptB); + std::cout << "Segment-corner : " << vToStr(ptB) << " added to B" << std::endl; + } + } + } + + // segment - ray + for(TetraCorner corner = X ; corner < NO_TET_CORNER ; corner = TetraCorner(corner + 1)) + { + if(testSegmentRayIntersection(seg, corner)) + { + double* ptB = new double[3]; + copyVector3(&COORDS_TET_CORNER[3 * corner], ptB); + _polygonB.push_back(ptB); + std::cout << "Segment-ray : " << vToStr(ptB) << " added to B" << std::endl; + } + } + + // segment - halfstrip + for(TetraEdge edge = XY ; edge <= ZX ; edge = TetraEdge(edge + 1)) + { + if(testSegmentHalfstripIntersection(seg, edge)) + { + double* ptB = new double[3]; + calcIntersectionPtSegmentHalfstrip(seg, edge, ptB); + _polygonB.push_back(ptB); + std::cout << "Segment-halfstrip : " << vToStr(ptB) << " added to B" << std::endl; + } + } + } + + // inclusion tests + for(TriCorner corner = P ; corner < NO_TRI_CORNER ; corner = TriCorner(corner + 1)) + { + // { XYZ - inclusion only possible if in Tetrahedron? + // tetrahedron + if(testCornerInTetrahedron(corner)) + { + double* ptA = new double[3]; + copyVector3(&_coords[5*corner], ptA); + _polygonA.push_back(ptA); + std::cout << "Inclusion tetrahedron : " << vToStr(ptA) << " added to A" << std::endl; + } + + // XYZ - plane + if(testCornerOnXYZFacet(corner)) + { + double* ptB = new double[3]; + copyVector3(&_coords[5*corner], ptB); + _polygonA.push_back(ptB); + std::cout << "Inclusion XYZ-plane : " << vToStr(ptB) << " added to B" << std::endl; + } + } } /** @@ -96,7 +386,31 @@ namespace INTERP_UTILS */ void TransformedTriangle::calculatePolygonBarycenter(const IntersectionPolygon poly, double* barycenter) { - // not implemented + std::cout << "--- Calculating polygon barycenter" << std::endl; + + // get the polygon points + std::vector& polygon = (poly == A) ? _polygonA : _polygonB; + + // calculate barycenter + const int m = polygon.size(); + + for(int j = 0 ; j < 3 ; ++j) + { + barycenter[j] = 0.0; + } + + if(m != 0) + { + for(int i = 0 ; i < m ; ++i) + { + const double* pt = polygon[i]; + for(int j = 0 ; j < 3 ; ++j) + { + barycenter[j] += pt[j] / double(m); + } + } + } + std::cout << "Barycenter is " << vToStr(barycenter) << std::endl; } /** @@ -112,7 +426,49 @@ namespace INTERP_UTILS */ void TransformedTriangle::sortIntersectionPolygon(const IntersectionPolygon poly, const double* barycenter) { - // not implemented + std::cout << "--- Sorting polygon ..."<< std::endl; + + using ::ProjectedCentralCircularSortOrder; + typedef ProjectedCentralCircularSortOrder SortOrder; // change is only necessary here and in constructor + typedef SortOrder::CoordType CoordType; + + // get the polygon points + std::vector& polygon = (poly == A) ? _polygonA : _polygonB; + + if(polygon.size() == 0) + return; + + // determine type of sorting + CoordType type = SortOrder::XY; + if(poly == A) // B is on h = 0 plane -> ok + { + // is triangle parallel to x == 0 ? + if(isTriangleInPlaneOfFacet(OZX)) + { + type = SortOrder::YZ; + } + else if(isTriangleInPlaneOfFacet(OYZ)) + { + type = SortOrder::XZ; + } + } + + // create order object + SortOrder order(barycenter, type); + + // sort vector with this object + // NB : do not change place of first object, with respect to which the order + // is defined + sort((polygon.begin()), polygon.end(), order); + //stable_sort((++polygon.begin()), polygon.end(), order); + + + std::cout << "Sorted polygon is " << std::endl; + for(int i = 0 ; i < polygon.size() ; ++i) + { + std::cout << vToStr(polygon[i]) << std::endl; + } + } /** @@ -128,9 +484,82 @@ namespace INTERP_UTILS */ double TransformedTriangle::calculateVolumeUnderPolygon(IntersectionPolygon poly, const double* barycenter) { - // not implemented - return -3.14; + std::cout << "--- Calculating volume under polygon" << std::endl; + + // get the polygon points + std::vector& polygon = (poly == A) ? _polygonA : _polygonB; + + double vol = 0.0; + const int m = polygon.size(); + + for(int i = 0 ; i < m ; ++i) + { + const double* ptCurr = polygon[i]; // pt "i" + const double* ptNext = polygon[(i + 1) % m]; // pt "i+1" (pt m == pt 0) + + const double factor1 = ptCurr[2] + ptNext[2] + barycenter[2]; + const double factor2 = + ptCurr[0]*(ptNext[1] - barycenter[1]) + + ptNext[0]*(barycenter[1] - ptCurr[1]) + + barycenter[0]*(ptCurr[1] - ptNext[1]); + vol += (factor1 * factor2) / 6.0; + } + + // std::cout << "Abs. Volume is " << vol << std::endl; + return vol; + } + + + //////////////////////////////////////////////////////////////////////////////////// + /// Detection of (very) degenerate cases //////////// + //////////////////////////////////////////////////////////////////////////////////// + + /** + * Checks if the triangle lies in the plane of a given facet + * + * @param facet one of the facets of the tetrahedron + * @returns true if PQR lies in the plane of the facet, false if not + */ + bool TransformedTriangle::isTriangleInPlaneOfFacet(const TetraFacet facet) + { + + // coordinate to check + const int coord = static_cast(facet); + + for(TriCorner c = P ; c < NO_TRI_CORNER ; c = TriCorner(c + 1)) + { + // ? should have epsilon-equality here? + if(_coords[5*c + coord] != 0.0) + { + return false; + } + } + + return true; } + bool TransformedTriangle::isTriangleBelowTetraeder() + { + for(TriCorner c = P ; c < NO_TRI_CORNER ; c = TriCorner(c + 1)) + { + // check z-coords for all points + if(_coords[5*c + 2] > 0.0) + { + return false; + } + } + return true; + } + +void TransformedTriangle::dumpCoords() +{ + std::cout << "Coords : "; + for(int i = 0 ; i < 3; ++i) + { + std::cout << vToStr(&_coords[5*i]) << ","; + } + std::cout << std::endl; +} + }; // NAMESPACE diff --git a/src/INTERP_KERNEL/TransformedTriangle.hxx b/src/INTERP_KERNEL/TransformedTriangle.hxx index 6a9209d6a..89e990907 100644 --- a/src/INTERP_KERNEL/TransformedTriangle.hxx +++ b/src/INTERP_KERNEL/TransformedTriangle.hxx @@ -6,6 +6,7 @@ #ifdef TESTING_INTERP_KERNEL class TransformedTriangleTest; class TransformedTriangleIntersectTest; +class TransformedTriangleCalcVolumeTest; #endif namespace INTERP_UTILS @@ -28,6 +29,7 @@ namespace INTERP_UTILS #ifdef TESTING_INTERP_KERNEL friend class ::TransformedTriangleTest; friend class ::TransformedTriangleIntersectTest; + friend class ::TransformedTriangleCalcVolumeTest; #endif /** @@ -35,31 +37,35 @@ namespace INTERP_UTILS * and the triangle. */ /// Corners of tetrahedron - enum TetraCorner { O = 0, X, Y, Z }; + enum TetraCorner { O = 0, X, Y, Z, NO_TET_CORNER }; /// Edges of tetrahedron - enum TetraEdge { OX = 0, OY, OZ, XY, YZ, ZX, H01, H10 }; + enum TetraEdge { OX = 0, OY, OZ, XY, YZ, ZX, H01, H10, NO_TET_EDGE }; /// Facets (faces) of tetrahedron - enum TetraFacet { OYZ = 0, OZX, OXY, XYZ }; + enum TetraFacet { OYZ = 0, OZX, OXY, XYZ, NO_TET_FACET }; /// Corners of triangle - enum TriCorner { P = 0, Q, R }; + enum TriCorner { P = 0, Q, R, NO_TRI_CORNER }; /// Segments (edges) of triangle - enum TriSegment { PQ = 0, QR, RP }; + enum TriSegment { PQ = 0, QR, RP, NO_TRI_SEGMENT }; /// Polygons - enum IntersectionPolygon{ A = 0, B }; + enum IntersectionPolygon{ A = 0, B, NO_INTERSECTION_POLYGONS }; /// Double products /// NB : order corresponds to TetraEdges (Grandy, table III) - enum DoubleProduct { C_YZ = 0, C_ZX, C_XY, C_ZH, C_XH, C_YH, C_01, C_10 }; + enum DoubleProduct { C_YZ = 0, C_ZX, C_XY, C_ZH, C_XH, C_YH, C_01, C_10, NO_DP }; TransformedTriangle(double* p, double* q, double* r); ~TransformedTriangle(); double calculateIntersectionVolume(); + + // temporary debug method + void dumpCoords(); + private: @@ -75,6 +81,13 @@ namespace INTERP_UTILS double calculateVolumeUnderPolygon(IntersectionPolygon poly, const double* barycenter); + //////////////////////////////////////////////////////////////////////////////////// + /// Detection of (very) degenerate cases //////////// + //////////////////////////////////////////////////////////////////////////////////// + + bool isTriangleInPlaneOfFacet(const TetraFacet facet); + + bool isTriangleBelowTetraeder(); //////////////////////////////////////////////////////////////////////////////////// /// Intersection test methods and intersection point calculations //////// @@ -105,7 +118,7 @@ namespace INTERP_UTILS bool testCornerInTetrahedron(const TriCorner corner) const; bool testCornerOnXYZFacet(const TriCorner corner) const; - + //////////////////////////////////////////////////////////////////////////////////// /// Utility methods used in intersection tests /////////////// @@ -119,6 +132,8 @@ namespace INTERP_UTILS bool testSegmentIntersectsFacet(const TriSegment seg, const TetraFacet facet) const; + bool testSegmentIntersectsH(const TriSegment seg); + bool testSurfaceAboveCorner(const TetraCorner corner) const; bool testTriangleSurroundsRay(const TetraCorner corner) const; @@ -154,6 +169,10 @@ namespace INTERP_UTILS std::vector _polygonA, _polygonB; double _barycenterA[3], _barycenterB[3]; + // used for debugging + bool _validTP[4]; + + //////////////////////////////////////////////////////////////////////////////////// /// Constants ///////////////// //////////////////////////////////////////////////////////////////////////////////// @@ -176,9 +195,9 @@ namespace INTERP_UTILS // values used to decide how imprecise the double products // should be to set them to 0.0 - static const double MACH_EPS; // machine epsilon - static const double MULT_PREC_F; // precision of multiplications (Grandy : f) - static const double THRESHOLD_F; // threshold for zeroing (Grandy : F/f) + static const long double MACH_EPS; // machine epsilon + static const long double MULT_PREC_F; // precision of multiplications (Grandy : f) + static const long double THRESHOLD_F; // threshold for zeroing (Grandy : F/f) static const double TRIPLE_PRODUCT_ANGLE_THRESHOLD; @@ -189,6 +208,8 @@ namespace INTERP_UTILS // signs associated with entries in DP_FOR_SEGMENT_FACET_INTERSECTION static const double SIGN_FOR_SEG_FACET_INTERSECTION[12]; + // coordinates of corners of tetrahedron + static const double COORDS_TET_CORNER[12]; }; diff --git a/src/INTERP_KERNEL/TransformedTriangle_intersect.cxx b/src/INTERP_KERNEL/TransformedTriangle_intersect.cxx index 1e7d61bde..0adc9b3e0 100644 --- a/src/INTERP_KERNEL/TransformedTriangle_intersect.cxx +++ b/src/INTERP_KERNEL/TransformedTriangle_intersect.cxx @@ -3,576 +3,482 @@ #include #include #include +#include namespace INTERP_UTILS { - + //////////////////////////////////////////////////////////////////////////////////// /// Constants ///////////////// //////////////////////////////////////////////////////////////////////////////////// + const int TransformedTriangle::DP_OFFSET_1[8] = {1, 2, 0, 2, 0, 1, 4, 1}; + const int TransformedTriangle::DP_OFFSET_2[8] = {2, 0, 1, 3, 3, 3, 0, 4}; - // correspondance facet - double product - // Grandy, table IV - const TransformedTriangle::DoubleProduct TransformedTriangle::DP_FOR_SEG_FACET_INTERSECTION[12] = + const int TransformedTriangle::COORDINATE_FOR_DETERMINANT_EXPANSION[12] = { - C_XH, C_XY, C_ZX, // OYZ - C_YH, C_YZ, C_XY, // OZX - C_ZH, C_ZX, C_YZ, // OXY - C_XH, C_YH, C_ZH // XYZ + 0, 1, 2, // O + 3, 1, 2, // X + 0, 3, 2, // Y + 0, 1, 3 // Z }; - // signs associated with entries in DP_FOR_SEGMENT_FACET_INTERSECTION - const double TransformedTriangle::SIGN_FOR_SEG_FACET_INTERSECTION[12] = + + const TransformedTriangle::DoubleProduct TransformedTriangle::DP_FOR_DETERMINANT_EXPANSION[12] = { - 1.0, 1.0, -1.0, - 1.0, 1.0, -1.0, - 1.0, 1.0, -1.0, - 1.0, 1.0, 1.0 + C_YZ, C_ZX, C_XY, // O + C_YZ, C_ZH, C_YH, // X + C_ZH, C_ZX, C_XH, // Y + C_YH, C_XH, C_XY // Z }; + + //const double TransformedTriangle::MACH_EPS = 1.0e-15; + const long double TransformedTriangle::MACH_EPS = std::numeric_limits::epsilon(); + const long double TransformedTriangle::MULT_PREC_F = 4.0 * TransformedTriangle::MACH_EPS; + const long double TransformedTriangle::THRESHOLD_F = 20.0; - - //////////////////////////////////////////////////////////////////////////////////// - /// Intersection test methods and intersection point calculations //////// - //////////////////////////////////////////////////////////////////////////////////// - /** - * Tests if the given edge of the tetrahedron intersects the triangle PQR. (Grandy, eq [17]) - * - * @param edge edge of tetrahedron - * @returns true if PQR intersects the edge, and the edge is not in the plane of the triangle. - */ - bool TransformedTriangle::testSurfaceEdgeIntersection(const TetraEdge edge) const - { - return testTriangleSurroundsEdge(edge) && testEdgeIntersectsTriangle(edge); - } + const double TransformedTriangle::TRIPLE_PRODUCT_ANGLE_THRESHOLD = 0.1; - /** - * Calculates the point of intersection between the given edge of the tetrahedron and the - * triangle PQR. (Grandy, eq [22]) - * - * @pre testSurfaceEdgeIntersection(edge) returns true - * @param edge edge of tetrahedron - * @param pt array of three doubles in which to store the coordinates of the intersection point - */ - void TransformedTriangle::calcIntersectionPtSurfaceEdge(const TetraEdge edge, double* pt) const - { - // not implemented - } - - /** - * Tests if the given segment of the triangle intersects the given facet of the tetrahedron. - * (Grandy, eq. [19]) - * - * @param seg segment of the triangle - * @param facet facet of the tetrahedron - * @returns true if the segment intersects the facet - */ - bool TransformedTriangle::testSegmentFacetIntersection(const TriSegment seg, const TetraFacet facet) const - { - return testFacetSurroundsSegment(seg, facet) && testSegmentIntersectsFacet(seg, facet); - } + // coordinates of corner ptTetCorner + const double TransformedTriangle::COORDS_TET_CORNER[12] = + { + 0.0, 0.0, 0.0, // O + 1.0, 0.0, 0.0, // X + 0.0, 1.0, 0.0, // Y + 0.0, 0.0, 1.0 // Z + }; + //////////////////////////////////////////////////////////////////////////////////// + /// Double and triple product calculations /////////////// + //////////////////////////////////////////////////////////////////////////////////// + /** - * Calculates the point of intersection between the given segment of the triangle - * and the given facet of the tetrahedron. (Grandy, eq. [23]) + * Pre-calculates all double products for this triangle, and stores + * them internally. This method makes compensation for precision errors, + * and it is thus the "stable" double products that are stored. * - * @pre testSurfaceEdgeIntersection(seg, facet) returns true - * - * @param seg segment of the triangle - * @param facet facet of the tetrahedron - * @param pt array of three doubles in which to store the coordinates of the intersection point */ - void TransformedTriangle::calcIntersectionPtSegmentFacet(const TriSegment seg, const TetraFacet facet, double* pt) const + void TransformedTriangle::preCalculateDoubleProducts(void) { - // not implemented - } + if(_isDoubleProductsCalculated) + return; - /** - * Tests if the given segment of the triangle intersects the given edge of the tetrahedron (Grandy, eq. [20] - * - * @param seg segment of the triangle - * @param edge edge of tetrahedron - * @returns true if the segment intersects the edge - */ - bool TransformedTriangle::testSegmentEdgeIntersection(const TriSegment seg, const TetraEdge edge) const - { - // facets shared by each edge - static const TetraFacet FACET_FOR_EDGE[12] = - { - OXY, OZX, // OX - OXY, OYZ, // OY - OZX, OYZ, // OZ - OXY, XYZ, // XY - OYZ, XYZ, // YZ - OZX, XYZ // ZX - }; + // aliases to shorten code + typedef TransformedTriangle::DoubleProduct DoubleProduct; + typedef TransformedTriangle::TetraCorner TetraCorner; + typedef TransformedTriangle::TriSegment TriSegment; + typedef TransformedTriangle TT; - if(calcStableC(seg,DoubleProduct( edge )) != 0.0) - { - return false; - } - else + // -- calculate all unstable double products -- store in _doubleProducts + for(TriSegment seg = TT::PQ ; seg <= TT::RP ; seg = TriSegment(seg + 1)) { - // check condition that the double products for one of the two - // facets adjacent to the edge has a positive product - bool isFacetCondVerified = false; - TetraFacet facet[2]; - for(int i = 0 ; i < 2 ; ++i) + for(DoubleProduct dp = TT::C_YZ ; dp <= TT::C_10 ; dp = DoubleProduct(dp + 1)) { - facet[i] = FACET_FOR_EDGE[2*edge + i]; - - // find the two c-values -> the two for the other edges of the facet - int idx1 = 0 ; - int idx2 = 1; - DoubleProduct dp1 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx1]; - DoubleProduct dp2 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx2]; - if(dp1 == DoubleProduct( edge )) - { - idx1 = 2; - dp1 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx1]; - } - else if(dp2 == DoubleProduct( edge )) - { - idx2 = 2; - dp2 = DP_FOR_SEG_FACET_INTERSECTION[3*facet[i] + idx2]; - } - - const double c1 = SIGN_FOR_SEG_FACET_INTERSECTION[3*facet[i]+idx1]*calcStableC(seg, dp1); - const double c2 = SIGN_FOR_SEG_FACET_INTERSECTION[3*facet[i]+idx2]*calcStableC(seg, dp2); - - isFacetCondVerified = isFacetCondVerified || c1*c2 > 0.0; - } - if(!isFacetCondVerified) - { - return false; - } - else - { - return testSegmentIntersectsFacet(seg, facet[0]) || testSegmentIntersectsFacet(seg, facet[1]); + _doubleProducts[8*seg + dp] = calcUnstableC(seg, dp); } } - } - - /** - * Calculates the point of intersection between the given segment of the triangle - * and the given edge of the tetrahedron. (Grandy, eq. [25]) - * - * @pre testSegmentEdgeIntersection(seg, edge) returns true - * - * @param seg segment of the triangle - * @param edge edge of the tetrahedron - * @param pt array of three doubles in which to store the coordinates of the intersection point - */ - void TransformedTriangle::calcIntersectionPtSegmentEdge(const TriSegment seg, const TetraEdge edge, double* pt) const - { - // not implemented - } - - /** - * Tests if the given segment of the triangle intersects the given corner of the tetrahedron. - * (Grandy, eq. [21]) - * - * @param seg segment of the triangle - * @param corner corner of the tetrahedron - * @returns true if the segment intersects the corner - */ - bool TransformedTriangle::testSegmentCornerIntersection(const TriSegment seg, const TetraCorner corner) const - { - // edges meeting at a given corner - static const TetraEdge EDGES_FOR_CORNER[12] = - { - OX, OY, OZ, // O - OX, XY, ZX, // X - OY, XY, YZ, // Y - OZ, ZX, YZ // Z - }; + - // facets meeting at a given corner - static const TetraFacet FACETS_FOR_CORNER[12] = + // -- (1) for each segment : check that double products satisfy Grandy, [46] + // -- and make corrections if not + for(TriSegment seg = TT::PQ ; seg <= TT::RP ; seg = TriSegment(seg + 1)) { - OXY, OYZ, OZX, // O - OZX, OXY, XYZ, // X - OYZ, XYZ, OXY, // Y - OZX, XYZ, OYZ // Z - }; + const double term1 = _doubleProducts[8*seg + TT::C_YZ] * _doubleProducts[8*seg + TT::C_XH]; + const double term2 = _doubleProducts[8*seg + TT::C_ZX] * _doubleProducts[8*seg + TT::C_YH]; + const double term3 = _doubleProducts[8*seg + TT::C_XY] * _doubleProducts[8*seg + TT::C_ZH]; - // check double products are zero - for(int i = 0 ; i < 3 ; ++i) - { - const TetraEdge edge = EDGES_FOR_CORNER[3*corner + i]; - const DoubleProduct dp = DoubleProduct( edge ); - const double c = calcStableC(seg, dp); - if(c != 0.0) + // std::cout << std::endl; + //std::cout << "for seg " << seg << " consistency " << term1 + term2 + term3 << std::endl; + //std::cout << "term1 :" << term1 << " term2 :" << term2 << " term3: " << term3 << std::endl; + + // if(term1 + term2 + term3 != 0.0) + const int num_zero = (term1 == 0.0 ? 1 : 0) + (term2 == 0.0 ? 1 : 0) + (term3 == 0.0 ? 1 : 0); + const int num_neg = (term1 < 0.0 ? 1 : 0) + (term2 < 0.0 ? 1 : 0) + (term3 < 0.0 ? 1 : 0); + + + if(num_zero == 2 || (num_zero !=3 && num_neg == 0) || (num_zero !=3 && num_neg == 3)) { - return false; + //std::cout << "inconsistent! "<< std::endl; + + // find TetraCorner nearest to segment + double min_dist; + TetraCorner min_corner; + + for(TetraCorner corner = TT::O ; corner <= TT::Z ; corner = TetraCorner(corner + 1)) + { + // calculate distance corner - segment axis + // NB uses fact that TriSegment <=> TriCorner that is first point of segment (PQ <=> P) + const TriCorner ptP_idx = TriCorner(seg); + const TriCorner ptQ_idx = TriCorner( (seg + 1) % 3); + + const double ptP[3] = { _coords[5*ptP_idx], _coords[5*ptP_idx + 1], _coords[5*ptP_idx + 2] }; + const double ptQ[3] = { _coords[5*ptQ_idx], _coords[5*ptQ_idx + 1], _coords[5*ptQ_idx + 2] }; + + // coordinates of corner + const double ptTetCorner[3] = + { + COORDS_TET_CORNER[3*corner ], + COORDS_TET_CORNER[3*corner + 1], + COORDS_TET_CORNER[3*corner + 2] + }; + + // dist^2 = ( PQ x CP )^2 / |PQ|^2 where C is the corner point + + // difference vectors + const double diffPQ[3] = { ptQ[0] - ptP[0], ptQ[1] - ptP[1], ptQ[2] - ptP[2] }; + const double diffCornerP[3] = { ptP[0] - ptTetCorner[0], ptP[1] - ptTetCorner[1], ptP[2] - ptTetCorner[2] }; + + //{ use functions in VectorUtils for this + + // cross product of difference vectors + const double cross[3] = + { + diffPQ[1]*diffCornerP[2] - diffPQ[2]*diffCornerP[1], + diffPQ[2]*diffCornerP[0] - diffPQ[0]*diffCornerP[2], + diffPQ[0]*diffCornerP[1] - diffPQ[1]*diffCornerP[0], + }; + + + const double cross_squared = cross[0]*cross[0] + cross[1]*cross[1] + cross[2]*cross[2]; + const double norm_diffPQ_squared = diffPQ[0]*diffPQ[0] + diffPQ[1]*diffPQ[1] + diffPQ[2]*diffPQ[2]; + const double dist = cross_squared / norm_diffPQ_squared; + + // update minimum (initializs with first corner) + if(corner == TT::O || dist < min_dist) + { + min_dist = dist; + min_corner = corner; + } + } + + // set the three corresponding double products to 0.0 + static const DoubleProduct DOUBLE_PRODUCTS[12] = + { + TT::C_YZ, TT::C_ZX, TT::C_XY, // O + TT::C_YZ, TT::C_ZH, TT::C_YH, // X + TT::C_ZX, TT::C_ZH, TT::C_XH, // Y + TT::C_XY, TT::C_YH, TT::C_XH // Z + }; + + for(int i = 0 ; i < 3 ; ++i) { + DoubleProduct dp = DOUBLE_PRODUCTS[3*min_corner + i]; + + // std::cout << std::endl << "inconsistent dp :" << dp << std::endl; + _doubleProducts[8*seg + dp] = 0.0; + } + } } - - // check segment intersect a facet - for(int i = 0 ; i < 3 ; ++i) + + + // -- (2) check that each double product statisfies Grandy, [47], else set to 0 + + for(TriSegment seg = TT::PQ ; seg <= TT::RP ; seg = TriSegment(seg + 1)) { - const TetraFacet facet = FACETS_FOR_CORNER[3*corner + i]; - if(testSegmentIntersectsFacet(seg, facet)) + for(DoubleProduct dp = TT::C_YZ ; dp <= TT::C_10 ; dp = DoubleProduct(dp + 1)) { - return true; - } - } - - return false; - } - + // find the points of the triangle + // 0 -> P, 1 -> Q, 2 -> R + const int pt1 = seg; + const int pt2 = (seg + 1) % 3; - /** - * Tests if the triangle PQR intersects the ray pointing in the upwards z - direction - * from the given corner of the tetrahedron. (Grandy eq. [29]) - * - * @param corner corner of the tetrahedron on the h = 0 facet (X, Y, or Z) - * @returns true if the upwards ray from the corner intersects the triangle. - */ - bool TransformedTriangle::testSurfaceRayIntersection(const TetraCorner corner) const - { - return testTriangleSurroundsRay( corner ) && testSurfaceAboveCorner( corner ); - } + // find offsets + const int off1 = DP_OFFSET_1[dp]; + const int off2 = DP_OFFSET_2[dp]; - /** - * Tests if the given segment of the triangle intersects the half-strip above the - * given edge of the h = 0 plane. (Grandy, eq. [30]) - * - * @param seg segment of the triangle - * @param edge edge of the h = 0 plane of the tetrahedron (XY, YZ, ZX) - * @returns true if the upwards ray from the corner intersects the triangle. - */ - bool TransformedTriangle::testSegmentHalfstripIntersection(const TriSegment seg, const TetraEdge edg) - { - // get right index here to avoid "filling out" array - const int edgeIndex = static_cast(edg) - 3; - - // double products used in test - // products 1 and 2 for each edge -> first condition in Grandy [30] - // products 3 and 4 for each edge -> third condition - // NB : some uncertainty whether these last are correct - static const DoubleProduct DP_FOR_HALFSTRIP_INTERSECTION[12] = - { - C_10, C_01, C_ZH, C_YZ, // XY - C_01, C_XY, C_XH, C_ZX, // YZ - C_XY, C_10, C_YH, C_XY // ZX - }; + const long double term1 = static_cast(_coords[5*pt1 + off1]) * static_cast(_coords[5*pt2 + off2]); + const long double term2 = static_cast(_coords[5*pt1 + off2]) * static_cast(_coords[5*pt2 + off1]); - // facets to use in second condition (S_m) - static TetraFacet FACET_FOR_HALFSTRIP_INTERSECTION[3] = - { - XYZ, // XY - OYZ, // YZ - OZX // ZX - }; - - const double cVals[4] = - { - calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex]), - calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex + 1]), - calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex + 2]), - calcStableC(seg, DP_FOR_HALFSTRIP_INTERSECTION[4*edgeIndex + 3]) - }; + const long double delta = MULT_PREC_F * ( std::abs(term1) + std::abs(term2) ); + + if( std::abs(static_cast(_doubleProducts[8*seg + dp])) < THRESHOLD_F*delta ) + { + if(_doubleProducts[8*seg + dp] != 0.0) + { + std::cout << "Double product for (seg,dp) = (" << seg << ", " << dp << ") = " << std::abs(_doubleProducts[8*seg + dp]) << " is imprecise, reset to 0.0" << std::endl; + } + _doubleProducts[8*seg + dp] = 0.0; + + } + } + } - const TetraFacet facet = FACET_FOR_HALFSTRIP_INTERSECTION[edgeIndex]; - - // std::cout << "Halfstrip tests : " << (cVals[0]*cVals[1] < 0.0) << ", " << testSegmentIntersectsFacet(seg, facet) << ", " << (cVals[2]*cVals[3] > 0.0) << std::endl; - - return (cVals[0]*cVals[1] < 0.0) && testSegmentIntersectsFacet(seg, facet) && (cVals[2]*cVals[3] > 0.0); + _isDoubleProductsCalculated = true; } /** - * Calculates the point of intersection between the given segment of the triangle - * and the halfstrip above the given edge of the tetrahedron. (Grandy, eq. [31]) + * Pre-calculates all triple products for the tetrahedron with respect to + * this triangle, and stores them internally. This method takes into account + * the problem of errors due to cancellation. * - * @pre testSegmentHalfstripIntersection(seg, edge) returns true - * - * @param seg segment of the triangle - * @param edge edge of the tetrahedron defining the halfstrip - * @param pt array of three doubles in which to store the coordinates of the intersection point */ - void TransformedTriangle::calcIntersectionPtSegmentHalfstrip(const TriSegment seg, const TetraEdge edge, double* pt) const + void TransformedTriangle::preCalculateTripleProducts(void) { - // not implemented - } - - /** - * Tests if the given segment of triangle PQR intersects the ray pointing - * in the upwards z - direction from the given corner of the tetrahedron. (Grandy eq. [29]) - * - * @param seg segment of the triangle PQR - * @param corner corner of the tetrahedron on the h = 0 facet (X, Y, or Z) - * @returns true if the upwards ray from the corner intersects the segment. - */ - bool TransformedTriangle::testSegmentRayIntersection(const TriSegment seg, const TetraCorner corner) const - { - assert(corner == X || corner == Y || corner == Z); + if(_isTripleProductsCalculated) + return; - // readjust index since O is not used - const int cornerIdx = static_cast(corner) - 1; - - // double products to use in test - // dp 1 -> cond 1 - // dp 2-7 -> cond 3 - //? NB : last two rows are not completely understood and may contain errors - static const DoubleProduct DP_SEGMENT_RAY_INTERSECTION[21] = + for(TetraCorner corner = O ; corner <= Z ; corner = TetraCorner(corner + 1)) { - C_10, C_YH, C_ZH, C_01, C_XY, C_YH, C_XY, // X - C_01, C_XH, C_ZH, C_XY, C_10, C_XH, C_ZX, // Y - C_XY, C_YH, C_XH, C_10, C_01, C_ZH, C_YZ // Z - }; + bool isGoodRowFound = false; - // facets to use - //? not sure this is correct - static const TetraFacet FACET_SEGMENT_RAY_INTERSECTION[3] = - { - OZX, // X - OXY, // Y - OYZ, // Z - }; - + // find edge / row to use + int minRow; + double minAngle; + bool isMinInitialised = false; - const DoubleProduct dp0 = DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx]; - - //? epsilon-equality here? - if(dp0 == 0.0) // cond. 1 - { - - if(testSegmentIntersectsFacet(seg, FACET_SEGMENT_RAY_INTERSECTION[cornerIdx])) // cond. 2.1 - { - const double H1 = _coords[5*seg + 4]; - const double H2 = _coords[5*( (seg + 1) % 3) + 4]; - - // S_H -> cond. 2.2 - // std::cout << "H1 = " << H1 << ", H2= " << H2 << std::endl; - if(H1*H2 <= 0.0 && H1 != H2) // should equality be in "epsilon-sense" ? - { - - const double cVals[6] = - { - calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 1]), - calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 2]), - calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 3]), - calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 4]), - calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 5]), - calcStableC(seg, DP_SEGMENT_RAY_INTERSECTION[7*cornerIdx + 6]), + for(int row = 1 ; row < 4 ; ++row) + { + DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3*corner + (row - 1)]; + + // get edge by using correspondance between Double Product and Edge + TetraEdge edge = TetraEdge(dp); + + // use edge only if it is surrounded by the surface + if( testTriangleSurroundsEdge(edge) ) + { + isGoodRowFound = true; + + // -- calculate angle between edge and PQR + // find normal to PQR - cross PQ and PR + const double pq[3] = + { + _coords[5*Q] - _coords[5*P], + _coords[5*Q + 1] - _coords[5*P + 1], + _coords[5*Q + 2] - _coords[5*P + 2] + }; + + const double pr[3] = + { + _coords[5*R] - _coords[5*P], + _coords[5*R + 1] - _coords[5*P + 1], + _coords[5*R + 2] - _coords[5*P + 2] + }; + + const double normal[3] = + { + pq[1]*pr[2] - pq[2]*pr[1], + pq[2]*pr[0] - pq[0]*pr[2], + pq[0]*pr[1] - pq[1]*pr[0] + }; + + static const double EDGE_VECTORS[18] = + { + 1.0, 0.0, 0.0, // OX + 0.0, 1.0, 0.0, // OY + 0.0, 0.0, 1.0, // OZ + 0.0,-1.0, 1.0, // YZ + 1.0, 0.0,-1.0, // ZX + -1.0,-1.0, 1.0 // XY + }; + + const double edgeVec[3] = { + EDGE_VECTORS[3*edge], + EDGE_VECTORS[3*edge + 1], + EDGE_VECTORS[3*edge + 2], }; - // cond. 3 - return ( (cVals[0] + cVals[1])*(cVals[2] - cVals[3]) - cVals[4]*cVals[5] ) < 0.0; - } + const double lenNormal = sqrt(normal[0]*normal[0] + normal[1]*normal[1] + normal[2]*normal[2]); + const double lenEdgeVec = sqrt(edgeVec[0]*edgeVec[0] + edgeVec[1]*edgeVec[1] + edgeVec[2]*edgeVec[2]); + const double dotProd = normal[0]*edgeVec[0] + normal[1]*edgeVec[1] + normal[2]*edgeVec[2]; + + const double angle = 3.14159262535 - acos( dotProd / ( lenNormal * lenEdgeVec ) ); + + if(!isMinInitialised || angle < minAngle) + { + minAngle = angle; + minRow = row; + isMinInitialised = true; + } + + } } - } - - return false; - } - - /** - * Tests if the given corner of triangle PQR lies in the interior of the unit tetrahedron - * (0 <= x_p, y_p, z_p, h_p <= 1) - * - * @param corner corner of the triangle PQR - * @returns true if the corner lies in the interior of the unit tetrahedron. - */ - bool TransformedTriangle::testCornerInTetrahedron(const TriCorner corner) const - { - const double pt[4] = - { - _coords[5*corner], // x - _coords[5*corner + 1], // y - _coords[5*corner + 2], // z - _coords[5*corner + 3] // z - }; - - for(int i = 0 ; i < 4 ; ++i) - { - if(pt[i] < 0.0 || pt[i] > 1.0) + + if(isGoodRowFound) { - return false; + if(minAngle < TRIPLE_PRODUCT_ANGLE_THRESHOLD) + { + _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, true); + } + else + { + _tripleProducts[corner] = calcTByDevelopingRow(corner, minRow, false); + } + _validTP[corner] = true; } - } - return true; - } - - /** - * Tests if the given corner of triangle PQR lies on the facet h = 0 (the XYZ facet) - * (0 <= x_p, y_p, z_p <= 1 && h_p = 0) - * - * @param corner corner of the triangle PQR - * @returns true if the corner lies on the facet h = 0 - */ - bool TransformedTriangle::testCornerOnXYZFacet(const TriCorner corner) const - { - const double pt[4] = - { - _coords[5*corner], // x - _coords[5*corner + 1], // y - _coords[5*corner + 2], // z - _coords[5*corner + 3] // h - }; - - if(pt[3] != 0.0) - { - return false; - } - - for(int i = 0 ; i < 3 ; ++i) - { - if(pt[i] < 0.0 || pt[i] > 1.0) + else { - return false; + // this value will not be used + // we set it to whatever + // std::cout << "Triple product not calculated for corner " << corner << std::endl; + _tripleProducts[corner] = -3.14159265; + _validTP[corner] = false; + } + } - return true; + + _isTripleProductsCalculated = true; } - - //////////////////////////////////////////////////////////////////////////////////// - /// Utility methods used in intersection tests /////////////// - //////////////////////////////////////////////////////////////////////////////////// /** - * Tests if the triangle PQR surrounds the axis on which the - * given edge of the tetrahedron lies. + * Returns the stable double product c_{xy}^{pq} + * + * @pre The stable double products have been calculated with preCalculateDoubleProducts. + * @param seg segment of triangle + * @param dp double product sought + * + * @returns stabilised double product c_{xy}^{pq} * - * @param edge edge of tetrahedron - * @returns true if PQR surrounds edge, false if not (see Grandy, eq. [53]) */ - bool TransformedTriangle::testTriangleSurroundsEdge(const TetraEdge edge) const + double TransformedTriangle::calcStableC(const TriSegment seg, const DoubleProduct dp) const { - // NB DoubleProduct enum corresponds to TetraEdge enum according to Grandy, table III - // so we can use the edge directly - - // optimization : we could use _doubleProducts directly here - const double cPQ = calcStableC(TransformedTriangle::PQ, DoubleProduct(edge)); - const double cQR = calcStableC(TransformedTriangle::QR, DoubleProduct(edge)); - const double cRP = calcStableC(TransformedTriangle::RP, DoubleProduct(edge)); - - // if two or more c-values are zero we disallow x-edge intersection - // Grandy, p.446 - const int numZeros = (cPQ == 0.0 ? 1 : 0) + (cQR == 0.0 ? 1 : 0) + (cRP == 0.0 ? 1 : 0); - - return (cPQ*cQR >= 0.0) && (cQR*cRP >=0.0) && (cRP*cPQ >= 0.0) && numZeros < 2; + assert(_isDoubleProductsCalculated); + return _doubleProducts[8*seg + dp]; } + /** - * Tests if the corners of the given edge lie on different sides of the triangle PQR. + * Returns the stable triple product t_X for a given corner + * The triple product gives the signed volume of the tetrahedron between + * this corner and the triangle PQR. These triple products have been calculated + * in a way to avoid problems with cancellation. * - * @param edge edge of the tetrahedron - * @returns true if the corners of the given edge lie on different sides of the triangle PQR - * or if one (but not both) lies in the plane of the triangle. + * @pre double products have already been calculated + * @pre triple products have already been calculated + * @param corner corner for which the triple product is calculated + * @returns triple product associated with corner (see Grandy, eqs. [50]-[52]) */ - bool TransformedTriangle::testEdgeIntersectsTriangle(const TetraEdge edge) const + double TransformedTriangle::calcStableT(const TetraCorner corner) const { - - assert(edge < H01); - - // correspondance edge - triple products - // for edges OX, ..., ZX (Grandy, table III) - static const TetraCorner TRIPLE_PRODUCTS[12] = - { - X, O, // OX - Y, O, // OY - Z, O, // OZ - Y, Z, // YZ - Z, X, // ZX - X, Y // XY - }; - - // Grandy, [16] - const double t1 = calcStableT(TRIPLE_PRODUCTS[2*edge]); - const double t2 = calcStableT(TRIPLE_PRODUCTS[2*edge + 1]); - - //? should equality with zero use epsilon? - return (t1*t2 <= 0.0) && (t1 - t2 != 0.0); + assert(_isTripleProductsCalculated); + // assert(_validTP[corner]); + return _tripleProducts[corner]; } /** - * Tests if the given facet of the tetrahedron surrounds the axis on which the - * given segment of the triangle lies. + * Calculates the given double product c_{xy}^{pq} = x_p*y_q - y_p*x_q for a + * a segment PQ of the triangle. This method does not compensate for + * precision errors. + * + * @param seg segment of triangle + * @param dp double product sought + * + * @returns double product c_{xy}^{pq} * - * @param seg segment of the triangle - * @param facet facet of the tetrahedron - * @returns true if the facet surrounds the segment, false if not (see Grandy, eq. [18]) */ - bool TransformedTriangle::testFacetSurroundsSegment(const TriSegment seg, const TetraFacet facet) const + double TransformedTriangle::calcUnstableC(const TriSegment seg, const DoubleProduct dp) const { + + // find the points of the triangle + // 0 -> P, 1 -> Q, 2 -> R + const int pt1 = seg; + const int pt2 = (seg + 1) % 3; - const double signs[3] = - { - SIGN_FOR_SEG_FACET_INTERSECTION[3*facet], - SIGN_FOR_SEG_FACET_INTERSECTION[3*facet + 1], - SIGN_FOR_SEG_FACET_INTERSECTION[3*facet + 2] - }; - - const double c1 = signs[0]*calcStableC(seg, DP_FOR_SEG_FACET_INTERSECTION[3*facet]); - const double c2 = signs[1]*calcStableC(seg, DP_FOR_SEG_FACET_INTERSECTION[3*facet + 1]); - const double c3 = signs[2]*calcStableC(seg, DP_FOR_SEG_FACET_INTERSECTION[3*facet + 2]); + // find offsets + const int off1 = DP_OFFSET_1[dp]; + const int off2 = DP_OFFSET_2[dp]; - return (c1*c3 > 0.0) && (c2*c3 > 0.0); + return _coords[5*pt1 + off1] * _coords[5*pt2 + off2] - _coords[5*pt1 + off2] * _coords[5*pt2 + off1]; } /** - * Tests if the corners of the given segment lie on different sides of the given facet. - * - * @param seg segment of the triangle - * @param facet facet of the tetrahedron - * @returns true if the corners of the given segment lie on different sides of the facet - * or if one (but not both) lies in the plane of the facet. (see Grandy, eq. [18]) - */ - bool TransformedTriangle::testSegmentIntersectsFacet(const TriSegment seg, const TetraFacet facet) const - { - // use correspondance facet a = 0 <=> offset for coordinate a in _coords - // and also correspondance segment AB => corner A - const double coord1 = _coords[5*seg + facet]; - const double coord2 = _coords[5*( (seg + 1) % 3) + facet]; - - //? should we use epsilon-equality here in second test? - // std::cout << "coord1 : " << coord1 << " coord2 : " << coord2 << std::endl; - return (coord1*coord2 <= 0.0) && (coord1 != coord2); - } - - /** - * Tests if the triangle PQR lies above a given corner in the z-direction (implying that the - * ray pointing upward in the z-direction from the corner can intersect the triangle) - * (Grandy eq.[28]) - * - * @param corner corner of the tetrahedron - * @returns true if the triangle lies above the corner in the z-direction + * Calculates triple product associated with the given corner of tetrahedron, developing + * the determinant by the given row. The triple product gives the signed volume of + * the tetrahedron between this corner and the triangle PQR. If the flag project is true, + * one coordinate is projected out in order to eliminate errors in the intersection point + * calculation due to cancellation. + * + * @pre double products have already been calculated + * @param corner corner for which the triple product is calculated + * @param row row (1 <= row <= 3) used to calculate the determinant + * @param project indicates whether or not to perform projection as inidicated in Grandy, p.446 + * @returns triple product associated with corner (see Grandy, [50]-[52]) */ - bool TransformedTriangle::testSurfaceAboveCorner(const TetraCorner corner) const - { - //? is it always YZ here ? - const double normal = calcStableC(PQ, C_YZ) + calcStableC(QR, C_YZ) + calcStableC(RP, C_YZ); - return ( calcStableT(corner) * normal ) >= 0.0; - } - /** - * Tests if the triangle PQR surrounds the ray pointing upwards in the z-direction - * from the given corner. - * - * @param corner corner on face XYZ of tetrahedron - * @returns true if PQR surrounds ray, false if not (see Grandy, eq. [18]) - */ - bool TransformedTriangle::testTriangleSurroundsRay(const TetraCorner corner) const + double TransformedTriangle::calcTByDevelopingRow(const TetraCorner corner, const int row, const bool project) const { - assert(corner == X || corner == Y || corner == Z); - - // double products to use for the possible corners - static const DoubleProduct DP_FOR_RAY_INTERSECTION[4] = + + // OVERVIEW OF CALCULATION + // --- sign before the determinant + // the sign used depends on the sign in front of the triple product (Grandy, [15]), + // and the convention used in the definition of the double products + + // the sign in front of the determinant gives the following schema for the three terms (I): + // corner/row 1 2 3 + // O (sign:+) + - + + // X (sign:-) - + - + // Y (sign:-) - + - + // Z (sign:-) - + - + + // the 2x2 determinants are the following (C_AB <=> A_p*B_q - B_p*A_q, etc) + // corner/row 1 2 3 + // O (sign:+) C_YZ C_XZ C_XY + // X (sign:-) C_YZ C_HZ C_HY + // Y (sign:-) C_HZ C_XZ C_XH + // Z (sign:-) C_YH C_XH C_XY + + // these are represented in DP_FOR_DETERMINANT_EXPANSION, + // except for the fact that certain double products are inversed (C_AB <-> C_BA) + + // comparing with the DOUBLE_PRODUCTS and using the fact that C_AB = -C_BA + // we deduce the following schema (II) : + // corner/row 1 2 3 + // O (sign:+) + - + + // X (sign:-) + - - + // Y (sign:-) - - + + // Z (sign:-) + + + + + // comparing the two schemas (I) and (II) gives us the following matrix of signs, + // putting 1 when the signs in (I) and (II) are equal and -1 when they are different : + + static const int SIGNS[12] = { - DoubleProduct(0), // O - only here to fill out and make indices match - C_10, // X - C_01, // Y - C_XY // Z + 1, 1, 1, + -1,-1, 1, + 1,-1,-1, + -1, 1,-1 }; - const DoubleProduct dp = DP_FOR_RAY_INTERSECTION[corner]; + // find the offsets of the rows of the determinant + const int offset = COORDINATE_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)]; + + const DoubleProduct dp = DP_FOR_DETERMINANT_EXPANSION[3 * corner + (row - 1)]; + + const int sign = SIGNS[3 * corner + (row - 1)]; - const double cPQ = calcStableC(PQ, dp); const double cQR = calcStableC(QR, dp); const double cRP = calcStableC(RP, dp); + const double cPQ = calcStableC(PQ, dp); + + // coordinate to use for projection (Grandy, [57]) with edges + // OX, OY, OZ, YZ, ZX, XY in order : + // (y, z, x, h, h, h) + // for the first three we could also use {2, 0, 1} + static const int PROJECTION_COORDS[6] = { 1, 2, 0, 3, 3, 3 } ; + + double alpha = 0.0; + + const int coord = PROJECTION_COORDS[ dp ]; + + // coordinate values for P, Q and R + const double coordValues[3] = { _coords[5*P + coord], _coords[5*Q + coord], _coords[5*R + coord] }; + + if(project) + { + // products coordinate values with corresponding double product + const double coordDPProd[3] = { coordValues[0] * cQR, coordValues[1] * cRP, coordValues[0] * cPQ }; + + const double sumDPProd = coordDPProd[0] + coordDPProd[1] + coordDPProd[2]; + const double sumDPProdSq = coordDPProd[0]*coordDPProd[0] + coordDPProd[1]*coordDPProd[1] + coordDPProd[2]*coordDPProd[2]; + alpha = sumDPProd / sumDPProdSq; + } - //? NB here we have no correction for precision - is this good? - // Our authority Grandy says nothing - return ( cPQ*cQR > 0.0 ) && ( cPQ*cRP > 0.0 ); + const double p_term = _coords[5*P + offset] * cQR * (1.0 - alpha * coordValues[0] * cQR) ; + const double q_term = _coords[5*Q + offset] * cRP * (1.0 - alpha * coordValues[1] * cRP) ; + const double r_term = _coords[5*R + offset] * cPQ * (1.0 - alpha * coordValues[2] * cPQ) ; + + // NB : using plus also for the middle term compensates for a double product + // which is inversely ordered + // std::cout << "Triple product for corner " << corner << ", row " << row << " = " << sign*( p_term + q_term + r_term ) << std::endl; + return sign*( p_term + q_term + r_term ); } diff --git a/src/INTERP_KERNEL/VectorUtils.hxx b/src/INTERP_KERNEL/VectorUtils.hxx new file mode 100644 index 000000000..e0dc18162 --- /dev/null +++ b/src/INTERP_KERNEL/VectorUtils.hxx @@ -0,0 +1,61 @@ +#ifndef __VECTOR_UTILS_HXX__ +#define __VECTOR_UTILS_HXX__ + +#include +#include +#include + +namespace INTERP_UTILS +{ + + void copyVector3(const double* src, double* dest) + { + for(int i = 0 ; i < 3 ; ++i) + { + dest[i] = src[i]; + } + } + + const std::string vToStr(const double* pt) + { + std::stringstream ss(std::ios::out); + ss << "[" << pt[0] << ", " << pt[1] << ", " << pt[2] << "]"; + return ss.str(); + } + + void cross(const double* v1, const double* v2,double* res) + { + res[0] = v1[1]*v2[2] - v1[2]*v2[1]; + res[1] = v1[2]*v2[0] - v1[0]*v2[2]; + res[2] = v1[0]*v2[1] - v1[1]*v2[0]; + } + + double dot(const double* v1, const double* v2) + { + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; + } + + double norm(const double* v) + { + return sqrt(dot(v,v)); + } + + double angleBetweenVectors(const double* v1, const double* v2, const double* n) + { + const double denominator = dot(v1, v2); + double v3[3]; + + cross(v1, v2, v3); + const double numerator = dot(n, v3); + return atan2(numerator, denominator); + + } + + + +}; + + + + +#endif -- 2.39.2