Skip to content

Commit

Permalink
[Geometry] Update method intersectionWithEdge signature and redirect …
Browse files Browse the repository at this point in the history 
…all methods to it in EdgeSetGeometryAlgorithms (sofa-framework#4194)

* [Geometry] Update method signature of intersectionWithEdge to pass barycentric coordinates instead of 3D coord.

* [Topology.Container] Update all method compute2EdgesIntersection to use the generic method in Edge class

* Apply suggestions from code review

Co-authored-by: Frederick Roy <[email protected]>

* Update EdgeSetGeometryAlgorithms.inl

* Update EdgeSetGeometryAlgorithms.inl

* fix sofacuda

* Fix snapping error

* Fix incision test output values

* Update Sofa/Component/Topology/Container/Dynamic/src/sofa/component/topology/container/dynamic/EdgeSetGeometryAlgorithms.h

Co-authored-by: Paul Baksic <[email protected]>

---------

Co-authored-by: Frederick Roy <[email protected]>
Co-authored-by: Frederick Roy <[email protected]>
Co-authored-by: Paul Baksic <[email protected]>
  • Loading branch information
@epernod @fredroy @bakpaul
4 people authored Apr 12, 2024
1 parent 7eec68b commit df6198a
Show file tree
Hide file tree
Showing 7 changed files with 124 additions and 145 deletions.
17 changes: 12 additions & 5 deletions ...ntainer/Dynamic/src/sofa/component/topology/container/dynamic/EdgeSetGeometryAlgorithms.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,11 +130,19 @@ class EdgeSetGeometryAlgorithms : public PointSetGeometryAlgorithms<DataTypes>
/** \brief Compute the intersection coordinate of the 2 input straight lines. Lines vector director are computed using coord given in input.
* @param edge1 tab Coord[2] from the 2 vertices composing first edge
* @param edge2 same for second edge
* @param intersected bool default value true, changed as false if no intersection is done.
* @param intersected bool set to true if intersection otherwise false.
* @return Coord of intersection point, 0 if no intersection.
*/
Coord compute2EdgesIntersection (const Coord edge1[2], const Coord edge2[2], bool& intersected);

/** \brief Compute the intersection coordinate of an Edge from the topology and a segment defined by 2 points [a, b].
* @param edgeID index of the first edge
* @param segment defined by 2 points [a, b]
* @param intersected bool set to true if intersection otherwise false.
* @return Coord of intersection point, 0 if no intersection.
*/
Coord computeEdgeSegmentIntersection(const EdgeID edgeID, const type::Vec3& a, const type::Vec3& b, bool& intersected);

bool computeEdgePlaneIntersection (EdgeID edgeID, sofa::type::Vec<3,Real> pointOnPlane, sofa::type::Vec<3,Real> normalOfPlane, sofa::type::Vec<3,Real>& intersection);
bool computeRestEdgePlaneIntersection (EdgeID edgeID, sofa::type::Vec<3,Real> pointOnPlane, sofa::type::Vec<3,Real> normalOfPlane, sofa::type::Vec<3,Real>& intersection);

Expand All @@ -160,10 +168,9 @@ class EdgeSetGeometryAlgorithms : public PointSetGeometryAlgorithms<DataTypes>
/** return a pointer to the container of cubature points */
NumericalIntegrationDescriptor<Real,1> &getEdgeNumericalIntegrationDescriptor();

bool computeEdgeSegmentIntersection(EdgeID edgeID,
const sofa::type::Vec<3,Real>& a,
const sofa::type::Vec<3, Real>& b,
Real &baryCoef);

SOFA_ATTRIBUTE_DEPRECATED("v24.06", "v24.12", "Use the method computeEdgeSegmentIntersection returning a Coord")
bool computeEdgeSegmentIntersection(EdgeID edgeID, const type::Vec3& a, const type::Vec3& b, Real &baryCoef);


// compute barycentric coefficients
Expand Down
139 changes: 46 additions & 93 deletions ...ainer/Dynamic/src/sofa/component/topology/container/dynamic/EdgeSetGeometryAlgorithms.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -246,6 +246,7 @@ void EdgeSetGeometryAlgorithms< DataTypes >::defineEdgeCubaturePoints() {
}
edgeNumericalIntegration.addQuadratureMethod(m,10,qpa);
}

template< class DataTypes>
typename DataTypes::Real EdgeSetGeometryAlgorithms< DataTypes >::computeEdgeLength( const EdgeID i) const
{
Expand Down Expand Up @@ -438,9 +439,13 @@ auto EdgeSetGeometryAlgorithms<DataTypes>::computePointProjectionOnEdge (const E

// Compute Coord of projection point H:
Coord coord_H = compute2EdgesIntersection ( coord_edge1, coord_edge2, intersected);
sofa::type::Vec<3, Real> h; DataTypes::get(h[0], h[1], h[2], coord_H);

return computeEdgeBarycentricCoordinates(h, theEdge[0], theEdge[1]);
if (intersected)
{
sofa::type::Vec<3, Real> h; DataTypes::get(h[0], h[1], h[2], coord_H);
return computeEdgeBarycentricCoordinates(h, theEdge[0], theEdge[1]);
}
else
return sofa::type::vector< SReal >();
}

template<class DataTypes>
Expand Down Expand Up @@ -493,73 +498,43 @@ bool EdgeSetGeometryAlgorithms<DataTypes>::computeRestEdgePlaneIntersection (Edg

}


template<class DataTypes>
typename DataTypes::Coord EdgeSetGeometryAlgorithms<DataTypes>::compute2EdgesIntersection (const Coord edge1[2], const Coord edge2[2], bool& intersected)
{
auto a0 = type::Vec3(DataTypes::getCPos(edge1[0]));
auto a1 = type::Vec3(DataTypes::getCPos(edge1[1]));
auto b0 = type::Vec3(DataTypes::getCPos(edge2[0]));
auto b1 = type::Vec3(DataTypes::getCPos(edge2[1]));

// Creating director vectors:
Coord vec1 = edge1[1] - edge1[0];
Coord vec2 = edge2[1] - edge2[0];
Coord X;
for (unsigned int i=0; i<Coord::spatial_dimensions; i++)
X[i] = 0;

int ind1 = -1;
int ind2 = -1;
constexpr Real epsilon = static_cast<Real>(0.0001);
Real lambda = 0.0;
Real alpha = 0.0;

// Searching vector composante not null:
for (unsigned int i=0; i<Coord::spatial_dimensions; i++)
{
if ( (vec1[i] > epsilon) || (vec1[i] < -epsilon) )
{
ind1 = i;

for (unsigned int j = 0; j<Coord::spatial_dimensions; j++)
if ( (vec2[j] > epsilon || vec2[j] < -epsilon) && (j != i))
{
ind2 = j;
type::Vec2 baryCoords(type::NOINIT);
intersected = sofa::geometry::Edge::intersectionWithEdge(a0, a1, b0, b1, baryCoords);

// Solving system:
Real coef_lambda = vec1[ind1] - ( vec1[ind2]*vec2[ind1]/vec2[ind2] );
type::vector< Coord > ancestors = {edge1[0], edge1[1]};
type::vector< Real > coefs = { static_cast<Real>(baryCoords[0]), static_cast<Real>(baryCoords[1])};

if (coef_lambda < epsilon && coef_lambda > -epsilon)
break;

lambda = ( edge2[0][ind1] - edge1[0][ind1] + (edge1[0][ind2] - edge2[0][ind2])*vec2[ind1]/vec2[ind2]) * 1/coef_lambda;
alpha = (edge1[0][ind2] + lambda * vec1[ind2] - edge2[0][ind2]) * 1 /vec2[ind2];
break;
}
}
return DataTypes::interpolate(ancestors, coefs);
}

if (lambda != 0.0)
break;
}

if ((ind1 == -1) || (ind2 == -1))
{
msg_error() << "Vector director is null." ;
intersected = false;
return X;
}
template<class DataTypes>
typename DataTypes::Coord EdgeSetGeometryAlgorithms<DataTypes>::computeEdgeSegmentIntersection(const EdgeID edgeID, const type::Vec3& a, const type::Vec3& b, bool& intersected)
{
const Edge& theEdge = this->m_topology->getEdge(edgeID);
const VecCoord& pos = (this->object->read(core::ConstVecCoordId::position())->getValue());

// Compute X coords:
for (unsigned int i = 0; i<Coord::spatial_dimensions; i++)
X[i] = edge1[0][i] + (float)lambda * vec1[i];
const typename DataTypes::Coord& e0 = pos[theEdge[0]];
const typename DataTypes::Coord& e1 = pos[theEdge[1]];
auto p0 = type::Vec3(DataTypes::getCPos(e0));
auto p1 = type::Vec3(DataTypes::getCPos(e1));

intersected = true;
type::Vec2 baryCoords(type::NOINIT);
intersected = sofa::geometry::Edge::intersectionWithEdge(p0, p1, a, b, baryCoords);

// Check if lambda found is really a solution
for (unsigned int i = 0; i<Coord::spatial_dimensions; i++)
if ( (X[i] - edge2[0][i] - alpha * vec2[i]) > 0.1 )
{
msg_error() << "Edges don't intersect themself." ;
intersected = false;
}
type::vector< Coord > ancestors = {e0, e1};
type::vector< Real > coefs = { static_cast<Real>(baryCoords[0]), static_cast<Real>(baryCoords[1]) };

return X;
return DataTypes::interpolate(ancestors, coefs);
}


Expand Down Expand Up @@ -770,44 +745,22 @@ void EdgeSetGeometryAlgorithms<DataTypes>::initPointAdded(PointID index, const c

template<class DataTypes>
bool EdgeSetGeometryAlgorithms<DataTypes>::computeEdgeSegmentIntersection(EdgeID edgeID,
const sofa::type::Vec<3, Real>& a,
const sofa::type::Vec<3, Real>& b,
const type::Vec3& a,
const type::Vec3& b,
Real &baryCoef)
{
bool is_intersect = false;

const Edge& e = this->m_topology->getEdge(edgeID);
const VecCoord& p = (this->object->read(core::ConstVecCoordId::position())->getValue());
const typename DataTypes::Coord& c0 = p[e[0]];
const typename DataTypes::Coord& c1 = p[e[1]];

sofa::type::Vec<3, Real> p0{ c0[0],c0[1],c0[2] };
sofa::type::Vec<3, Real> p1{ c1[0],c1[1],c1[2] };
sofa::type::Vec<3, Real> pa{ a[0],a[1],a[2] };
sofa::type::Vec<3, Real> pb{ b[0],b[1],b[2] };

sofa::type::Vec<3, Real> v_0a = p0 - pa;
sofa::type::Vec<3, Real> v_ba = pb - pa;
sofa::type::Vec<3, Real> v_10 = p1 - p0;

Real d0aba, dba10, d0a10, dbaba, d1010;

d0aba = v_0a * v_ba;
dba10 = v_ba * v_ba;
d0a10 = v_0a * v_10;
dbaba = v_ba * v_ba;
d1010 = v_10 * v_10;

Real deno, num;
deno = d1010 * dbaba - dba10 * dba10;

if (abs(deno) > std::numeric_limits<Real>::epsilon())
{
num = d0aba * dba10 - d0a10 * dbaba;
const Edge& theEdge = this->m_topology->getEdge(edgeID);
const VecCoord& pos = (this->object->read(core::ConstVecCoordId::position())->getValue());

const typename DataTypes::Coord& e0 = pos[theEdge[0]];
const typename DataTypes::Coord& e1 = pos[theEdge[1]];
auto p0 = type::Vec3(DataTypes::getCPos(e0));
auto p1 = type::Vec3(DataTypes::getCPos(e1));

type::Vec2 baryCoords(type::NOINIT);
bool is_intersect = sofa::geometry::Edge::intersectionWithEdge(p0, p1, a, b, baryCoords);
baryCoef = baryCoords[0];

baryCoef = num / deno;
is_intersect = true;
}
return is_intersect;
}

Expand Down
12 changes: 8 additions & 4 deletions ...r/Dynamic/src/sofa/component/topology/container/dynamic/TriangleSetGeometryAlgorithms.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4428,8 +4428,10 @@ void TriangleSetGeometryAlgorithms<DataTypes>::SnapBorderPath(PointID pa, Coord&

auto new_coord = this->computePointProjectionOnEdge(theEdge, thePoint, intersected);

if (!intersected)
msg_error() << "Orthogonal projection failed";
if (!intersected){
// Orthogonal projection failed, not possible to snap this point on the ith edge of the triangle
continue;
}

topoPath_list[0] = geometry::ElementType::EDGE;

Expand Down Expand Up @@ -4532,8 +4534,10 @@ void TriangleSetGeometryAlgorithms<DataTypes>::SnapBorderPath(PointID pa, Coord&
DataTypes::get(thePoint[0], thePoint[1], thePoint[2], b);
auto new_coord = this->computePointProjectionOnEdge(theEdge, thePoint, intersected);

if (!intersected)
msg_error() << "Orthogonal projection failed";
if (!intersected){
// Orthogonal projection failed, not possible to snap this point on the ith edge of the triangle
continue;
}

topoPath_list.back() = geometry::ElementType::EDGE;
indices_list.back() = theEdge;
Expand Down
6 changes: 3 additions & 3 deletions Sofa/Component/Topology/Utility/tests/TopologicalChangeProcessor_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,9 +121,9 @@ struct InciseProcessor_test : TopologicalChangeProcessor_test
m_instance.simulate(0.05);
}

EXPECT_EQ(topoCon->getNbTriangles(), 1677);
EXPECT_EQ(topoCon->getNbEdges(), 2704);
EXPECT_EQ(topoCon->getNbPoints(), 1026);
EXPECT_EQ(topoCon->getNbTriangles(), 1648);
EXPECT_EQ(topoCon->getNbEdges(), 2646);
EXPECT_EQ(topoCon->getNbPoints(), 997);

return true;
}
Expand Down
16 changes: 8 additions & 8 deletions Sofa/framework/Geometry/src/sofa/geometry/Edge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,15 +216,15 @@ struct Edge
* @tparam T scalar
* @param pA, pB nodes of the first edge
* @param pC, pD nodes of the second edge
* @param intersection node will be filled if there is an intersection otherwise will return std::numeric_limits<T>::min()
* @param intersectionBaryCoord barycentric coordinates of the intersection point expressed as alpa * pA + beta * pB if there is an intersection , node will be filled if there is an intersection otherwise will return [0, 0]
* @return bool true if there is an intersection, otherwise false
*/
template<typename Node,
typename T = std::decay_t<decltype(*std::begin(std::declval<Node>()))>,
typename = std::enable_if_t<std::is_scalar_v<T>>
>
[[nodiscard]]
static constexpr bool intersectionWithEdge(const Node& pA, const Node& pB, const Node& pC, const Node& pD, Node& intersection)
static constexpr bool intersectionWithEdge(const Node& pA, const Node& pB, const Node& pC, const Node& pD, sofa::type::Vec<2, T>& intersectionBaryCoord)
{
// The 2 segment equations using pX on edge1 and pY on edge2 can be defined by:
// pX = pA + alpha (pB - pA)
Expand All @@ -243,20 +243,20 @@ struct Edge

if (alphaDenom < std::numeric_limits<T>::epsilon()) // collinear
{
intersection = sofa::type::Vec<2, T>(std::numeric_limits<T>::min(), std::numeric_limits<T>::min());
intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
return false;
}

const T alpha = alphaNom / alphaDenom;

if (alpha < 0 || alpha > 1)
{
intersection = sofa::type::Vec<2, T>(std::numeric_limits<T>::min(), std::numeric_limits<T>::min());
intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
return false;
}
else
{
intersection = pA + alpha * AB;
intersectionBaryCoord = sofa::type::Vec<2, T>(1-alpha, alpha);
return true;
}
}
Expand Down Expand Up @@ -286,7 +286,7 @@ struct Edge

if (alphaDenom < std::numeric_limits<T>::epsilon()) // alpha == inf, not sure what it means geometrically, colinear?
{
intersection = sofa::type::Vec<3, T>(std::numeric_limits<T>::min(), std::numeric_limits<T>::min(), std::numeric_limits<T>::min());
intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
return false;
}

Expand All @@ -300,12 +300,12 @@ struct Edge
|| alpha > 1 || beta > 1 // if alpha > 1 means intersection but after outside from [AB]
|| (pY - pX).norm2() > EQUALITY_THRESHOLD ) // if pY and pX are not se same means no intersection.
{
intersection = sofa::type::Vec<3, T>(std::numeric_limits<T>::min(), std::numeric_limits<T>::min(), std::numeric_limits<T>::min());
intersectionBaryCoord = sofa::type::Vec<2, T>(0, 0);
return false;
}
else
{
intersection = pX;
intersectionBaryCoord = sofa::type::Vec<2, T>(1-alpha, alpha);
return true;
}
}
Expand Down
Loading

0 comments on commit df6198a

Please sign in to comment.