70{
71
72 std::vector<std::pair<size_t, T>> ret_matches;
73
74 pSys.getContactDetection()->getMatches(pInt, ret_matches);
75
76 PARTICLETYPE<T>* p2 = NULL;
77
78
79 for (const auto& it : ret_matches) {
80
82
83 p2 = &pSys[it.first];
84
85
86 T delta = (p2->getRad() + p->getRad()) -
util::sqrt(it.second);
87
88
89 T
M = p->getMass() * p2->getMass() / (p->getMass() + p2->getMass());
90
91 T R = p->getRad() * p2->getRad() / (p->getRad() + p2->getRad());
92
93 std::vector < T > _velR(3, T());
94 _velR[0] = -(p2->getVel()[0] - p->getVel()[0]);
95 _velR[1] = -(p2->getVel()[1] - p->getVel()[1]);
96 _velR[2] = -(p2->getVel()[2] - p->getVel()[2]);
97
98 std::vector < T > _d(3, T());
99 std::vector < T > _normal(3, T());
100
101
102 _d[0] = p2->getPos()[0] - p->getPos()[0];
103 _d[1] = p2->getPos()[1] - p->getPos()[1];
104 _d[2] = p2->getPos()[2] - p->getPos()[2];
105
108 }
109 else {
110 return;
111 }
112
113 Vector<T, 3> d_(_d);
114 Vector<T, 3> velR_(_velR);
115 T dot = velR_[0] * _normal[0] + velR_[1] * _normal[1] + velR_[2] * _normal[2];
116
117
118
119 std::vector < T > _velN(3, T());
120 _velN[0] = dot * _normal[0];
121 _velN[1] = dot * _normal[1];
122 _velN[2] = dot * _normal[2];
123
124
125
126 std::vector < T > _velT(3, T());
127 _velT[0] = _velR[0] - _velN[0];
128 _velT[1] = _velR[1] - _velN[1];
129 _velT[2] = _velR[2] - _velN[2];
130
131 if (delta > 0.) {
132
133
134
135
136
138 if (_validationKruggelEmden) {
139 kn = 7.35e9;
140 }
141
142
143
144 std::vector < T > Fs_n(3, T());
145 Fs_n[0] = -kn *
util::pow(delta, 1.5) * _normal[0];
146 Fs_n[1] = -kn *
util::pow(delta, 1.5) * _normal[1];
147 Fs_n[2] = -kn *
util::pow(delta, 1.5) * _normal[2];
148
149
150
151
152
153
155 if (_validationKruggelEmden) {
156 eta_n = 1.96e5;
157 }
158
159 std::vector < T > Fd_n(3, T());
160 Fd_n[0] = -eta_n * _velN[0] *
util::sqrt(delta);
161 Fd_n[1] = -eta_n * _velN[1] *
util::sqrt(delta);
162 Fd_n[2] = -eta_n * _velN[2] *
util::sqrt(delta);
163
164 std::vector < T > F_n(3, T());
165 F_n[0] = Fs_n[0] + Fd_n[0];
166 F_n[1] = Fs_n[1] + Fd_n[1];
167 F_n[2] = Fs_n[2] + Fd_n[2];
168
169
170
171
173
174
176
177
178 std::vector < T > F_t(3, T());
179 F_t[0] = -eta_t * _velT[0];
180 F_t[1] = -eta_t * _velT[1];
181 F_t[2] = -eta_t * _velT[2];
182
183
184
185 force[0] = _scale1 * F_n[0] + _scale2 * F_t[0];
186 force[1] = _scale1 * F_n[1] + _scale2 * F_t[1];
187 force[2] = _scale1 * F_n[2] + _scale2 * F_t[2];
188
189 p->getForce()[0] += force[0] * 0.5 ;
190 p->getForce()[1] += force[1] * 0.5 ;
191 p->getForce()[2] += force[2] * 0.5 ;
192 p2->getForce()[0] -= force[0] * 0.5 ;
193 p2->getForce()[1] -= force[1] * 0.5 ;
194 p2->getForce()[2] -= force[2] * 0.5 ;
195 }
196 }
197 }
198}
platform_constant Fraction M[Q][Q]
cpu::simd::Pack< T > sqrt(cpu::simd::Pack< T > value)
T norm(const std::vector< T > &a)
l2 norm of a vector of arbitrary length
Vector< T, D > normalize(const Vector< T, D > &a)
bool nearZero(const ADf< T, DIM > &a)