1#include "TetrahedronSetToPointSet.h"
3#include "Matrix/MatrixFunc.h"
4#include "Collision/NeighborPointQuery.h"
6template <typename Real>
7DYN_FUNC inline Real PP_Weight(const Real r, const Real h)
10 if (q > 1.0f) return 0.0f;
12 return (1.0f - q * q);
18 template<typename TDataType>
19 TetrahedronSetToPointSet<TDataType>::TetrahedronSetToPointSet()
25 template<typename TDataType>
26 TetrahedronSetToPointSet<TDataType>::TetrahedronSetToPointSet(std::shared_ptr<TetrahedronSet<TDataType>> from, std::shared_ptr<PointSet<TDataType>> to)
32 template<typename TDataType>
33 TetrahedronSetToPointSet<TDataType>::~TetrahedronSetToPointSet()
39 template<typename TDataType>
40 bool TetrahedronSetToPointSet<TDataType>::initializeImpl()
46 //TODO: fix the problem
47 template <typename Real, typename Coord>
48 __global__ void K_ApplyTransform(
49 DArray<Coord> to, //new position of surface mesh
50 DArray<Coord> from,//inner particle's new position
51 DArray<Coord> initTo, //initial
52 DArray<Coord> initFrom,
53 DArrayList<int> neighbors,
54 Real smoothingLength) //radius
56 int pId = threadIdx.x + (blockIdx.x * blockDim.x);
57 if (pId >= to.size()) return;
62 Coord initTo_i = initTo[pId];
63 Coord accDisplacement_i = Coord(0);
65 List<int>& list_i = neighbors[pId];
66 int nbSize = list_i.size();
68 Real total_weight1 = 0.0f;
69 Mat3f mat_i = Mat3f(0);
71 Real total_weight2 = 0.0f;
72 Mat3f deform_i = Mat3f(0.0f);
74 for (int ne = 0; ne < nbSize; ne++)
79 Real r1 = (initTo_i - initFrom[j]).norm();//j->to
82 Real r2 = (initFrom[j] - initTo_i).norm();//to->j
88 Real weight1 = PP_Weight(r1, smoothingLength);
89 Coord q = (initFrom[j] - initTo_i) / smoothingLength * sqrt(weight1);
91 mat_i(0, 0) += q[0] * q[0]; mat_i(0, 1) += q[0] * q[1]; mat_i(0, 2) += q[0] * q[2];
92 mat_i(1, 0) += q[1] * q[0]; mat_i(1, 1) += q[1] * q[1]; mat_i(1, 2) += q[1] * q[2];
93 mat_i(2, 0) += q[2] * q[0]; mat_i(2, 1) += q[2] * q[1]; mat_i(2, 2) += q[2] * q[2];
95 total_weight1 += weight1;
100 Real weight2 = PP_Weight(r2, smoothingLength);
102 Coord p = (from[j] - to[pId]) / smoothingLength;
103 Coord q2 = (initFrom[j] - initTo_i) / smoothingLength * weight2;
105 deform_i(0, 0) += p[0] * q2[0]; deform_i(0, 1) += p[0] * q2[1]; deform_i(0, 2) += p[0] * q2[2];
106 deform_i(1, 0) += p[1] * q2[0]; deform_i(1, 1) += p[1] * q2[1]; deform_i(1, 2) += p[1] * q2[2];
107 deform_i(2, 0) += p[2] * q2[0]; deform_i(2, 1) += p[2] * q2[1]; deform_i(2, 2) += p[2] * q2[2];
108 total_weight2 += weight2;
114 if (total_weight1 > EPSILON)
116 mat_i *= (1.0f / total_weight1);
119 Mat3f R(0), U(0), D(0), V(0);
120 polarDecomposition(mat_i, R, U, D, V);
122 Real threshold = 0.0001f*smoothingLength;
123 D(0, 0) = D(0, 0) > threshold ? 1.0 / D(0, 0) : 1.0;
124 D(1, 1) = D(1, 1) > threshold ? 1.0 / D(1, 1) : 1.0;
125 D(2, 2) = D(2, 2) > threshold ? 1.0 / D(2, 2) : 1.0;
127 mat_i = V * D * U.transpose(); //inverse
131 if (total_weight2 > EPSILON)
133 deform_i *= (1.0f / total_weight2);
134 deform_i = deform_i * mat_i;//deformation gradient
138 total_weight2 = 1.0f;
142 for (int ne = 0; ne < nbSize; ne++)
145 Real r = (initFrom[j] - initTo[pId]).norm();
147 if (r > 0.01f * smoothingLength)
149 Real weight = PP_Weight(r, smoothingLength);
151 Coord deformed_ij = deform_i * (initTo[pId] - initFrom[j]);//F*u(ji)
152 deformed_ij = deformed_ij.norm() > EPSILON ? deformed_ij.normalize() : Coord(0, 0, 0);
154 totalWeight += weight;
155 accDisplacement_i += weight * (from[j] + r * deformed_ij);
158 accDisplacement_i = totalWeight > EPSILON ? (accDisplacement_i / totalWeight) : accDisplacement_i;
159 to[pId] = accDisplacement_i;
162 template<typename TDataType>
163 bool TetrahedronSetToPointSet<TDataType>::apply()
165 uint pDim = cudaGridSize(m_to->getPoints().size(), BLOCK_SIZE);
167 K_ApplyTransform << <pDim, BLOCK_SIZE >> > (
170 m_initTo->getPoints(),
171 m_initFrom->getPoints(),
178 template<typename TDataType>
179 void TetrahedronSetToPointSet<TDataType>::match(std::shared_ptr<TetrahedronSet<TDataType>> from, std::shared_ptr<PointSet<TDataType>> to)
181 m_initFrom = std::make_shared<TetrahedronSet<TDataType>>();
182 m_initTo = std::make_shared<PointSet<TDataType>>();
184 m_initFrom->copyFrom(*from);
185 m_initTo->copyFrom(*to);
187 auto nbQuery = std::make_shared<NeighborPointQuery<TDataType>>();
189 nbQuery->inRadius()->setValue(m_radius);
190 nbQuery->inPosition()->allocate()->assign(m_initFrom->getPoints());
191 nbQuery->inOther()->allocate()->assign(m_initTo->getPoints());
195 mNeighborIds.assign(nbQuery->outNeighborIds()->getData());
198 DEFINE_CLASS(TetrahedronSetToPointSet);