diff --git a/src/pe_coupling/geometry/PeIntersectionRatio.cpp b/src/pe_coupling/geometry/PeIntersectionRatio.cpp index c9ef56e2df4f0f7b15b7aeebe4b65cda5b65f094..bd5518724940238b95c0971395e0e81c61d94e71 100644 --- a/src/pe_coupling/geometry/PeIntersectionRatio.cpp +++ b/src/pe_coupling/geometry/PeIntersectionRatio.cpp @@ -22,6 +22,8 @@ #include "pe_coupling/geometry/PeIntersectionRatio.h" +#include "core/math/Matrix3.h" + namespace walberla { namespace lbm { @@ -52,6 +54,68 @@ real_t intersectionRatioSpherePe( const pe::Sphere & sphere, } +real_t intersectionRatioPlanePe( const pe::Plane & plane, + const Vector3<real_t> & fluidPoint, + const Vector3<real_t> & direction ) +{ + WALBERLA_ASSERT( !walberla::geometry::contains<pe::RigidBody>( plane, fluidPoint ), "fluidPoint: " << fluidPoint ); + WALBERLA_ASSERT( walberla::geometry::contains<pe::RigidBody>( plane, fluidPoint + direction ), "fluidPoint + direction: " << fluidPoint + direction ); + + + Vector3<real_t> planeCenter( plane.getPosition() ); + Vector3<real_t> planeNormal( plane.getNormal() ); + + real_t denom = planeNormal * direction; + + auto diff = planeCenter - fluidPoint; + + WALBERLA_ASSERT_FLOAT_UNEQUAL(denom, real_t(0)); + + real_t delta = diff * planeNormal / denom; + + WALBERLA_ASSERT_GREATER_EQUAL( delta, real_t( 0 ) ); + WALBERLA_ASSERT_LESS_EQUAL( delta, real_t( 1 ) ); + + return delta; + +} + +real_t intersectionRatioEllipsoidPe( const pe::Ellipsoid & ellipsoid, + const Vector3<real_t> & fluidPoint, + const Vector3<real_t> & direction ) +{ + WALBERLA_ASSERT( !walberla::geometry::contains<pe::RigidBody>( ellipsoid, fluidPoint ), "fluidPoint: " << fluidPoint ); + WALBERLA_ASSERT( walberla::geometry::contains<pe::RigidBody>( ellipsoid, fluidPoint + direction ), "fluidPoint + direction: " << fluidPoint + direction ); + + Vector3<real_t> transformedP = ellipsoid.pointFromWFtoBF(fluidPoint); + Vector3<real_t> transformedDir = ellipsoid.vectorFromWFtoBF(direction); + + Vector3<real_t> semiAxes = ellipsoid.getSemiAxes(); + + Matrix3<real_t> M = Matrix3<real_t>::makeDiagonalMatrix(real_t(1)/semiAxes[0], real_t(1)/semiAxes[1], real_t(1)/semiAxes[2]); + + Vector3<real_t> P_M = M*transformedP; + Vector3<real_t> d_M = M*transformedDir; + + const real_t a = d_M*d_M; + const real_t b = real_t(2)*P_M*d_M; + const real_t c = P_M*P_M - real_t(1); + + const real_t discriminant = b*b - real_t(4)*a*c; + + WALBERLA_ASSERT_GREATER_EQUAL(discriminant, real_t(0), "No intersection possible!"); + WALBERLA_ASSERT_FLOAT_UNEQUAL(a, real_t(0)); + + const real_t root = std::sqrt(discriminant); + real_t delta = (-b - root) / (real_t(2) * a); + + WALBERLA_ASSERT_GREATER_EQUAL( delta, real_t( 0 ) ); + WALBERLA_ASSERT_LESS_EQUAL( delta, real_t( 1 ) ); + + return delta; + +} + } // namespace lbm } // namespace walberla diff --git a/src/pe_coupling/geometry/PeIntersectionRatio.h b/src/pe_coupling/geometry/PeIntersectionRatio.h index d6fb8d517d43835eda7518388d9e3f0be8bd08c0..8096657194ec707d42b1a96ba461efe1d955529a 100644 --- a/src/pe_coupling/geometry/PeIntersectionRatio.h +++ b/src/pe_coupling/geometry/PeIntersectionRatio.h @@ -26,6 +26,8 @@ #include "pe_coupling/geometry/PeBodyOverlapFunctions.h" #include "pe/rigidbody/RigidBody.h" +#include "pe/rigidbody/Ellipsoid.h" +#include "pe/rigidbody/Plane.h" #include "pe/rigidbody/Sphere.h" #include "pe/Types.h" @@ -37,6 +39,13 @@ real_t intersectionRatioSpherePe( const pe::Sphere & sphere, const Vector3<real_t> & fluidPoint, const Vector3<real_t> & direction ); +real_t intersectionRatioPlanePe( const pe::Plane & plane, + const Vector3<real_t> & fluidPoint, + const Vector3<real_t> & direction ); + +real_t intersectionRatioEllipsoidPe( const pe::Ellipsoid & ellipsoid, + const Vector3<real_t> & fluidPoint, + const Vector3<real_t> & direction ); inline real_t intersectionRatio( const pe::RigidBody & peRigidBody, const Vector3<real_t> & fluidPoint, @@ -50,6 +59,19 @@ inline real_t intersectionRatio( const pe::RigidBody & peRigidBody, WALBERLA_ASSERT_LESS_EQUAL( std::fabs( ( fluidPoint + ratio * direction - sphere.getPosition() ).length() - sphere.getRadius() ), epsilon ); return ratio; } + else if ( peRigidBody.getTypeID() == pe::Plane::getStaticTypeID() ) + { + const pe::Plane & plane = static_cast< const pe::Plane & >( peRigidBody ); + const real_t ratio = intersectionRatioPlanePe( plane, fluidPoint, direction ); + WALBERLA_ASSERT_FLOAT_EQUAL( ( fluidPoint + ratio * direction - plane.getPosition() ) * plane.getNormal(), real_t(0) ); + return ratio; + } + else if ( peRigidBody.getTypeID() == pe::Ellipsoid::getStaticTypeID() ) + { + const pe::Ellipsoid & ellipsoid = static_cast< const pe::Ellipsoid & >( peRigidBody ); + const real_t ratio = intersectionRatioEllipsoidPe( ellipsoid, fluidPoint, direction ); + return ratio; + } // Add more pe bodies here if specific intersectionRatio(...) function is available else {