/
dampingForce.cpp
225 lines (199 loc) · 9.8 KB
/
dampingForce.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
#include "dampingForce.h"
#include <iostream>
dampingForce::dampingForce(elasticRod &m_rod, timeStepper &m_stepper,
double m_viscosity, std::vector<double> m_Ct, std::vector<double> m_Cp, std::vector<double> m_Cz)
{
rod = &m_rod;
stepper = &m_stepper;
viscosity = m_viscosity;
dt = rod->dt;
Ct = m_Ct;
Cp = m_Cp;
Cz = m_Cz;
Id3 << 1, 0, 0,
0, 1, 0,
0, 0, 1;
f.setZero(3);
ForceVec = VectorXd::Zero(rod->ndof); // added
}
dampingForce::~dampingForce()
{
;
}
void dampingForce::computeFd()
{
ForceVec = VectorXd::Zero(rod->ndof); // added
for (int i = 2; i < rod->nv; i++)
{
u = rod->getVelocity(i);
if (i > 2 && i < rod->nv - 1)
{
p1 = rod->getVertex(i - 1);
p2 = rod->getVertex(i + 1);
t = p2 - p1;
t = t / t.norm();
}
else if (i == 2)
{
p1 = rod->getVertex(i);
p2 = rod->getVertex(i + 1);
t = p2 - p1;
t = t / t.norm();
}
else
{
p1 = rod->getVertex(i - 1);
p2 = rod->getVertex(i);
t = p2 - p1;
t = t / t.norm();
}
tmp_Ct = Ct[i - 2]; // tangential
tmp_Cp = Cp[i - 2]; // normal
tmp_Cz = Cz[i - 2]; // z-dir tan.crossnorm
parvec = (u.dot(t)) * t;
normvec = (u - (u.dot(t)) * t);
lastvec = normvec.cross(t);
if ((normvec.norm()) == 0)
{
if (i == 2)
{
f = -(tmp_Ct * t + tmp_Cp * normvec + tmp_Cz * lastvec) * u.norm() * viscosity * rod->voronoiLen(3) / 2;
}
else
{
f = -(tmp_Ct * t + tmp_Cp * normvec + tmp_Cz * lastvec) * u.norm() * viscosity * rod->voronoiLen(i);
}
}
else
{
if (i == 2)
{
f = -(tmp_Ct * t + tmp_Cp * normvec / normvec.norm() + tmp_Cz * lastvec / lastvec.norm()) * u.norm() * viscosity * rod->voronoiLen(3) / 2;
}
else
{
f = -(tmp_Ct * t + tmp_Cp * normvec / normvec.norm() + tmp_Cz * lastvec / lastvec.norm()) * u.norm() * viscosity * rod->voronoiLen(i);
}
}
// cout << f;
for (int k = 0; k < 3; k++)
{
ind = 4 * i + k;
ForceVec(ind) = ForceVec(ind) + f(k);
stepper->addForce(ind, -f[k]); // subtracting external force
}
}
}
void dampingForce::computeJd()
{
// Remember that dF/dx = 1/dt * dF/dv
for (int i = 2; i < rod->nv; i++)
{
u = rod->getVelocity(i);
ux = u(0);
uy = u(1);
uz = u(2);
if (i > 2 && i < rod->nv - 1)
{
p1 = rod->getVertex(i - 1);
xm1 = p1(0);
ym1 = p1(1);
zm1 = p1(2);
p2 = rod->getVertex(i + 1);
xp1 = p2(0);
yp1 = p2(1);
zp1 = p2(2);
t = p2 - p1;
t = t / t.norm();
dl = t.norm();
}
else if (i == 2)
{
p1 = rod->getVertex(i);
x = p1(0);
y = p1(1);
z = p1(2);
p2 = rod->getVertex(i + 1);
xp1 = p2(0);
yp1 = p2(1);
zp1 = p2(2);
t = p2 - p1;
t = t / t.norm();
dl = t.norm();
}
else
{
p1 = rod->getVertex(i - 1);
xm1 = p1(0);
ym1 = p1(1);
zm1 = p1(2);
p2 = rod->getVertex(i);
x = p2(0);
y = p2(1);
z = p2(2);
t = p2 - p1;
t = t / t.norm();
dl = t.norm();
}
parvec = (u.dot(t)) * t;
normvec = (u - (u.dot(t)) * t);
lastvec = normvec.cross(t);
tmp_Ct = Ct[i - 2]; // tangential
tmp_Cp = Cp[i - 2]; // normal
tmp_Cz = Cz[i - 2]; // z-dir tan.crossnorm
if (i == 2)
{
jac(0, 0) = tmp_Cp * (1 - (x - xp1) * (x - xp1) / dl * dl) + (tmp_Ct * (x - xp1) * (x - xp1)) / dl * dl + (tmp_Cz * (x - xp1) * (uz * (-y + yp1) + uy * (z - zp1))) / dl * dl;
jac(1, 0) = -((tmp_Cp * (x - xp1) * (y - yp1)) / dl * dl) + (tmp_Ct * (x - xp1) * (y - yp1)) / dl * dl + (tmp_Cz * (ux * (x - xp1) + uy * (y - yp1) + uz * (z - zp1)) * (-z + zp1)) / dl * dl + (tmp_Cz * (x - xp1) * (uz * (x - xp1) + ux * (-z + zp1))) / dl * dl;
jac(2, 0) = (tmp_Cz * (x - xp1) * (uy * (-x + xp1) + ux * (y - yp1))) / dl * dl + (tmp_Cz * (y - yp1) * (ux * (x - xp1) + uy * (y - yp1) + uz * (z - zp1))) / dl * dl - (tmp_Cp * (x - xp1) * (z - zp1)) / dl * dl + (tmp_Ct * (x - xp1) * (z - zp1)) / dl * dl;
jac(0, 1) = -((tmp_Cp * (x - xp1) * (y - yp1)) / dl * dl) + (tmp_Ct * (x - xp1) * (y - yp1)) / dl * dl + (tmp_Cz * (y - yp1) * (uz * (-y + yp1) + uy * (z - zp1))) / dl * dl + (tmp_Cz * (ux * (x - xp1) + uy * (y - yp1) + uz * (z - zp1)) * (z - zp1)) / dl * dl;
jac(1, 1) = tmp_Cp * (1 - (y - yp1) * (y - yp1) / dl * dl) + (tmp_Ct * (y - yp1) * (y - yp1)) / dl * dl + (tmp_Cz * (y - yp1) * (uz * (x - xp1) + ux * (-z + zp1))) / dl * dl;
jac(2, 1) = (tmp_Cz * (uy * (-x + xp1) + ux * (y - yp1)) * (y - yp1)) / dl * dl + (tmp_Cz * (-x + xp1) * (ux * (x - xp1) + uy * (y - yp1) + uz * (z - zp1))) / dl * dl - (tmp_Cp * (y - yp1) * (z - zp1)) / dl * dl + (tmp_Ct * (y - yp1) * (z - zp1)) / dl * dl;
jac(0, 2) = (tmp_Cz * (-y + yp1) * (ux * (x - xp1) + uy * (y - yp1) + uz * (z - zp1))) / dl * dl - (tmp_Cp * (x - xp1) * (z - zp1)) / dl * dl + (tmp_Ct * (x - xp1) * (z - zp1)) / dl * dl + (tmp_Cz * (uz * (-y + yp1) + uy * (z - zp1)) * (z - zp1)) / dl * dl;
jac(1, 2) = (tmp_Cz * (x - xp1) * (ux * (x - xp1) + uy * (y - yp1) + uz * (z - zp1))) / dl * dl - (tmp_Cp * (y - yp1) * (z - zp1)) / dl * dl + (tmp_Ct * (y - yp1) * (z - zp1)) / dl * dl + (tmp_Cz * (z - zp1) * (uz * (x - xp1) + ux * (-z + zp1))) / dl * dl;
jac(2, 2) = tmp_Cp * (1 - (z - zp1) * (z - zp1) / dl * dl) + (tmp_Cz * (uy * (-x + xp1) + ux * (y - yp1)) * (z - zp1)) / dl * dl + (tmp_Ct * (z - zp1) * (z - zp1)) / dl * dl;
}
else if (i > 2 && i < rod->nv - 1)
{
jac(0, 0) = tmp_Cp * (1 - (xm1 - xp1) * (xm1 - xp1) / (4 * dl * dl)) + (tmp_Ct * (xm1 - xp1) * (xm1 - xp1)) / (4 * dl * dl) + (tmp_Cz * (xm1 - xp1) * (uz * (-ym1 + yp1) + uy * (zm1 - zp1))) / (4 * dl * dl);
jac(1, 0) = -1 / 4 * (tmp_Cp * (xm1 - xp1) * (ym1 - yp1)) / dl * dl + (tmp_Ct * (xm1 - xp1) * (ym1 - yp1)) / (4 * dl * dl) + (tmp_Cz * (ux * (xm1 - xp1) + uy * (ym1 - yp1) + uz * (zm1 - zp1)) * (-zm1 + zp1)) / (4 * dl * dl) + (tmp_Cz * (xm1 - xp1) * (uz * (xm1 - xp1) + ux * (-zm1 + zp1))) / (4 * dl * dl);
jac(2, 0) = (tmp_Cz * (xm1 - xp1) * (uy * (-xm1 + xp1) + ux * (ym1 - yp1))) / (4 * dl * dl) + (tmp_Cz * (ym1 - yp1) * (ux * (xm1 - xp1) + uy * (ym1 - yp1) + uz * (zm1 - zp1))) / (4 * dl * dl) - (tmp_Cp * (xm1 - xp1) * (zm1 - zp1)) / (4 * dl * dl) + (tmp_Ct * (xm1 - xp1) * (zm1 - zp1)) / (4 * dl * dl);
jac(0, 1) = -1 / 4 * (tmp_Cp * (xm1 - xp1) * (ym1 - yp1)) / dl * dl + (tmp_Ct * (xm1 - xp1) * (ym1 - yp1)) / (4 * dl * dl) + (tmp_Cz * (ym1 - yp1) * (uz * (-ym1 + yp1) + uy * (zm1 - zp1))) / (4 * dl * dl) + (tmp_Cz * (ux * (xm1 - xp1) + uy * (ym1 - yp1) + uz * (zm1 - zp1)) * (zm1 - zp1)) / (4 * dl * dl);
jac(1, 1) = tmp_Cp * (1 - (ym1 - yp1) * (ym1 - yp1) / (4 * dl * dl)) + (tmp_Ct * (ym1 - yp1) * (ym1 - yp1)) / (4 * dl * dl) + (tmp_Cz * (ym1 - yp1) * (uz * (xm1 - xp1) + ux * (-zm1 + zp1))) / (4 * dl * dl);
jac(2, 1) = (tmp_Cz * (uy * (-xm1 + xp1) + ux * (ym1 - yp1)) * (ym1 - yp1)) / (4 * dl * dl) + (tmp_Cz * (-xm1 + xp1) * (ux * (xm1 - xp1) + uy * (ym1 - yp1) + uz * (zm1 - zp1))) / (4 * dl * dl) - (tmp_Cp * (ym1 - yp1) * (zm1 - zp1)) / (4 * dl * dl) + (tmp_Ct * (ym1 - yp1) * (zm1 - zp1)) / (4 * dl * dl);
jac(0, 2) = (tmp_Cz * (-ym1 + yp1) * (ux * (xm1 - xp1) + uy * (ym1 - yp1) + uz * (zm1 - zp1))) / (4 * dl * dl) - (tmp_Cp * (xm1 - xp1) * (zm1 - zp1)) / (4 * dl * dl) + (tmp_Ct * (xm1 - xp1) * (zm1 - zp1)) / (4 * dl * dl) + (tmp_Cz * (uz * (-ym1 + yp1) + uy * (zm1 - zp1)) * (zm1 - zp1)) / (4 * dl * dl);
jac(1, 2) = (tmp_Cz * (xm1 - xp1) * (ux * (xm1 - xp1) + uy * (ym1 - yp1) + uz * (zm1 - zp1))) / (4 * dl * dl) - (tmp_Cp * (ym1 - yp1) * (zm1 - zp1)) / (4 * dl * dl) + (tmp_Ct * (ym1 - yp1) * (zm1 - zp1)) / (4 * dl * dl) + (tmp_Cz * (zm1 - zp1) * (uz * (xm1 - xp1) + ux * (-zm1 + zp1))) / (4 * dl * dl);
jac(2, 2) = tmp_Cp * (1 - (zm1 - zp1) * (zm1 - zp1) / (4 * dl * dl)) + (tmp_Cz * (uy * (-xm1 + xp1) + ux * (ym1 - yp1)) * (zm1 - zp1)) / (4 * dl * dl) + (tmp_Ct * (zm1 - zp1) * (zm1 - zp1)) / (4 * dl * dl);
}
else
{
jac(0, 0) = (tmp_Cp * (dl * dl - (x - xm1) * (x - xm1)) + tmp_Ct * (x - xm1) * (x - xm1) + tmp_Cz * (x - xm1) * (uz * (-y + ym1) + uy * (z - zm1))) / dl * dl;
jac(1, 0) = (-(tmp_Cp * (x - xm1) * (y - ym1)) + tmp_Ct * (x - xm1) * (y - ym1) + tmp_Cz * (ux * (x - xm1) + uy * (y - ym1) + uz * (z - zm1)) * (-z + zm1) + tmp_Cz * (x - xm1) * (uz * (x - xm1) + ux * (-z + zm1))) / dl * dl;
jac(2, 0) = (tmp_Cz * (x - xm1) * (uy * (-x + xm1) + ux * (y - ym1)) + tmp_Cz * (y - ym1) * (ux * (x - xm1) + uy * (y - ym1) + uz * (z - zm1)) - tmp_Cp * (x - xm1) * (z - zm1) + tmp_Ct * (x - xm1) * (z - zm1)) / dl * dl;
jac(0, 1) = (-(tmp_Cp * (x - xm1) * (y - ym1)) + tmp_Ct * (x - xm1) * (y - ym1) + tmp_Cz * (y - ym1) * (uz * (-y + ym1) + uy * (z - zm1)) + tmp_Cz * (ux * (x - xm1) + uy * (y - ym1) + uz * (z - zm1)) * (z - zm1)) / dl * dl;
jac(1, 1) = (tmp_Cp * (dl * dl - (y - ym1) * (y - ym1)) + tmp_Ct * (y - ym1) * (y - ym1) + tmp_Cz * (y - ym1) * (uz * (x - xm1) + ux * (-z + zm1))) / dl * dl;
jac(2, 1) = (tmp_Cz * (uy * (-x + xm1) + ux * (y - ym1)) * (y - ym1) + tmp_Cz * (-x + xm1) * (ux * (x - xm1) + uy * (y - ym1) + uz * (z - zm1)) - tmp_Cp * (y - ym1) * (z - zm1) + tmp_Ct * (y - ym1) * (z - zm1)) / dl * dl;
jac(0, 2) = (tmp_Cz * (-y + ym1) * (ux * (x - xm1) + uy * (y - ym1) + uz * (z - zm1)) - tmp_Cp * (x - xm1) * (z - zm1) + tmp_Ct * (x - xm1) * (z - zm1) + tmp_Cz * (uz * (-y + ym1) + uy * (z - zm1)) * (z - zm1)) / dl * dl;
jac(1, 2) = (tmp_Cz * (x - xm1) * (ux * (x - xm1) + uy * (y - ym1) + uz * (z - zm1)) - tmp_Cp * (y - ym1) * (z - zm1) + tmp_Ct * (y - ym1) * (z - zm1) + tmp_Cz * (z - zm1) * (uz * (x - xm1) + ux * (-z + zm1))) / dl * dl;
jac(2, 2) = (tmp_Cp * (dl * dl - (z - zm1) * (z - zm1)) + tmp_Cz * (uy * (-x + xm1) + ux * (y - ym1)) * (z - zm1) + tmp_Ct * (z - zm1) * (z - zm1)) / dl * dl;
}
// Explicit setup
// jac(0, 0) = 0;
// jac(1, 0) = 0;
// jac(2, 0) = 0;
// jac(0, 1) = 0;
// jac(1, 1) = 0;
// jac(0, 2) = 0;
// jac(1, 2) = 0;
// jac(2, 2) = 0;
for (int kx = 0; kx < 3; kx++)
{
indx = 4 * i + kx;
for (int ky = 0; ky < 3; ky++)
{
indy = 4 * i + ky;
stepper->addJacobian(indx, indy, +jac(kx, ky)); // subtracting external force
}
}
}
}