1 //============================================================================
2 //  Copyright (c) Kitware, Inc.
3 //  All rights reserved.
4 //  See LICENSE.md for details.
5 //
6 //  This software is distributed WITHOUT ANY WARRANTY; without even
7 //  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
8 //  PURPOSE.  See the above copyright notice for more information.
9 //============================================================================
10 #ifndef lcl_internal_Common_h
11 #define lcl_internal_Common_h
12 
13 #include <lcl/FieldAccessor.h>
14 #include <lcl/internal/Math.h>
15 
16 #include <cstdint>
17 #include <type_traits>
18 
19 #define LCL_STATIC_ASSERT_PCOORDS_IS_FLOAT_TYPE(PCoordType)                   \
20   static_assert(std::is_floating_point<ComponentType<PCoordType>>::value,      \
21                 "parametric coordinates should be of floating point type");
22 
23 namespace lcl
24 {
25 namespace internal
26 {
27 
28 ///=========================================================================
29 template<typename CoordType>
findParametricDistance(const CoordType & pvals,IdComponent numVals)30 LCL_EXEC inline ComponentType<CoordType> findParametricDistance(
31   const CoordType& pvals, IdComponent numVals) noexcept
32 {
33   using T = ComponentType<CoordType>;
34 
35   T pDistMax = 0.0f;
36   for (IdComponent i = 0; i < numVals; ++i)
37   {
38     ComponentType<CoordType> pDist = 0.0f;
39     if (component(pvals, i) < T(0))
40     {
41       pDist = -1.0f * component(pvals, i);
42     }
43     else if (component(pvals, i) > T(1))
44     {
45       pDist = component(pvals, i) - 1.0f;
46     }
47     if (pDist > pDistMax)
48     {
49       pDistMax = pDist;
50     }
51   }
52 
53   return pDistMax;
54 }
55 
56 ///=========================================================================
57 /// Forward declaration
58 #define FORWARD_DECLAR_PARAMETRIC_DERIVATIVE(tag)                                                  \
59 template <typename Values, typename CoordType, typename Result>                                    \
60 LCL_EXEC inline void parametricDerivative(                                                        \
61   lcl::tag, const Values& values, IdComponent comp, const CoordType&, Result&& result) noexcept
62 
63 FORWARD_DECLAR_PARAMETRIC_DERIVATIVE(Triangle);
64 FORWARD_DECLAR_PARAMETRIC_DERIVATIVE(Quad);
65 FORWARD_DECLAR_PARAMETRIC_DERIVATIVE(Tetra);
66 FORWARD_DECLAR_PARAMETRIC_DERIVATIVE(Hexahedron);
67 FORWARD_DECLAR_PARAMETRIC_DERIVATIVE(Wedge);
68 FORWARD_DECLAR_PARAMETRIC_DERIVATIVE(Pyramid);
69 
70 #undef FORWARD_DECLAR_PARAMETRIC_DERIVATIVE
71 
72 ///=========================================================================
73 template <typename T>
74 class Space2D
75 {
76 public:
Space2D(const Vector<T,3> & origin,const Vector<T,3> & p1,const Vector<T,3> & p2)77   explicit LCL_EXEC Space2D(const Vector<T, 3>& origin, const Vector<T, 3>& p1, const Vector<T, 3>& p2) noexcept
78   {
79     this->Origin = origin;
80     this->XAxis = p1 - origin;
81     auto normal = internal::cross(this->XAxis, p2 - origin);
82     this->YAxis = internal::cross(normal, this->XAxis);
83 
84     internal::normalize(this->XAxis);
85     internal::normalize(this->YAxis);
86   }
87 
to2DPoint(Vector<T,3> pt)88   LCL_EXEC Vector<T, 2> to2DPoint(Vector<T, 3> pt) const noexcept
89   {
90     pt -= this->Origin;
91     return Vector<T, 2>{ internal::dot(pt, this->XAxis), internal::dot(pt, this->YAxis) };
92   }
93 
to3DVec(const Vector<T,2> & vec)94   LCL_EXEC Vector<T, 3> to3DVec(const Vector<T, 2>& vec) const noexcept
95   {
96     return (this->XAxis * vec[0]) + (this->YAxis * vec[1]);
97   }
98 
99 private:
100   Vector<T, 3> Origin;
101   Vector<T, 3> XAxis, YAxis;
102 };
103 
104 template <typename CellTag, typename Points, typename PCoords, typename T>
jacobian2D(CellTag tag,const Points & points,const PCoords & pcoords,Matrix<T,2,2> & jacobian)105 LCL_EXEC inline void jacobian2D(
106   CellTag tag, const Points& points, const PCoords& pcoords, Matrix<T, 2, 2>& jacobian) noexcept
107 {
108   T pd[2];
109   parametricDerivative(tag, points, 0, pcoords, pd);
110   jacobian(0, 0) = pd[0];
111   jacobian(0, 1) = pd[1];
112   parametricDerivative(tag, points, 1, pcoords, pd);
113   jacobian(1, 0) = pd[0];
114   jacobian(1, 1) = pd[1];
115 }
116 
117 template <typename CellTag, typename Points, typename Values, typename CoordType, typename Result>
derivative2D(CellTag tag,const Points & points,const Values & values,const CoordType & pcoords,Result && dx,Result && dy,Result && dz)118 LCL_EXEC inline lcl::ErrorCode derivative2D(
119   CellTag tag,
120   const Points& points,
121   const Values& values,
122   const CoordType& pcoords,
123   Result&& dx,
124   Result&& dy,
125   Result&& dz) noexcept
126 {
127   LCL_STATIC_ASSERT_PCOORDS_IS_FLOAT_TYPE(CoordType);
128 
129   using ProcessingType = internal::ClosestFloatType<typename Values::ValueType>;
130   using ResultCompType = ComponentType<Result>;
131 
132   constexpr IdComponent numPoints = CellTag{}.numberOfPoints();
133 
134   Vector<ProcessingType, 3> pts[numPoints];
135   for (int i = 0; i < numPoints; ++i)
136   {
137     points.getTuple(i, pts[i]);
138   }
139 
140   // 2-D coordinate system on the cell's plane
141   Space2D<ProcessingType> planeSpace(pts[0], pts[1], pts[numPoints - 1]);
142   Vector<ProcessingType, 2> pts2d[numPoints];
143   for (int i = 0; i < numPoints; ++i)
144   {
145     pts2d[i] = planeSpace.to2DPoint(pts[i]);
146   }
147 
148   Matrix<ProcessingType, 2, 2> jacobian;
149   jacobian2D(tag, makeFieldAccessorNestedSOA(pts2d, 2), pcoords, jacobian);
150   Matrix<ProcessingType, 2, 2> invJacobian;
151   LCL_RETURN_ON_ERROR(matrixInverse(jacobian, invJacobian))
152 
153   for (IdComponent c = 0; c < values.getNumberOfComponents(); ++c)
154   {
155     Vector<ProcessingType, 2> dvdp;
156     parametricDerivative(tag, values, c, pcoords, dvdp);
157     auto d2D = matrixMultiply(dvdp, invJacobian);
158     auto d3D = planeSpace.to3DVec(d2D);
159 
160     component(dx, c) = static_cast<ResultCompType>(d3D[0]);
161     component(dy, c) = static_cast<ResultCompType>(d3D[1]);
162     component(dz, c) = static_cast<ResultCompType>(d3D[2]);
163   }
164 
165   return ErrorCode::SUCCESS;
166 }
167 
168 template <typename CellTag, typename Points, typename WCoordType, typename PCoordType>
worldToParametric2D(CellTag tag,const Points & points,const WCoordType & wcoords,PCoordType && pcoords)169 LCL_EXEC inline lcl::ErrorCode worldToParametric2D(
170   CellTag tag,
171   const Points& points,
172   const WCoordType& wcoords,
173   PCoordType&& pcoords) noexcept
174 {
175   LCL_STATIC_ASSERT_PCOORDS_IS_FLOAT_TYPE(PCoordType);
176 
177   using TIn = typename Points::ValueType;
178   using TOut = ComponentType<PCoordType>;
179 
180   constexpr IdComponent numPoints = CellTag{}.numberOfPoints();
181 
182   Vector<TIn, 3> pts[numPoints];
183   for (int i = 0; i < numPoints; ++i)
184   {
185     points.getTuple(i, pts[i]);
186   }
187 
188   // 2-D coordinate system on the cell's plane
189   Space2D<TIn> planeSpace(pts[0], pts[1], pts[numPoints - 1]);
190   Vector<TIn, 2> pts2d[numPoints];
191   for (int i = 0; i < numPoints; ++i)
192   {
193     pts2d[i] = planeSpace.to2DPoint(pts[i]);
194   }
195 
196   auto jacobianEvaluator = [&pts2d](const Vector<TOut, 2>& pc, Matrix<TIn, 2, 2>& jacobian) {
197     jacobian2D(CellTag{}, makeFieldAccessorNestedSOA(pts2d, 2), pc, jacobian);
198     return ErrorCode::SUCCESS;
199   };
200 
201   auto functionEvaluator = [&points, &planeSpace](const Vector<TOut, 2>& pc, Vector<TIn, 2>& wc) {
202     Vector<TIn, 3> wc3(0);
203     LCL_RETURN_ON_ERROR(parametricToWorld(CellTag{}, points, pc, wc3))
204     wc = planeSpace.to2DPoint(wc3);
205 
206     return ErrorCode::SUCCESS;
207   };
208 
209   Vector<TIn, 3> wcVec{component(wcoords, 0), component(wcoords, 1), component(wcoords, 2)};
210   Vector<TOut, 2> pcVec;
211   LCL_RETURN_ON_ERROR(parametricCenter(tag, pcVec))
212   auto status = newtonsMethod(
213     jacobianEvaluator, functionEvaluator, planeSpace.to2DPoint(wcVec), pcVec);
214 
215   if (status == ErrorCode::SUCCESS || status == ErrorCode::SOLUTION_DID_NOT_CONVERGE)
216   {
217     component(pcoords, 0) = pcVec[0];
218     component(pcoords, 1) = pcVec[1];
219   }
220 
221   return status;
222 }
223 
224 ///=========================================================================
225 template <typename CellTag, typename Points, typename PCoords, typename T>
jacobian3D(CellTag tag,const Points & points,const PCoords & pcoords,Matrix<T,3,3> & jacobian)226 LCL_EXEC inline void jacobian3D(
227   CellTag tag, const Points& points, const PCoords& pcoords, Matrix<T, 3, 3>& jacobian) noexcept
228 {
229   T pd[3];
230   parametricDerivative(tag, points, 0, pcoords, pd);
231   jacobian(0, 0) = pd[0];
232   jacobian(0, 1) = pd[1];
233   jacobian(0, 2) = pd[2];
234   parametricDerivative(tag, points, 1, pcoords, pd);
235   jacobian(1, 0) = pd[0];
236   jacobian(1, 1) = pd[1];
237   jacobian(1, 2) = pd[2];
238   parametricDerivative(tag, points, 2, pcoords, pd);
239   jacobian(2, 0) = pd[0];
240   jacobian(2, 1) = pd[1];
241   jacobian(2, 2) = pd[2];
242 }
243 
244 template <typename CellTag, typename Points, typename Values, typename CoordType, typename Result>
derivative3D(CellTag tag,const Points & points,const Values & values,const CoordType & pcoords,Result && dx,Result && dy,Result && dz)245 LCL_EXEC inline lcl::ErrorCode derivative3D(
246   CellTag tag,
247   const Points& points,
248   const Values& values,
249   const CoordType& pcoords,
250   Result&& dx,
251   Result&& dy,
252   Result&& dz) noexcept
253 {
254   LCL_STATIC_ASSERT_PCOORDS_IS_FLOAT_TYPE(CoordType);
255 
256   using ProcessingType = internal::ClosestFloatType<typename Values::ValueType>;
257   using ResultCompType = ComponentType<Result>;
258 
259   Matrix<ProcessingType, 3, 3> jacobian;
260   jacobian3D(tag, points, pcoords, jacobian);
261   Matrix<ProcessingType, 3, 3> invJacobian;
262   LCL_RETURN_ON_ERROR(matrixInverse(jacobian, invJacobian))
263 
264   for (IdComponent c = 0; c < values.getNumberOfComponents(); ++c)
265   {
266     Vector<ProcessingType, 3> dvdp;
267     parametricDerivative(tag, values, c, pcoords, dvdp);
268     auto deriv = matrixMultiply(dvdp, invJacobian);
269     component(dx, c) = static_cast<ResultCompType>(deriv[0]);
270     component(dy, c) = static_cast<ResultCompType>(deriv[1]);
271     component(dz, c) = static_cast<ResultCompType>(deriv[2]);
272   }
273 
274   return ErrorCode::SUCCESS;
275 }
276 
277 template <typename CellTag, typename Points, typename WCoordType, typename PCoordType>
worldToParametric3D(CellTag tag,const Points & points,const WCoordType & wcoords,PCoordType && pcoords)278 LCL_EXEC inline lcl::ErrorCode worldToParametric3D(
279   CellTag tag,
280   const Points& points,
281   const WCoordType& wcoords,
282   PCoordType&& pcoords) noexcept
283 {
284   LCL_STATIC_ASSERT_PCOORDS_IS_FLOAT_TYPE(PCoordType);
285 
286   using TIn = typename Points::ValueType;
287   using TOut = ComponentType<PCoordType>;
288 
289   auto jacobianEvaluator = [tag, &points](const Vector<TOut, 3>& pc, Matrix<TIn, 3, 3>& jacobian) {
290     jacobian3D(tag, points, pc, jacobian);
291     return ErrorCode::SUCCESS;
292   };
293 
294   auto functionEvaluator = [tag, &points](const Vector<TOut, 3>& pc, Vector<TIn, 3>& wc) {
295     return parametricToWorld(tag, points, pc, wc);
296   };
297 
298   internal::Vector<TIn, 3> wcVec{component(wcoords, 0), component(wcoords, 1), component(wcoords, 2)};
299   internal::Vector<TOut, 3> pcVec;
300   LCL_RETURN_ON_ERROR(parametricCenter(tag, pcVec))
301   auto status = newtonsMethod(jacobianEvaluator, functionEvaluator, wcVec, pcVec);
302 
303   if (status == ErrorCode::SUCCESS || status == ErrorCode::SOLUTION_DID_NOT_CONVERGE)
304   {
305     component(pcoords, 0) = pcVec[0];
306     component(pcoords, 1) = pcVec[1];
307     component(pcoords, 2) = pcVec[2];
308   }
309 
310   return status;
311 }
312 
313 }
314 } // lcl::internal
315 
316 #endif //lcl_internal_Common_h
317