33 const double roundness)
35 R1 = radius_total - radius_small;
38 jac.resize(4, 4,
false);
43 CHKERRABORT(PETSC_COMM_WORLD,
ierr);
48 R1 = radius_total - radius_small;
51 jac.resize(4, 4,
false);
63 CHKERRABORT(PETSC_COMM_WORLD,
ierr);
66 template <
typename T1>
70 VectorDouble pp({my_point(0), my_point(1), my_point(2)});
72 if (this->useSuperQuadric)
82 template <
typename T1>
86 VectorDouble pp({my_point(0), my_point(1), my_point(2)});
94 template <
typename T1>
99 VectorDouble pp({my_point(0), my_point(1), my_point(2)});
107 const double &x = var(0);
108 const double &y = var(1);
109 const double &z = var(2);
110 return -pow(
rr, 2) + pow(-
R1 + sqrt(pow(x, 2) + pow(y, 2)), 2. /
tt) +
116 const double &x = var(0);
117 const double &y = var(1);
118 const double &z = var(2);
119 const double prod1 = sqrt(pow(x, 2) + pow(y, 2));
120 return -prod1 +
rr * z > 0;
125 const double &x = var(0);
126 const double &y = var(1);
127 const double &z = var(2);
128 const double &
lambda = var(3);
130 const double prod1 = sqrt(pow(x, 2) + pow(y, 2));
131 f(0) =
p(0) - x + (2 *
lambda * x * (-
R1 + prod1)) / prod1;
132 f(1) =
p(1) - y + (2 *
lambda * y * (-
R1 + prod1)) / prod1;
134 f(3) = -pow(
rr, 2) + pow(-
R1 + prod1, 2) + pow(z, 2);
141 const double &x = var(0);
142 const double &y = var(1);
143 const double &z = var(2);
144 const double &
lambda = var(3);
145 const double prod0 = pow(x, 2) + pow(y, 2);
146 const double prod1 = sqrt(prod0);
148 jac(0, 0) = -1 + (2 *
lambda * pow(x, 2)) / (prod0) -
149 (2 *
lambda * pow(x, 2) * (-
R1 + prod1)) / pow(prod0, 1.5) +
150 (2 *
lambda * (-
R1 + prod1)) / prod1;
152 (2 *
lambda * x * y) / (prod0) -
153 (2 *
lambda * x * y * (-
R1 + prod1)) / pow(prod0, 1.5);
154 jac(0, 2) =
jac(2, 0) = 0;
155 jac(0, 3) =
jac(3, 0) = (2 * x * (-
R1 + prod1)) / prod1;
156 jac(1, 1) = -1 + (2 *
lambda * pow(y, 2)) / (prod0) -
157 (2 *
lambda * pow(y, 2) * (-
R1 + prod1)) / pow(prod0, 1.5) +
158 (2 *
lambda * (-
R1 + prod1)) / prod1;
159 jac(1, 2) =
jac(2, 1) = 0;
160 jac(1, 3) =
jac(3, 1) = (2 * y * (-
R1 + prod1)) / prod1;
162 jac(2, 3) =
jac(3, 2) = 2 * z;
168 MoFEMErrorCode
computeConeF(
const VectorDouble &
p,
const VectorDouble &var) {
170 const double &x = var(0);
171 const double &y = var(1);
172 const double &z = var(2);
173 const double &
lambda = var(3);
175 const double prod1 = sqrt(pow(x, 2) + pow(y, 2));
176 f(0) =
p(0) - x + (
lambda * x) / prod1;
177 f(1) =
p(1) - y + (
lambda * y) / prod1;
179 f(3) = prod1 -
rr * z;
186 const double &x = var(0);
187 const double &y = var(1);
188 const double &z = var(2);
189 const double &
lambda = var(3);
190 const double prod0 = pow(x, 2) + pow(y, 2);
193 -1 - (
lambda * pow(x, 2)) / pow(prod0, 1.5) +
lambda / sqrt(prod0);
194 jac(0, 1) =
jac(1, 0) = -((
lambda * x * y) / pow(prod0, 1.5));
195 jac(0, 2) =
jac(2, 0) = 0;
196 jac(0, 3) =
jac(3, 0) = x / sqrt(prod0);
198 -1 - (
lambda * pow(y, 2)) / pow(prod0, 1.5) +
lambda / sqrt(prod0);
199 jac(1, 2) =
jac(2, 1) = 0;
200 jac(1, 3) =
jac(3, 1) = y / sqrt(prod0);
209 const VectorDouble &var) {
211 const double &x = var(0);
212 const double &y = var(1);
213 const double &z = var(2);
214 const double &
lambda = var(3);
215 const double prod1 = sqrt(pow(x, 2) + pow(y, 2));
216 const double tt2 = 2. /
tt;
219 (2 *
lambda * x * (-
R1 + prod1) * pow(abs(-
R1 + prod1), -2 + tt2)) /
222 (2 *
lambda * y * (-
R1 + prod1) * pow(abs(-
R1 + prod1), -2 + tt2)) /
224 f(2) =
p(2) - z + (2 *
lambda * z * pow(abs(z), -2 + tt2)) /
tt;
225 f(3) = -pow(
rr, tt2) + pow(abs(-
R1 + prod1), tt2) + pow(abs(z), tt2);
232 const double &x = var(0);
233 const double &y = var(1);
235 const double &
lambda = var(3);
236 const double tt2 = 2. /
tt;
237 const double prod0 = pow(x, 2) + pow(y, 2);
238 const double prod1 = sqrt(prod0);
239 const double prod2 = abs(-
R1 + prod1);
240 const double prod3 = pow(-
R1 + prod1, 2);
241 const double prod4 = pow(prod2, -2 + tt2);
246 (2 *
lambda * (-2 + tt2) * pow(x, 2) * prod3 * pow(prod2, -4 + tt2)) /
248 (2 *
lambda * pow(x, 2) * prod4) / (
tt * (prod0)) -
249 (2 *
lambda * pow(x, 2) * (-
R1 + prod1) * prod4) /
250 (
tt * pow(prod0, 1.5)) +
251 (2 *
lambda * (-
R1 + prod1) * prod4) / (
tt * prod1);
253 (2 *
lambda * (-2 + tt2) * x * y * prod3 * pow(prod2, -4 + tt2)) /
255 (2 *
lambda * x * y * prod4) / (
tt * (prod0)) -
256 (2 *
lambda * x * y * (-
R1 + prod1) * prod4) / (
tt * pow(prod0, 1.5));
257 jac(0, 2) =
jac(2, 0) = 0;
258 jac(0, 3) =
jac(3, 0) = (2 * x * (-
R1 + prod1) * prod4) / (
tt * prod1);
261 (2 *
lambda * (-2 + tt2) * pow(y, 2) * prod3 * pow(prod2, -4 + tt2)) /
263 (2 *
lambda * pow(y, 2) * prod4) / (
tt * (prod0)) -
264 (2 *
lambda * pow(y, 2) * (-
R1 + prod1) * prod4) /
265 (
tt * pow(prod0, 1.5)) +
266 (2 *
lambda * (-
R1 + prod1) * prod4) / (
tt * prod1);
267 jac(1, 2) =
jac(2, 1) = 0;
268 jac(1, 3) =
jac(3, 1) = (2 * y * (-
R1 + prod1) * prod4) / (
tt * prod1);
271 (2 *
lambda * (-2 + tt2) * pow(z, 2) * pow(abs(z), -4 + tt2)) /
tt +
272 (2 *
lambda * pow(abs(z), -2 + tt2)) /
tt;
273 jac(2, 3) =
jac(3, 2) = (2 * z * pow(abs(z), -2 + tt2)) /
tt;
287 auto my_rand_mn = [](
double M,
double N) {
288 return M + (rand() / (RAND_MAX / (
N -
M)));
291 std::ofstream afile(
"torus_pts.csv", std::ios::ate);
292 if (afile.is_open()) {
294 VectorDouble
p({0, 0, 0});
296 for (
int n = 0;
n != 1e6;
n++) {
297 double x = my_rand_mn(-
R1 -
rr * 2,
R1 +
rr * 2);
298 double y = my_rand_mn(-
R1 -
rr * 2,
R1 +
rr * 2);
299 double z = my_rand_mn(-
R1 -
rr * 2,
R1 +
rr * 2);
300 VectorDouble coords({x, y, z});
307 afile << x <<
"," << y <<
"," << z <<
"\n";
317 auto my_rand_mn = [](
double M,
double N) {
318 return M + (rand() / (RAND_MAX / (
N -
M)));
321 std::ofstream afile0(
"torus_pts0.csv", std::ios::ate);
322 std::ofstream afile1(
"torus_pts1.csv", std::ios::ate);
323 if (afile0.is_open() && afile1.is_open()) {
327 for (
int n = 0;
n != 1e4;
n++) {
328 double x = my_rand_mn(-
R1 -
rr * 2,
R1 +
rr * 2);
329 double y = my_rand_mn(-
R1 -
rr * 2,
R1 +
rr * 2);
330 double z = my_rand_mn(-
R1 -
rr * 2,
R1 +
rr * 2);
331 VectorDouble coords({x, y, z});
339 afile0 << x <<
"," << y <<
"," << z <<
"\n";
340 afile1 << output(0) <<
"," << output(1) <<
"," << output(2) <<
"\n";
361 auto inverse = [](
double *
A,
int N) {
362 int *ipv =
new int[
N];
364 double *work =
new double[lwork];
373 inverse(
jac.data().data(), 4);
374 VectorDouble f_cp(4);
376 noalias(
f) = prod(
jac, f_cp);
379 SETERRQ1(PETSC_COMM_SELF, 1,
"error solve dgesv info = %d", info);
386 VectorDouble &output) {
388 VectorDouble var({
p(0),
p(1),
p(2)});
391 auto my_rand_mn = [](
double M,
double N) {
392 return M + (rand() / (RAND_MAX / (
N -
M)));
402 const double eps = 1e-8;
403 const int max_it = 35;
407 for (;
i != max_it; ++
i) {
411 noalias(x_k) = var -
f;
413 res_norm = norm_2(x_k - var);
414 if (res_norm <
eps) {
415 VectorDouble out({x_k(0), x_k(1), x_k(2)});
416 const double dist = norm_2(out -
p);
422 if (
i > 30 && res_norm >
eps)
423 cout <<
"WE HAVE A PROBLEM " << res_norm << endl;
428 VectorDouble &output) {
430 VectorDouble var({
p(0),
p(1),
p(2)});
437 const double eps = 1e-8;
438 const int max_it = 35;
441 for (;
i != max_it; ++
i) {
445 noalias(x_k) = var -
f;
447 res_norm = norm_2(x_k - var);
448 if (res_norm <
eps) {
449 VectorDouble out({x_k(0), x_k(1), x_k(2)});
450 const double dist = norm_2(out -
p);
456 if (
i > 30 && res_norm >
eps)
457 cout <<
"WE HAVE A PROBLEM " << res_norm << endl;
463 static PetscErrorCode
FormFunction(SNES snes, Vec xvec, Vec fvec,
void *ctx) {
465 const PetscScalar *xx;
467 array<PetscInt, 4> idx = {0, 1, 2, 3};
468 VecGetArrayRead(xvec, &xx);
471 VectorDouble &pt = my_ctx->
p;
472 VectorDouble var({xx[0], xx[1], xx[2], xx[3]});
474 VecRestoreArrayRead(xvec, &xx);
476 VecSetValues(fvec, 4, idx.data(), &*my_ctx->
f.begin(), INSERT_VALUES);
482 const PetscScalar *xx;
484 array<PetscInt, 4> idx = {0, 1, 2, 3};
486 VecGetArrayRead(x, &xx);
490 VectorDouble var({xx[0], xx[1], xx[2], xx[3]});
492 auto &my_jac = my_ctx->
jac;
493 VecRestoreArrayRead(x, &xx);
495 MatSetValues(
B, 4, idx.data(), 4, idx.data(), &*my_jac.data().begin(),
497 MatAssemblyBegin(
B, MAT_FINAL_ASSEMBLY);
498 MatAssemblyEnd(
B, MAT_FINAL_ASSEMBLY);
503 VectorDouble &output) {
512 VectorDouble init_guess;
515 auto my_rand_mn = [](
double M,
double N) {
516 return M + (rand() / (RAND_MAX / (
N -
M)));
520 xa[0] = init_guess(0);
521 xa[1] = init_guess(1);
522 xa[2] = init_guess(2);
523 xa[3] = init_guess(3);
528 SNESConvergedReason reason;
533 CHKERR PetscPrintf(PETSC_COMM_SELF,
534 "** WARNING Closest point iterations failed. Number "
535 "of Newton iterations = %D\n",
539 const PetscScalar *mx;
541 VecGetArrayRead(
vecX, &mx);
546 VecRestoreArrayRead(
vecX, &mx);
554 SNESCreate(PETSC_COMM_WORLD, &
sNES);
555 const PetscInt
n = 4;
559 MatCreateSeqDense(PETSC_COMM_SELF,
n,
n, &
jac(0, 0), &
J);
566 VecCreateSeq(PETSC_COMM_SELF,
n, &
vecR);
567 VecCreateSeq(PETSC_COMM_SELF,
n, &
vecX);
574 KSPSetType(
ksp, KSPPREONLY);
578 KSPSetTolerances(
ksp, 1e-8, PETSC_DEFAULT, PETSC_DEFAULT, 20);
581 SNESSetTolerances(
sNES, 1e-8, 1e-12, 0, 25, 1000);
583 SNESLineSearchSetType(
lineSearch, SNESLINESEARCHBT);
static PetscErrorCode ierr
#define MoFEMFunctionReturnHot(a)
Last executable line of each PETSc function used for error handling. Replaces return()
#define MoFEMFunctionBegin
First executable line of each MoFEM function, used for error handling. Final line of MoFEM functions ...
#define MoFEMFunctionReturn(a)
Last executable line of each PETSc function used for error handling. Replaces return()
#define CHKERR
Inline error check.
#define MoFEMFunctionBeginHot
First executable line of each MoFEM function, used for error handling. Final line of MoFEM functions ...
FTensor::Index< 'n', SPACE_DIM > n
FTensor::Index< 'i', SPACE_DIM > i
static __CLPK_integer lapack_dgetrf(__CLPK_integer m, __CLPK_integer n, __CLPK_doublereal *a, __CLPK_integer lda, __CLPK_integer *ipiv)
static __CLPK_integer lapack_dgetri(__CLPK_integer n, __CLPK_doublereal *a, __CLPK_integer lda, __CLPK_integer *ipiv, __CLPK_doublereal *work, __CLPK_integer lwork)
SNESLineSearch lineSearch
bool isPointInsideTorus(const T &var)
ArbitrarySurfaceData(const double radius_total, const double radius_small, const double roundness)
Tensor1< double, 3 > getClosestPointToSuperquadric(Tensor1< T1, 3 > &my_point)
static PetscErrorCode FormJacobian(SNES snes, Vec x, Mat M, Mat B, void *ctx)
MoFEMErrorCode solveTorusClosestPointSNES(const VectorDouble &pt, VectorDouble &output)
MoFEMErrorCode computeTorusJacobian(const VectorDouble &var)
MoFEMErrorCode PrintTorusDebug()
MoFEMErrorCode PrintTorusAnimDebug()
MoFEMErrorCode solveConeClosestPoint(const VectorDouble &p, VectorDouble &output)
MoFEMErrorCode destroySNES()
Tensor1< double, 3 > getClosestPointToToroid(Tensor1< T1, 3 > &my_point)
MoFEMErrorCode solveLinearSystem(MatrixDouble &jac, VectorDouble &f)
MoFEMErrorCode computeSuperquadricJacobian(const VectorDouble &var)
ArbitrarySurfaceData(const double radius_total, const double radius_small)
MoFEMErrorCode computeConeJacobian(const VectorDouble &var)
bool isPointInsideCone(const T &var)
static PetscErrorCode FormFunction(SNES snes, Vec xvec, Vec fvec, void *ctx)
MoFEMErrorCode computeTorusF(const VectorDouble &p, const VectorDouble &var)
MoFEMErrorCode solveTorusClosestPoint(const VectorDouble &p, VectorDouble &output)
ArbitrarySurfaceData()=delete
MoFEMErrorCode computeSuperquadricF(const VectorDouble &p, const VectorDouble &var)
MoFEMErrorCode createSNES()
MoFEMErrorCode computeConeF(const VectorDouble &p, const VectorDouble &var)
Tensor1< double, 3 > getClosestPointToCone(Tensor1< T1, 3 > &my_point)