RayParticleIntersection.h 7.74 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
//======================================================================================================================
//
//  This file is part of waLBerla. waLBerla is free software: you can
//  redistribute it and/or modify it under the terms of the GNU General Public
//  License as published by the Free Software Foundation, either version 3 of
//  the License, or (at your option) any later version.
//
//  waLBerla is distributed in the hope that it will be useful, but WITHOUT
//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
//  for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with waLBerla (see COPYING.txt). If not, see <http://www.gnu.org/licenses/>.
//
16
//! \file
17
18
19
20
21
22
23
24
25
26
//! \author Christoph Rettinger <christoph.rettinger@fau.de>
//
//======================================================================================================================

#pragma once

#include <mesa_pd/common/Contains.h>
#include <mesa_pd/data/DataTypes.h>
#include <mesa_pd/data/Flags.h>
#include <mesa_pd/data/IAccessor.h>
27
#include <mesa_pd/data/shape/Ellipsoid.h>
28
29
30
31
32
33
34
35
36
#include <mesa_pd/data/shape/HalfSpace.h>
#include <mesa_pd/data/shape/Sphere.h>
#include <mesa_pd/kernel/SingleCast.h>

namespace walberla {
namespace mesa_pd {

/*
 * ray - particle intersection ratio functionality
37
38
 * can either be formulated in world frame coordinates (then the rotation of the geometry is not taken into account)
 * or in body frame coordinates (BF) which requires the point to be first transformed
39
40
 */

41
42
real_t raySphereIntersectionRatio( const Vec3& rayOrigin, const Vec3& rayDirection,
                                   const Vec3& spherePosition, const real_t sphereRadius )
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
{
   WALBERLA_ASSERT( !isPointInsideSphere(rayOrigin, spherePosition, sphereRadius ), "rayOrigin: " << rayOrigin );
   WALBERLA_ASSERT(  isPointInsideSphere(rayOrigin + rayDirection, spherePosition, sphereRadius ), "rayOrigin + rayDirection: " << rayOrigin + rayDirection );

   // get the physical sphere center

   real_t dirLength = rayDirection.length();
   Vector3< real_t > l = spherePosition - rayOrigin;
   real_t s = l * rayDirection / dirLength;
   real_t lsqr = l.sqrLength();
   real_t msqr = lsqr - s * s;
   real_t rsqr = real_c( sphereRadius * sphereRadius );
   real_t q = std::sqrt( rsqr - msqr );
   real_t delta = ( lsqr > rsqr ) ? s - q : s + q;
   delta /= dirLength;

59
60
   WALBERLA_ASSERT_GREATER_EQUAL( delta, 0_r );
   WALBERLA_ASSERT_LESS_EQUAL( delta, 1_r );
61
62
63
64

   return delta;
}

65
66
real_t rayHalfSpaceIntersectionRatio( const Vec3& rayOrigin, const Vec3& rayDirection,
                                      const Vec3& halfSpacePosition, const Vec3& halfSpaceNormal)
67
{
68
69
   WALBERLA_ASSERT( !isPointInsideHalfSpace( rayOrigin, halfSpacePosition, halfSpaceNormal ), "rayOrigin: " << rayOrigin );
   WALBERLA_ASSERT(  isPointInsideHalfSpace( rayOrigin + rayDirection, halfSpacePosition, halfSpaceNormal ), "rayOrigin + rayDirection: " << rayOrigin + rayDirection );
70

71
   real_t denom = halfSpaceNormal * rayDirection;
72
73
74

   auto diff = halfSpacePosition - rayOrigin;

75
   WALBERLA_ASSERT_FLOAT_UNEQUAL(denom, 0_r);
76

77
   real_t delta = diff * halfSpaceNormal / denom;
78

79
80
   WALBERLA_ASSERT_GREATER_EQUAL( delta, 0_r );
   WALBERLA_ASSERT_LESS_EQUAL( delta, 1_r );
81
82
83
84

   return delta;
}

85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
real_t rayEllipsoidIntersectionRatioBF( const Vec3& rayOriginBF, const Vec3& rayDirectionBF,
                                        const Vec3& ellipsoidSemiAxes)
{
   WALBERLA_ASSERT( !isPointInsideEllipsoidBF( rayOriginBF, ellipsoidSemiAxes ), "rayOriginBF: " << rayOriginBF );
   WALBERLA_ASSERT(  isPointInsideEllipsoidBF( rayOriginBF + rayDirectionBF, ellipsoidSemiAxes ), "rayOriginBF + rayDirectionBF: " << rayOriginBF + rayDirectionBF );

   Matrix3<real_t> M = Matrix3<real_t>::makeDiagonalMatrix(1_r/ellipsoidSemiAxes[0], 1_r/ellipsoidSemiAxes[1], 1_r/ellipsoidSemiAxes[2]);

   Vec3 P_M = M*rayOriginBF;
   Vec3 d_M = M*rayDirectionBF;

   const real_t a = d_M*d_M;
   const real_t b = 2_r*P_M*d_M;
   const real_t c = P_M*P_M - 1_r;

   const real_t discriminant = b*b - 4_r*a*c;

   WALBERLA_ASSERT_GREATER_EQUAL(discriminant, 0_r, "No intersection possible!");
   WALBERLA_ASSERT_FLOAT_UNEQUAL(a, 0_r);

   const real_t root = std::sqrt(discriminant);
   real_t delta = (-b - root) / (2_r * a);

   WALBERLA_ASSERT_GREATER_EQUAL( delta, 0_r );
   WALBERLA_ASSERT_LESS_EQUAL( delta - 1_r, math::Limits<real_t>::accuracy(), delta );

   return std::min(std::max(delta, real_c(0)), real_c(1));

}
114
115
116

template <typename ParticleAccessor_T>
real_t intersectionRatioBisection( const size_t particleIdx, const ParticleAccessor_T& ac,
117
                                   const Vec3& rayOrigin, const Vec3& rayDirection,
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
                                   real_t epsilon)
{
   mesa_pd::kernel::SingleCast singleCast;
   ContainsPointFunctor containsPointFctr;

   WALBERLA_ASSERT( !singleCast(particleIdx, ac, containsPointFctr, ac, rayOrigin), "rayOrigin: " << rayOrigin );
   WALBERLA_ASSERT(  singleCast(particleIdx, ac, containsPointFctr, ac, rayOrigin + rayDirection), "rayOrigin + rayDirection: " << rayOrigin + rayDirection );

   const real_t sqEpsilon         = epsilon * epsilon;
   const real_t sqDirectionLength = rayDirection.sqrLength();

   real_t q( 0.5 );
   real_t qDelta( 0.5 );

   while( qDelta * qDelta * sqDirectionLength >= sqEpsilon )
   {
134
135
      qDelta *= 0.5_r;
      Vec3 p = rayOrigin + q * rayDirection;
136
137
138
139
140
141
142
143
144
145
      if( singleCast(particleIdx, ac, containsPointFctr, ac, p) )
      {
         q -= qDelta;
      }
      else
      {
         q += qDelta;
      }
   }

146
147
   WALBERLA_ASSERT_GREATER_EQUAL( q, 0_r );
   WALBERLA_ASSERT_LESS_EQUAL( q, 1_r );
148
149
150
151
152
153
154

   return q;
}

struct RayParticleIntersectionRatioFunctor
{
   template<typename ParticleAccessor_T, typename Shape_T>
155
   real_t operator()(const size_t particleIdx, const Shape_T& /*shape*/, const ParticleAccessor_T& ac, const Vec3& rayOrigin, const Vec3& rayDirection, real_t epsilon )
156
157
158
159
160
161
162
   {
      static_assert(std::is_base_of<mesa_pd::data::IAccessor, ParticleAccessor_T>::value, "Provide a valid accessor as template");

      return intersectionRatioBisection(particleIdx, ac, rayOrigin, rayDirection, epsilon );
   }

   template<typename ParticleAccessor_T>
163
   real_t operator()(const size_t particleIdx, const mesa_pd::data::Sphere& sphere, const ParticleAccessor_T& ac, const Vec3& rayOrigin, const Vec3& rayDirection, real_t /*epsilon*/ )
164
165
166
167
168
169
170
   {
      static_assert(std::is_base_of<mesa_pd::data::IAccessor, ParticleAccessor_T>::value, "Provide a valid accessor as template");

      return raySphereIntersectionRatio(rayOrigin, rayDirection, ac.getPosition(particleIdx), sphere.getRadius() );
   }

   template<typename ParticleAccessor_T>
171
   real_t operator()(const size_t particleIdx, const mesa_pd::data::HalfSpace& halfSpace, const ParticleAccessor_T& ac, const Vec3& rayOrigin, const Vec3& rayDirection, real_t /*epsilon*/ )
172
173
174
   {
      static_assert(std::is_base_of<mesa_pd::data::IAccessor, ParticleAccessor_T>::value, "Provide a valid accessor as template");

175
      return rayHalfSpaceIntersectionRatio(rayOrigin, rayDirection, ac.getPosition(particleIdx), halfSpace.getNormal() );
176
177
   }

178
179
180
181
182
183
184
185
   template<typename ParticleAccessor_T>
   real_t operator()(const size_t particleIdx, const mesa_pd::data::Ellipsoid& ellipsoid, const ParticleAccessor_T& ac, const Vec3& rayOrigin, const Vec3& rayDirection, real_t /*epsilon*/ )
   {
      static_assert(std::is_base_of<mesa_pd::data::IAccessor, ParticleAccessor_T>::value, "Provide a valid accessor as template");

      return rayEllipsoidIntersectionRatioBF(mesa_pd::transformPositionFromWFtoBF(particleIdx, ac, rayOrigin), mesa_pd::transformVectorFromWFtoBF(particleIdx, ac, rayDirection), ellipsoid.getSemiAxes() );
   }

186
187
188
189
};

} //namespace mesa_pd
} //namespace walberla