1034 lines
41 KiB
C++
1034 lines
41 KiB
C++
/* ----------------------------------------------------------------------
|
|
This file is part of the LIGGGHTS DEM simulation engine, released by
|
|
DCS Computing Gmbh, Linz, Austria
|
|
http://www.dcs-computing.com, office@dcs-computing.com
|
|
|
|
LIGGGHTS® is part of CFDEM®project:
|
|
http://www.liggghts.com | http://www.cfdem.com
|
|
|
|
Core developer and main author:
|
|
Christoph Kloss, christoph.kloss@dcs-computing.com
|
|
|
|
LIGGGHTS® is open-source, distributed under the terms of the GNU Public
|
|
License, version 2 or later. It is distributed in the hope that it will
|
|
be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
|
|
of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. You should have
|
|
received a copy of the GNU General Public License along with LIGGGHTS®.
|
|
If not, see http://www.gnu.org/licenses . See also top-level README
|
|
and LICENSE files.
|
|
|
|
LIGGGHTS® and CFDEM® are registered trade marks of DCS Computing GmbH,
|
|
the producer of the LIGGGHTS® software and the CFDEM®coupling software
|
|
See http://www.cfdem.com/terms-trademark-policy for details.
|
|
|
|
-------------------------------------------------------------------------
|
|
Contributing author and copyright for this file:
|
|
(if not contributing author is listed, this file has been contributed
|
|
by the core developer)
|
|
|
|
Alexander Podlozhnyuk
|
|
Copyright 2015- DCS Computing GmbH, Linz
|
|
------------------------------------------------------------------------- */
|
|
|
|
#include "superquadric.h"
|
|
#include <algorithm>
|
|
|
|
#ifdef SUPERQUADRIC_ACTIVE_FLAG
|
|
#include "math_extra_liggghts_superquadric.h"
|
|
|
|
#define PHI_INV 0.61803398874989479 //: = 1/phi = 2.0/(1.0+sqrt(5.0))
|
|
|
|
Superquadric::Superquadric(const double *center_, const double *quat_, double *shape_, double *blockiness_)
|
|
{
|
|
set(center_, quat_, shape_, blockiness_);
|
|
}
|
|
|
|
//rotate from body fixed to global coordinate system
|
|
inline void Superquadric::rotate_local2global(const double *input_coord, double *result)
|
|
{
|
|
MathExtraLiggghtsNonspherical::matvec(rotation_matrix, input_coord, result);
|
|
}
|
|
|
|
//rotate from global coordinate to body fixed system
|
|
inline void Superquadric::rotate_global2local(const double *input_coord, double *result)
|
|
{
|
|
MathExtraLiggghtsNonspherical::transpose_matvec(rotation_matrix, input_coord, result);
|
|
}
|
|
|
|
//transform point in local coordinates to global coordinates
|
|
void Superquadric::local2global(const double *input_coord, double *result)
|
|
{
|
|
double x_temp[3];
|
|
rotate_local2global(input_coord, x_temp);
|
|
MathExtra::add3(x_temp, center, result); //result = x_temp + center
|
|
}
|
|
|
|
//transform point in global coordinates to local particle based coordinates
|
|
void Superquadric::global2local(const double *input_coord, double *result)
|
|
{
|
|
double x_temp[3];
|
|
MathExtra::sub3(input_coord, center, x_temp); //x_temp = x - center
|
|
rotate_global2local(x_temp, result); //rotate x_temp from global to local
|
|
}
|
|
|
|
void Superquadric::set(const double *center_, const double *quat_, double *shape_, double *blockiness_)
|
|
{
|
|
center = center_;
|
|
quat = quat_;
|
|
shape = shape_;
|
|
blockiness = blockiness_;
|
|
MathExtraLiggghtsNonspherical::quat_to_mat(quat, rotation_matrix);
|
|
calc_koef();
|
|
if(shape != NULL) {
|
|
shape_inv[0] = 1.0 / shape[0];
|
|
shape_inv[1] = 1.0 / shape[1];
|
|
shape_inv[2] = 1.0 / shape[2];
|
|
}
|
|
isEllipsoid = MathExtraLiggghts::compDouble(blockiness[0], 2.0, 1e-2) && MathExtraLiggghts::compDouble(blockiness[1], 2.0, 1e-2);
|
|
isCylinder = !MathExtraLiggghts::compDouble(blockiness[0], 2.0, 1e-2) && MathExtraLiggghts::compDouble(blockiness[1], 2.0, 1e-2);
|
|
|
|
if(isEllipsoid) {
|
|
useIntBlockiness = true;
|
|
} else {
|
|
useIntBlockiness = MathExtraLiggghtsNonspherical::isInteger(blockiness[0]) && MathExtraLiggghtsNonspherical::isInteger(blockiness[1]);
|
|
}
|
|
}
|
|
|
|
//value of the particle shape function at x in local reference frame
|
|
double Superquadric::shape_function_local(const double *input_coord)
|
|
{
|
|
const double n1 = blockiness[0];
|
|
const double n2 = blockiness[1];
|
|
double f;
|
|
if(useIntBlockiness) {
|
|
const int n1_int = floor(blockiness[0] + 1e-2);
|
|
const int n2_int = floor(blockiness[1] + 1e-2);
|
|
if(n1_int == n2_int) {
|
|
f = MathExtraLiggghtsNonspherical::pow_abs_int(input_coord[0]* shape_inv[0], n2_int) +
|
|
MathExtraLiggghtsNonspherical::pow_abs_int(input_coord[1]* shape_inv[1], n2_int) +
|
|
MathExtraLiggghtsNonspherical::pow_abs_int(input_coord[2]* shape_inv[2], n1_int) - 1.0;
|
|
} else {
|
|
f = (MathExtraLiggghtsNonspherical::pow_abs(
|
|
MathExtraLiggghtsNonspherical::pow_abs_int(input_coord[0]* shape_inv[0], n2_int) +
|
|
MathExtraLiggghtsNonspherical::pow_abs_int(input_coord[1]* shape_inv[1], n2_int), n1/n2) +
|
|
MathExtraLiggghtsNonspherical::pow_abs_int(input_coord[2]* shape_inv[2], n1_int) - 1.0);
|
|
}
|
|
} else {
|
|
f = (MathExtraLiggghtsNonspherical::pow_abs(
|
|
MathExtraLiggghtsNonspherical::pow_abs(input_coord[0]* shape_inv[0], n2) +
|
|
MathExtraLiggghtsNonspherical::pow_abs(input_coord[1]* shape_inv[1], n2), n1/n2) +
|
|
MathExtraLiggghtsNonspherical::pow_abs(input_coord[2]* shape_inv[2], n1) - 1.0);
|
|
}
|
|
return koef*f;
|
|
}
|
|
|
|
//gradient of the shape function, defined in canonical frame
|
|
void Superquadric::shape_function_gradient_local(const double *input_coord, double *result)
|
|
{
|
|
const double n1 = blockiness[0];
|
|
const double n2 = blockiness[1];
|
|
|
|
const double a = shape_inv[0];
|
|
const double b = shape_inv[1];
|
|
const double c = shape_inv[2];
|
|
|
|
//straightforward calculation of the shape function 1st derivatives
|
|
|
|
const double xa = input_coord[0] * a;
|
|
const double yb = input_coord[1] * b;
|
|
const double zc = input_coord[2] * c;
|
|
|
|
double xan21, ybn21, zcn11;
|
|
if(useIntBlockiness) {
|
|
const int n1_int = floor(n1 + 1e-2);
|
|
const int n2_int = floor(n2 + 1e-2);
|
|
xan21 = MathExtraLiggghtsNonspherical::pow_abs_int(xa, n2_int - 1);
|
|
ybn21 = MathExtraLiggghtsNonspherical::pow_abs_int(yb, n2_int - 1);
|
|
zcn11 = MathExtraLiggghtsNonspherical::pow_abs_int(zc, n1_int - 1);
|
|
} else {
|
|
xan21 = MathExtraLiggghtsNonspherical::pow_abs(xa, n2 - 1.0);
|
|
ybn21 = MathExtraLiggghtsNonspherical::pow_abs(yb, n2 - 1.0);
|
|
zcn11 = MathExtraLiggghtsNonspherical::pow_abs(zc, n1 - 1.0);
|
|
}
|
|
|
|
if(MathExtraLiggghts::compDouble(n1, n2, 1e-2)) {
|
|
result[0] = n1 * (koef * a) * MathExtraLiggghtsNonspherical::sign(xa) * xan21;
|
|
result[1] = n1 * (koef * b) * MathExtraLiggghtsNonspherical::sign(yb) * ybn21;
|
|
result[2] = n1 * (koef * c) * MathExtraLiggghtsNonspherical::sign(zc) * zcn11;
|
|
} else {
|
|
const double xy_term = xan21 * fabs(xa) + ybn21 * fabs(yb);
|
|
double xy_term_powed1 = MathExtraLiggghtsNonspherical::pow_abs(xy_term, n1 / n2 - 1.0);
|
|
result[0] = n1 * (koef * a) * MathExtraLiggghtsNonspherical::sign(xa) * xan21 * xy_term_powed1;
|
|
result[1] = n1 * (koef * b) * MathExtraLiggghtsNonspherical::sign(yb) * ybn21 * xy_term_powed1;
|
|
result[2] = n1 * (koef * c) * MathExtraLiggghtsNonspherical::sign(zc) * zcn11;
|
|
}
|
|
}
|
|
|
|
//hessian (matrix of the 2nd derivatives) of the shape function in local frame
|
|
void Superquadric::shape_function_hessian_local(const double *input_coord, double *result)
|
|
{
|
|
const double n1 = blockiness[0];
|
|
const double n2 = blockiness[1];
|
|
|
|
const double a = shape_inv[0];
|
|
const double b = shape_inv[1];
|
|
const double c = shape_inv[2];
|
|
|
|
const double xa = input_coord[0] * a;
|
|
const double yb = input_coord[1] * b;
|
|
const double zc = input_coord[2] * c;
|
|
|
|
//straightforward calculation of the 2nd derivatives
|
|
double xan22, ybn22, zcn12;
|
|
if(useIntBlockiness) {
|
|
const int n1_int = floor(n1 + 1e-2);
|
|
const int n2_int = floor(n2 + 1e-2);
|
|
xan22 = MathExtraLiggghtsNonspherical::pow_abs_int(xa, n2_int - 2);
|
|
ybn22 = MathExtraLiggghtsNonspherical::pow_abs_int(yb, n2_int - 2);
|
|
zcn12 = MathExtraLiggghtsNonspherical::pow_abs_int(zc, n1_int - 2);
|
|
} else {
|
|
xan22 = MathExtraLiggghtsNonspherical::pow_abs(xa, n2 - 2.0);
|
|
ybn22 = MathExtraLiggghtsNonspherical::pow_abs(yb, n2 - 2.0);
|
|
zcn12 = MathExtraLiggghtsNonspherical::pow_abs(zc, n1 - 2.0);
|
|
}
|
|
const double xan21 = xan22 * fabs(xa); // = MathExtraLiggghtsNonspherical::pow_abs(xa, n2 - 1.0)
|
|
const double ybn21 = ybn22 * fabs(yb); // = MathExtraLiggghtsNonspherical::pow_abs(yb, n2 - 1.0)
|
|
|
|
if(MathExtraLiggghts::compDouble(n1, n2, 1e-2)) { //MathExtraLiggghtsNonspherical::pow_abs(xy_term, n1/n2 - 1.0);
|
|
result[0] = (koef * a) * (n1 * (n2 - 1.0) * xan22) * a;
|
|
result[4] = (koef * b) * (n1 * (n2 - 1.0) * ybn22) * b;
|
|
result[8] = (koef * c) * (n1 * (n1 - 1.0) * zcn12) * c;
|
|
result[1] = result[3] = result[2] = result[6] = result[5] = result[7] = 0.0;
|
|
} else {
|
|
const double xy_term = xan21 * fabs(xa) + ybn21 * fabs(yb); // MathExtraLiggghtsNonspherical::pow_abs(xa, n2) + MathExtraLiggghtsNonspherical::pow_abs(yb, n2)
|
|
double xy_term_powed2;
|
|
if(MathExtraLiggghtsNonspherical::isInteger(n1/n2)) {
|
|
int n1_over_n2_int = floor(n1/n2 + 1e-2);
|
|
xy_term_powed2 = MathExtraLiggghtsNonspherical::pow_abs_int(xy_term, n1_over_n2_int - 2.0);
|
|
} else
|
|
xy_term_powed2 = MathExtraLiggghtsNonspherical::pow_abs(xy_term, n1/n2 - 2.0);
|
|
const double xy_term_powed1 = xy_term_powed2 * xy_term;
|
|
|
|
result[0] = (koef * a) * (n1 * (n2 - 1.0) * xan22 * xy_term_powed1 +
|
|
(n1 - n2) * n1 * xan21 * xan21 * xy_term_powed2) * a;
|
|
result[4] = (koef * b) * (n1 * (n2 - 1.0) * ybn22 * xy_term_powed1 +
|
|
(n1 - n2) * n1 * ybn21 * ybn21 * xy_term_powed2) * b;
|
|
result[1] = koef * (n1 - n2) * n1 * MathExtraLiggghtsNonspherical::sign(xa) * MathExtraLiggghtsNonspherical::sign(yb) * (xan21 * a) * (ybn21 * b) * xy_term_powed2;
|
|
|
|
result[3] = result[1];
|
|
result[8] = (koef * c) * (n1 * (n1 - 1.0) * zcn12) * c;
|
|
result[2] = result[6] = result[5] = result[7] = 0.0;
|
|
}
|
|
}
|
|
|
|
//compute shape function value, gradient and hessian matrix altogether in one function
|
|
void Superquadric::shape_function_props_local(const double *input_coord, double *f, double *grad, double *hess)
|
|
{
|
|
const double n1 = blockiness[0];
|
|
const double n2 = blockiness[1];
|
|
|
|
const double a = shape_inv[0];
|
|
const double b = shape_inv[1];
|
|
const double c = shape_inv[2];
|
|
|
|
const double xa = input_coord[0] * a;
|
|
const double yb = input_coord[1] * b;
|
|
const double zc = input_coord[2] * c;
|
|
|
|
double xan22, ybn22, zcn12;
|
|
if(useIntBlockiness) {
|
|
const int n1_int = floor(n1 + 1e-2);
|
|
const int n2_int = floor(n2 + 1e-2);
|
|
xan22 = MathExtraLiggghtsNonspherical::pow_abs_int(xa, n2_int - 2);
|
|
ybn22 = MathExtraLiggghtsNonspherical::pow_abs_int(yb, n2_int - 2);
|
|
zcn12 = MathExtraLiggghtsNonspherical::pow_abs_int(zc, n1_int - 2);
|
|
} else {
|
|
xan22 = MathExtraLiggghtsNonspherical::pow_abs(xa, n2 - 2.0);
|
|
ybn22 = MathExtraLiggghtsNonspherical::pow_abs(yb, n2 - 2.0);
|
|
zcn12 = MathExtraLiggghtsNonspherical::pow_abs(zc, n1 - 2.0);
|
|
}
|
|
const double xan21 = xan22 * fabs(xa); // = MathExtraLiggghtsNonspherical::pow_abs(xa, n2 - 1.0)
|
|
const double ybn21 = ybn22 * fabs(yb); // = MathExtraLiggghtsNonspherical::pow_abs(yb, n2 - 1.0)
|
|
const double zcn11 = zcn12 * fabs(zc); // = MathExtraLiggghtsNonspherical::pow_abs(zc, n1 - 1.0)
|
|
const double xan2 = xan21 * fabs(xa); // = MathExtraLiggghtsNonspherical::pow_abs(xa, n2)
|
|
const double ybn2 = ybn21 * fabs(yb); // = MathExtraLiggghtsNonspherical::pow_abs(yb, n2)
|
|
const double zcn1 = zcn11 * fabs(zc); // = MathExtraLiggghtsNonspherical::pow_abs(zc, n1)
|
|
|
|
if(MathExtraLiggghts::compDouble(n1, n2, 1e-2)) { //MathExtraLiggghtsNonspherical::pow_abs(xy_term, n1/n2 - 1.0);
|
|
*f = koef * (xan2 + ybn2 + zcn1 - 1.0);
|
|
grad[0] = koef * a * n1 * MathExtraLiggghtsNonspherical::sign(xa) * xan21;
|
|
grad[1] = koef * b * n1 * MathExtraLiggghtsNonspherical::sign(yb) * ybn21;
|
|
grad[2] = koef * c * n1 * MathExtraLiggghtsNonspherical::sign(zc) * zcn11;
|
|
|
|
if(hess != NULL) {
|
|
hess[0] = (koef * a) * (n1 * (n2 - 1.0) * xan22) * a;
|
|
hess[4] = (koef * b) * (n1 * (n2 - 1.0) * ybn22) * b;
|
|
hess[8] = (koef * c) * (n1 * (n1 - 1.0) * zcn12) * c;
|
|
hess[1] = hess[3] = hess[2] = hess[6] = hess[5] = hess[7] = 0.0;
|
|
}
|
|
}
|
|
else {
|
|
*f = koef * (MathExtraLiggghtsNonspherical::pow_abs(xan2 + ybn2, n1/n2) + zcn1 - 1.0);
|
|
const double xy_term = xan21 * fabs(xa) + ybn21 * fabs(yb); // MathExtraLiggghtsNonspherical::pow_abs(xa, n2) + MathExtraLiggghtsNonspherical::pow_abs(yb, n2)
|
|
double xy_term_powed2;
|
|
if(MathExtraLiggghtsNonspherical::isInteger(n1/n2)) {
|
|
int n1_over_n2_int = floor(n1/n2 + 1e-2);
|
|
xy_term_powed2 = MathExtraLiggghtsNonspherical::pow_abs_int(xy_term, n1_over_n2_int - 2.0);
|
|
} else
|
|
xy_term_powed2 = MathExtraLiggghtsNonspherical::pow_abs(xy_term, n1/n2 - 2.0);
|
|
double xy_term_powed1 = xy_term_powed2 * xy_term;
|
|
|
|
grad[0] = koef * a * n1 * MathExtraLiggghtsNonspherical::sign(xa) * xan21 * xy_term_powed1;
|
|
grad[1] = koef * b * n1 * MathExtraLiggghtsNonspherical::sign(yb) * ybn21 * xy_term_powed1;
|
|
grad[2] = koef * c * n1 * MathExtraLiggghtsNonspherical::sign(zc) * zcn11;
|
|
|
|
if(hess != NULL) {
|
|
hess[0] = (koef * a) * (n1 * (n2 - 1.0) * xan22 * xy_term_powed1 +
|
|
(n1 - n2) * n1 * xan21 * xan21 * xy_term_powed2) * a;
|
|
hess[4] = (koef * b) * (n1 * (n2 - 1.0) * ybn22 * xy_term_powed1 +
|
|
(n1 - n2) * n1 * ybn21 * ybn21 * xy_term_powed2) * b;
|
|
hess[1] = koef * (n1 - n2) * n1 * MathExtraLiggghtsNonspherical::sign(xa) * MathExtraLiggghtsNonspherical::sign(yb) * (xan21 * a) * (ybn21 * b) * xy_term_powed2;
|
|
|
|
hess[3] = hess[1];
|
|
hess[8] = (koef * c) * (n1 * (n1 - 1.0) * zcn12) * c;
|
|
hess[2] = hess[6] = hess[5] = hess[7] = 0.0;
|
|
}
|
|
}
|
|
}
|
|
|
|
//shape function props (value, gradient and hessian matrix) in global coordinates
|
|
void Superquadric::shape_function_props_global(const double *input_coord, double *f, double *grad, double *hess)
|
|
{
|
|
double coord_local[3];
|
|
global2local(input_coord, coord_local); //move to particle based reference frame
|
|
double grad_local[3];
|
|
if(hess != NULL) { //if we need hessian calculation
|
|
double hess_local[9];
|
|
shape_function_props_local(coord_local, f, grad_local, hess_local);
|
|
tensor_quat_rotate(hess_local, hess); // transform hessian to global frame
|
|
}
|
|
else //if we don't need hessian calculation
|
|
shape_function_props_local(coord_local, f, grad_local, NULL);
|
|
rotate_local2global(grad_local, grad); //transform gradient in global frame
|
|
}
|
|
|
|
//value of the particle shape function at x in global reference frame
|
|
double Superquadric::shape_function_global(const double *input_coord)
|
|
{
|
|
double coord_local[3];
|
|
global2local(input_coord, coord_local); //move to particle based (local) reference frame
|
|
return shape_function_local(coord_local); //calculate shape function value in local reference frame
|
|
}
|
|
|
|
//gradient of the shape function in global frame
|
|
void Superquadric::shape_function_gradient_global(const double *input_coord, double *result)
|
|
{
|
|
double x_local[3], grad_local[3];
|
|
global2local(input_coord, x_local); //transform point x to canonical frame
|
|
shape_function_gradient_local(x_local, grad_local); //calculate gradient in canonical frame
|
|
rotate_local2global(grad_local, result); //transform gradient in global frame
|
|
}
|
|
|
|
//hessian (matrix of the 2nd derivatives) of the shape function in global frame
|
|
void Superquadric::shape_function_hessian_global(const double *input_coord, double result[9])
|
|
{
|
|
double hessian_local[9], x_local[3];
|
|
global2local(input_coord, x_local); //transform point x to canonical frame
|
|
shape_function_hessian_local(x_local, hessian_local); //calculate hessian in canonical frame
|
|
tensor_quat_rotate(hessian_local, result); // transform hessian to global frame
|
|
}
|
|
|
|
//map point onto particle surface
|
|
void Superquadric::map_point(const double *input_coord, double *result)
|
|
{
|
|
double local_point[3];
|
|
global2local(input_coord, local_point);
|
|
double f = shape_function_local(local_point);
|
|
double alpha = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(f/koef + 1.0, 1.0/blockiness[0]);
|
|
for(int i = 0; i < 3; i++)
|
|
local_point[i] *= alpha;
|
|
local2global(local_point, result);
|
|
}
|
|
|
|
//map point onto particle surface
|
|
void Superquadric::map_point_local(double *input_coord, double scale)
|
|
{
|
|
double f = shape_function_local(input_coord);
|
|
double alpha = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(f/koef + 1.0, 1.0/blockiness[0]);
|
|
for(int i = 0; i < 3; i++)
|
|
input_coord[i] *= alpha*scale;
|
|
}
|
|
|
|
//calculate reference point
|
|
//old code, obsolete
|
|
void Superquadric::reference_point(int iphi, int nphi, int itheta, int ntheta, double *point)
|
|
{
|
|
double phi_start = 1.0e-2;
|
|
double theta_start = 1.0e-2;
|
|
double dphi = (2.0*M_PI - 2.0*phi_start) / static_cast<double>(nphi - 1);
|
|
double dtheta = (M_PI - 2.0*theta_start) / static_cast<double>(ntheta - 1);
|
|
double phi = dphi*static_cast<double>(iphi) + phi_start;
|
|
double theta = dtheta*static_cast<double>(itheta) + theta_start;
|
|
double r = std::max(std::max(shape[0], shape[1]), shape[2]);
|
|
double circle_point[3], local_point[3];
|
|
circle_point[0] = r * sin(theta)*cos(phi);
|
|
circle_point[1] = r * sin(theta)*sin(phi);
|
|
circle_point[2] = r * cos(theta);
|
|
local2global(circle_point, local_point);
|
|
map_point(local_point, point);
|
|
}
|
|
|
|
//old code, obsolete
|
|
void Superquadric::pre_initial_estimate(const double *input_point, int nphi, int ntheta, int *iphi, int *itheta)
|
|
{
|
|
double phi_start = 1.0e-2;
|
|
double theta_start = 1.0e-2;
|
|
double dphi = (2.0*M_PI -2.0*phi_start) / static_cast<double>(nphi - 1);
|
|
double dtheta = (M_PI - 2.0*theta_start) / static_cast<double>(ntheta - 1);
|
|
double phi, theta;
|
|
double local_point[3];
|
|
global2local(input_point, local_point);
|
|
double r = MathExtra::len3(local_point);
|
|
double cos_theta = local_point[2] / r;
|
|
double sin_theta = sqrt(1.0 - cos_theta*cos_theta);
|
|
if(sin_theta < sin(dtheta)) {
|
|
if(cos_theta > 0.0)
|
|
theta = theta_start;
|
|
else
|
|
theta = M_PI - theta_start;
|
|
phi = M_PI;
|
|
} else {
|
|
theta = acos(cos_theta);
|
|
double cos_phi = local_point[0] / sin_theta / r;
|
|
double sin_phi = local_point[1] / sin_theta / r;
|
|
if(sin_phi > 0.0)
|
|
phi = acos(cos_phi);
|
|
else
|
|
phi = 2.0 * M_PI - acos(cos_phi);
|
|
}
|
|
*iphi = static_cast<int>(round((phi - phi_start)/ dphi + nphi - 1)) % (nphi - 1);
|
|
*itheta = static_cast<int>(round((theta - theta_start) / dtheta + ntheta - 1)) % (ntheta - 1);
|
|
}
|
|
|
|
//shrink or blow particle
|
|
void Superquadric::apply_homothety(double scale_factor, double *input_point, double *result_point)
|
|
{
|
|
for(int i = 0; i < 3; i++)
|
|
shape[i] *= scale_factor;
|
|
double delta[3];
|
|
if(input_point) {
|
|
MathExtra::sub3(input_point, center, delta);
|
|
MathExtra::scale3(scale_factor, delta);
|
|
MathExtra::add3(center, delta, result_point);
|
|
}
|
|
}
|
|
|
|
//particle-wall intersection check
|
|
//finds the point of maximal penetration on the particle surface and the point of minimal shape function value on the surface
|
|
bool Superquadric::plane_intersection(const double *normal_global, const double *x0_global, double *point_of_maximal_penetration, double *point_of_lowest_potential) {
|
|
double point_of_maximal_penetration_local[3], x0_local[3], normal_local[3];
|
|
rotate_global2local(normal_global, normal_local);
|
|
global2local(x0_global, x0_local);
|
|
if(LAMMPS_NS::vectorDot3D(normal_local, x0_local) < 0.0) //check that normal vector aligned outwards
|
|
LAMMPS_NS::vectorNegate3D(normal_local); //make the normal aligned outwards
|
|
|
|
const double n1 = blockiness[0];
|
|
const double n2 = blockiness[1];
|
|
const double nx = normal_local[0];
|
|
const double ny = normal_local[1];
|
|
const double nz = normal_local[2];
|
|
const double a = shape[0];
|
|
const double b = shape[1];
|
|
const double c = shape[2];
|
|
|
|
double point_of_lowest_potential_local[3]; //point on wall with the lowest value of shape function of a given particle
|
|
|
|
const double d = LAMMPS_NS::vectorDot3D(normal_local, x0_local);
|
|
|
|
if(MathExtraLiggghts::compDouble(nx, 0.0, 1e-10) && MathExtraLiggghts::compDouble(ny, 0.0, 1e-10)) {
|
|
point_of_maximal_penetration_local[0] = point_of_maximal_penetration_local[1] = 0.0;
|
|
point_of_maximal_penetration_local[2] = 1.0;
|
|
|
|
point_of_lowest_potential_local[0] = point_of_lowest_potential_local[1] = 0.0;
|
|
point_of_lowest_potential_local[2] = d / nz;
|
|
} else {
|
|
|
|
if(fabs(nx) > fabs(ny)) {
|
|
double alpha = MathExtraLiggghtsNonspherical::pow_abs((ny * b) / (nx * a), 1.0 / (n2 - 1.0));
|
|
double gamma1 = 1.0 + MathExtraLiggghtsNonspherical::pow_abs(alpha, n2);
|
|
double gamma = MathExtraLiggghtsNonspherical::pow_abs(gamma1, n1/n2 - 1.0);
|
|
double beta = MathExtraLiggghtsNonspherical::pow_abs((nz * c) / (nx * a) * gamma, 1.0/(n1 - 1.0));
|
|
|
|
point_of_maximal_penetration_local[0] = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(MathExtraLiggghtsNonspherical::pow_abs(gamma1, n1/n2) + MathExtraLiggghtsNonspherical::pow_abs(beta, n1), 1.0 / n1);
|
|
point_of_maximal_penetration_local[1] = alpha * point_of_maximal_penetration_local[0];
|
|
point_of_maximal_penetration_local[2] = beta * point_of_maximal_penetration_local[0];
|
|
|
|
point_of_lowest_potential_local[0] = fabs(d) / (fabs(nx)*a + alpha * fabs(ny) * b + beta * fabs(nz) * c);
|
|
point_of_lowest_potential_local[1] = alpha * point_of_lowest_potential_local[0];
|
|
point_of_lowest_potential_local[2] = beta * point_of_lowest_potential_local[0];
|
|
} else {
|
|
double alpha = MathExtraLiggghtsNonspherical::pow_abs((nx * a) / (ny * b), 1.0 / (n2 - 1.0));
|
|
double gamma1 = 1.0 + MathExtraLiggghtsNonspherical::pow_abs(alpha, n2);
|
|
double gamma = MathExtraLiggghtsNonspherical::pow_abs(gamma1, n1/n2 - 1.0);
|
|
double beta = MathExtraLiggghtsNonspherical::pow_abs((nz * c) / (ny * b) * gamma, 1.0/(n1 - 1.0));
|
|
point_of_maximal_penetration_local[1] = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(MathExtraLiggghtsNonspherical::pow_abs(gamma1, n1/n2) + MathExtraLiggghtsNonspherical::pow_abs(beta, n1), 1.0 / n1);
|
|
point_of_maximal_penetration_local[0] = alpha * point_of_maximal_penetration_local[1];
|
|
point_of_maximal_penetration_local[2] = beta * point_of_maximal_penetration_local[1];
|
|
|
|
point_of_lowest_potential_local[1] = fabs(d) / (fabs(ny) * b + alpha * fabs(nx) * a + beta * fabs(nz) * c);
|
|
point_of_lowest_potential_local[0] = alpha * point_of_lowest_potential_local[1];
|
|
point_of_lowest_potential_local[2] = beta * point_of_lowest_potential_local[1];
|
|
}
|
|
for(int i = 0; i < 3; i++)
|
|
point_of_lowest_potential_local[i] *= shape[i] * MathExtraLiggghtsNonspherical::sign(normal_local[i]);
|
|
}
|
|
|
|
for(int i = 0; i < 3; i++)
|
|
point_of_maximal_penetration_local[i] *= shape[i] * MathExtraLiggghtsNonspherical::sign(normal_local[i]);
|
|
|
|
double ff = shape_function_local(point_of_lowest_potential_local);
|
|
local2global(point_of_maximal_penetration_local, point_of_maximal_penetration);
|
|
local2global(point_of_lowest_potential_local, point_of_lowest_potential);
|
|
if(ff < 0.0) //particle and wall intersect
|
|
return true;
|
|
else //particle and wall do not intersect
|
|
return false;
|
|
}
|
|
|
|
//find a point on edge incl.corners with a minimal value of particle shape function (point of lowest potential)
|
|
double Superquadric::line_intersection(const double *pointA, const double *pointB, double *closestPoint)
|
|
{
|
|
double A[3], B[3];
|
|
double closestPointLocal[3], closestPointLocal1[3], closestPointLocal2[3];
|
|
//using "golden section" search algorithm
|
|
//https://en.wikipedia.org/wiki/Golden_section_search
|
|
|
|
global2local(pointA, A);
|
|
global2local(pointB, B);
|
|
|
|
double a = 0.0;
|
|
double b = 1.0;
|
|
double eps = fabs(b-a);
|
|
double alpha1 = b - (b - a)*PHI_INV;
|
|
double alpha2 = a + (b - a)*PHI_INV;
|
|
for(int k = 0; k < 3; k++) {
|
|
closestPointLocal1[k] = A[k] + alpha1*(B[k] - A[k]);
|
|
closestPointLocal2[k] = A[k] + alpha2*(B[k] - A[k]);
|
|
}
|
|
double f1 = shape_function_local(closestPointLocal1);
|
|
double f2 = shape_function_local(closestPointLocal2);
|
|
while(eps > 1e-8) {
|
|
alpha1 = b - (b - a)*PHI_INV;
|
|
alpha2 = a + (b - a)*PHI_INV;
|
|
for(int k = 0; k < 3; k++) {
|
|
closestPointLocal1[k] = A[k] + alpha1*(B[k] - A[k]);
|
|
closestPointLocal2[k] = A[k] + alpha2*(B[k] - A[k]);
|
|
}
|
|
f1 = shape_function_local(closestPointLocal1);
|
|
f2 = shape_function_local(closestPointLocal2);
|
|
if(f1 >= f2) {
|
|
a = alpha1;
|
|
} else
|
|
b = alpha2;
|
|
eps = fabs(b-a);
|
|
}
|
|
for(int k = 0; k < 3; k++)
|
|
closestPointLocal[k] = 0.5*(closestPointLocal1[k] + closestPointLocal2[k]);
|
|
double f = shape_function_local(closestPointLocal);
|
|
double fA = shape_function_local(A);
|
|
double fB = shape_function_local(B);
|
|
if(fA < f) {
|
|
LAMMPS_NS::vectorCopy3D(pointA, closestPoint);
|
|
f = fA;
|
|
}
|
|
else if(fB < f) {
|
|
LAMMPS_NS::vectorCopy3D(pointB, closestPoint);
|
|
f = fB;
|
|
}
|
|
else
|
|
local2global(closestPointLocal, closestPoint);
|
|
return f;
|
|
}
|
|
|
|
//check superquadric-edge intersection (only yes or no)
|
|
bool Superquadric::edge_intersection(const double *pointA, const double *pointB)
|
|
{
|
|
double A[3], B[3];
|
|
double closestPointLocal[3], closestPointLocal1[3], closestPointLocal2[3];
|
|
//using "golden section" search algorithm
|
|
//https://en.wikipedia.org/wiki/Golden_section_search
|
|
|
|
global2local(pointA, A);
|
|
global2local(pointB, B);
|
|
|
|
double a = 0.0;
|
|
double b = 1.0;
|
|
double eps = fabs(b-a);
|
|
double alpha1 = b - (b - a)*PHI_INV;
|
|
double alpha2 = a + (b - a)*PHI_INV;
|
|
for(int k = 0; k < 3; k++) {
|
|
closestPointLocal1[k] = A[k] + alpha1*(B[k] - A[k]);
|
|
closestPointLocal2[k] = A[k] + alpha2*(B[k] - A[k]);
|
|
}
|
|
double f1 = shape_function_local(closestPointLocal1);
|
|
double f2 = shape_function_local(closestPointLocal2);
|
|
if(f1 < 0.0 or f2 < 0.0)
|
|
return true;
|
|
else {
|
|
while(eps > 1e-4) {
|
|
alpha1 = b - (b - a)*PHI_INV;
|
|
alpha2 = a + (b - a)*PHI_INV;
|
|
for(int k = 0; k < 3; k++) {
|
|
closestPointLocal1[k] = A[k] + alpha1*(B[k] - A[k]);
|
|
closestPointLocal2[k] = A[k] + alpha2*(B[k] - A[k]);
|
|
}
|
|
f1 = shape_function_local(closestPointLocal1);
|
|
f2 = shape_function_local(closestPointLocal2);
|
|
if(f1 < 0.0 || f2 < 0.0) {
|
|
return true;
|
|
} else {
|
|
if(f1 >= f2) {
|
|
a = alpha1;
|
|
} else
|
|
b = alpha2;
|
|
eps = fabs(b-a);
|
|
}
|
|
}
|
|
for(int k = 0; k < 3; k++)
|
|
closestPointLocal[k] = 0.5*(closestPointLocal1[k] + closestPointLocal2[k]);
|
|
double f = shape_function_local(closestPointLocal);
|
|
if(f < 0.0)
|
|
return true; //particle and edge intersect
|
|
else
|
|
return false; //particle and edge do not intersect
|
|
}
|
|
}
|
|
|
|
//rotate tensor
|
|
void Superquadric::tensor_quat_rotate(double *tensor, double *result)
|
|
{
|
|
double temp[9];
|
|
MathExtraLiggghtsNonspherical::times3(rotation_matrix, tensor, temp); // calc temp = A * M
|
|
MathExtraLiggghtsNonspherical::times3_transpose(temp, rotation_matrix, result); //calc result = A * M * A^T
|
|
}
|
|
|
|
//https://en.wikipedia.org/wiki/Mean_curvature
|
|
//calculates the mean curvature radius of a particle surface at a given point
|
|
double Superquadric::calc_curvature_coefficient(int curvature_radius, const double *input_point)
|
|
{
|
|
double local_point[3],hessian[9], F[3];
|
|
global2local(input_point, local_point);
|
|
double f = shape_function_local(local_point);
|
|
double alpha = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(f/koef + 1.0, 1.0/blockiness[0]);
|
|
for(int i = 0; i < 3; i++)
|
|
local_point[i] *= alpha;
|
|
shape_function_gradient_local(local_point, F);
|
|
rotate_local2global(F, gradient);
|
|
shape_function_hessian_local(local_point, hessian);
|
|
double F_mag = LAMMPS_NS::vectorMag3D(F);
|
|
double n[3];
|
|
LAMMPS_NS::vectorScalarMult3D(F, 1.0/F_mag, n);
|
|
double temp[3];
|
|
MathExtraLiggghtsNonspherical::matvec(hessian, n, temp);
|
|
if(curvature_radius == 0) { //mean curvature
|
|
return fabs(MathExtra::dot3(n, temp) - (hessian[0] + hessian[4] + hessian[8])) / (2.0 * F_mag);
|
|
} else { //gaussian curvature
|
|
double fx = F[0];
|
|
double fy = F[1];
|
|
double fz = F[2];
|
|
|
|
double fxx = hessian[0];
|
|
double fxy = hessian[1];
|
|
double fxz = hessian[2];
|
|
|
|
double fyy = hessian[4];
|
|
double fyz = hessian[5];
|
|
|
|
double fzz = hessian[8];
|
|
|
|
double mat[] = {
|
|
fxx, fxy, fxz, fx,
|
|
fxy, fyy, fyz, fy,
|
|
fxz, fyz, fzz, fz,
|
|
fx, fy, fz, 0
|
|
};
|
|
|
|
double K = -MathExtraLiggghtsNonspherical::determinant_4x4(mat) / (F_mag*F_mag*F_mag*F_mag);
|
|
return sqrt(fabs(K));
|
|
}
|
|
}
|
|
|
|
void Superquadric::set_shape(double a, double b, double c)
|
|
{
|
|
shape[0] = a;
|
|
shape[1] = b;
|
|
shape[2] = c;
|
|
|
|
shape_inv[0] = 1.0 / shape[0];
|
|
shape_inv[1] = 1.0 / shape[1];
|
|
shape_inv[2] = 1.0 / shape[2];
|
|
}
|
|
|
|
void Superquadric::set_blockiness(double n1, double n2)
|
|
{
|
|
blockiness[0] = n1;
|
|
blockiness[1] = n2;
|
|
calc_koef();
|
|
isEllipsoid = MathExtraLiggghts::compDouble(blockiness[0], 2.0, 1e-2) && MathExtraLiggghts::compDouble(blockiness[1], 2.0, 1e-2);
|
|
isCylinder = !MathExtraLiggghts::compDouble(blockiness[0], 2.0, 1e-2) && MathExtraLiggghts::compDouble(blockiness[1], 2.0, 1e-2);
|
|
if(isEllipsoid) {
|
|
useIntBlockiness = true;
|
|
} else {
|
|
useIntBlockiness = MathExtraLiggghtsNonspherical::isInteger(blockiness[0]) && MathExtraLiggghtsNonspherical::isInteger(blockiness[1]);
|
|
}
|
|
}
|
|
|
|
void Superquadric::calc_koef()
|
|
{
|
|
koef = MathExtraLiggghtsNonspherical::pow_abs(0.5, std::max(blockiness[0], blockiness[1])-2.0);
|
|
}
|
|
|
|
//calculates the intersection point between a line and particle surface with the Newton's method
|
|
double Superquadric::surface_line_intersection(bool use_alpha_ellipsoid, const double *start_point, const double *direction_vector, double alpha_prev, double *result)
|
|
{
|
|
return Superquadric::surface_line_intersection(20, use_alpha_ellipsoid, start_point, direction_vector, alpha_prev, result);
|
|
}
|
|
|
|
//calculates the intersection point between a line and particle surface with the Newton's method
|
|
double Superquadric::surface_line_intersection(const int max_num_iters, bool use_alpha_ellipsoid, const double *start_point, const double *direction_vector, double alpha_prev, double *result)
|
|
{
|
|
double start_point_local[3], direction_vector_local[3], point[3];
|
|
double grad_local[3];
|
|
|
|
global2local(start_point, start_point_local);
|
|
rotate_global2local(direction_vector, direction_vector_local); //rotate direction vector to the local reference frame
|
|
double alpha = alpha_prev;
|
|
double eps0 = 1e-14;
|
|
double alpha_ellipsoid = 0.0;
|
|
|
|
if(isEllipsoid || use_alpha_ellipsoid) {
|
|
double alpha1, alpha2;
|
|
ellipsoid_line_intersection_local(start_point_local, direction_vector_local, alpha1, alpha2);
|
|
|
|
if(isEllipsoid) { //particle is an ellipsoid/sphere
|
|
if(alpha1 > -eps0 && alpha2 < -eps0)
|
|
alpha = alpha1;
|
|
else if(alpha1 < -eps0 && alpha2 > -eps0)
|
|
alpha = alpha2;
|
|
else if(alpha1 < -eps0 && alpha2 < -eps0)
|
|
alpha = std::max(alpha1, alpha2);
|
|
else
|
|
alpha = std::min(alpha1, alpha2);
|
|
} else { //inscribed ellipsoid
|
|
double f = shape_function_local(start_point_local);
|
|
|
|
if(f > eps0)
|
|
alpha_ellipsoid = std::min(alpha1, alpha2);
|
|
else if (f < -eps0)
|
|
alpha_ellipsoid = std::max(alpha1, alpha2);
|
|
else {
|
|
alpha_ellipsoid = alpha = alpha_prev = 0.0;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(!isEllipsoid) {
|
|
double f_der, delta;
|
|
|
|
double a = 0.5*std::min(std::min(shape[0], shape[1]), shape[2]) / LAMMPS_NS::vectorMag3D(direction_vector);
|
|
const double eps = 1e-16; //small epsilon
|
|
|
|
alpha = MathExtraLiggghtsNonspherical::clamp(alpha, -a, a);
|
|
MathExtraLiggghtsNonspherical::yabx3D(start_point_local, alpha, direction_vector_local, point);
|
|
double f0 = shape_function_local(point);
|
|
if(use_alpha_ellipsoid) {
|
|
double f1;
|
|
double point_ellipsoid[3];
|
|
MathExtraLiggghtsNonspherical::yabx3D(start_point_local, alpha_ellipsoid, direction_vector_local, point_ellipsoid);
|
|
f1 = shape_function_local(point_ellipsoid);
|
|
if(fabs(f1) < fabs(f0)) {
|
|
alpha = alpha_ellipsoid;
|
|
f0 = f1;
|
|
LAMMPS_NS::vectorCopy3D(point_ellipsoid, point);
|
|
}
|
|
}
|
|
|
|
if(fabs(f0) < eps) {
|
|
local2global(point, result);
|
|
return alpha;
|
|
}
|
|
|
|
for(int i = 0; i < max_num_iters; i++) {
|
|
if(fabs(f0) < eps*koef)
|
|
break;
|
|
shape_function_gradient_local(point, grad_local);
|
|
f_der = LAMMPS_NS::vectorDot3D(grad_local, direction_vector_local);
|
|
|
|
if(fabs(f_der)<1e-10) {
|
|
delta = 1e-10;
|
|
double point_[3], grad_local_[3];
|
|
MathExtraLiggghtsNonspherical::yabx3D(point, delta, direction_vector_local, point_);
|
|
shape_function_gradient_local(point_, grad_local_);
|
|
double f_der_ = LAMMPS_NS::vectorDot3D(grad_local_, direction_vector_local);
|
|
if(f_der_ < 0.0)
|
|
delta = -delta;
|
|
}
|
|
else
|
|
delta = MathExtraLiggghtsNonspherical::clamp(-f0 / f_der, -a, a);
|
|
if(fabs(delta) <= eps) {
|
|
alpha += delta;
|
|
MathExtraLiggghtsNonspherical::yabx3D(point, delta, direction_vector_local, point);
|
|
break;
|
|
}
|
|
else {
|
|
while(fabs(delta) > eps) {
|
|
double point_[3];
|
|
MathExtraLiggghtsNonspherical::yabx3D(point, delta, direction_vector_local, point_);
|
|
double f_ = shape_function_local(point_);
|
|
if(fabs(f_) < fabs(f0) || fabs(f_) < eps*koef) {
|
|
f0 = f_;
|
|
alpha += delta;
|
|
LAMMPS_NS::vectorCopy3D(point_, point);
|
|
break;
|
|
} else
|
|
delta *= 0.5;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
result[0] = start_point[0] + alpha*direction_vector[0];
|
|
result[1] = start_point[1] + alpha*direction_vector[1];
|
|
result[2] = start_point[2] + alpha*direction_vector[2];
|
|
return alpha;
|
|
}
|
|
|
|
double Superquadric::calc_F(double *p, double *p0, double l, double *F)
|
|
{
|
|
double grad_local[3];
|
|
shape_function_gradient_local(p, grad_local);
|
|
F[0] = shape_function_local(p);
|
|
F[1] = l*grad_local[0] - p[0] + p0[0];
|
|
F[2] = l*grad_local[1] - p[1] + p0[1];
|
|
F[3] = l*grad_local[2] - p[2] + p0[2];
|
|
|
|
return LAMMPS_NS::vectorMag4D(F);
|
|
}
|
|
|
|
// UNUSED
|
|
/*
|
|
double Superquadric::point_surface_projection(const int max_num_iters, bool use_alpha_ellipsoid, const double *start_point, double alpha_prev, double *result)
|
|
{
|
|
const double tol = 1e-15;
|
|
const double tol2 = 1e-5;
|
|
const double size = std::max(std::max(shape[0],shape[1]),shape[2]);
|
|
|
|
double initial_estimate[3];
|
|
double p0[3], p[3];
|
|
double n[3], v[3];
|
|
double grad_local[3];
|
|
double F[4];
|
|
|
|
double direction_vector[3];
|
|
shape_function_gradient_global(start_point, direction_vector);
|
|
LAMMPS_NS::vectorNormalize3D(direction_vector);
|
|
if(shape_function_global(start_point) > 0.0)
|
|
LAMMPS_NS::vectorNegate3D(direction_vector); //{-7.7462629689949625e-01, -1.7969401213388331e-01, -6.0635316619524737e-01};
|
|
|
|
surface_line_intersection(max_num_iters, use_alpha_ellipsoid, start_point, direction_vector, alpha_prev, initial_estimate);
|
|
global2local(start_point, p0);
|
|
global2local(initial_estimate, p);
|
|
|
|
shape_function_gradient_local(p, grad_local);
|
|
LAMMPS_NS::vectorSubtract3D(p, p0, n);
|
|
MathExtra::cross3(n, grad_local, v);
|
|
double sine = sqrt(fabs(LAMMPS_NS::vectorDot3D(v,v) / (LAMMPS_NS::vectorDot3D(grad_local, grad_local) * LAMMPS_NS::vectorDot3D(n, n))));
|
|
|
|
double l = LAMMPS_NS::vectorDot3D(grad_local, n) / LAMMPS_NS::vectorDot3D(grad_local, grad_local);
|
|
|
|
double hess[9];
|
|
double delta0[] = {0.0, 0.0, 0.0, 0.0};
|
|
|
|
double res0 = 1e+99;
|
|
double res1 = res0;
|
|
double res2 = res1;
|
|
bool converged= false;
|
|
|
|
int iters = 0;
|
|
double magDelta = -1;
|
|
double f = -1;
|
|
for(int iter = 0; iter < max_num_iters; iter++) {
|
|
|
|
shape_function_gradient_local(p, grad_local);
|
|
shape_function_hessian_local(p, hess);
|
|
|
|
double J[] = { grad_local[0], grad_local[1], grad_local[2], 0.0,
|
|
l*hess[0]-1.0, l*hess[1], l*hess[2], grad_local[0],
|
|
l*hess[3], l*hess[4]-1.0, l*hess[5], grad_local[1],
|
|
l*hess[6], l*hess[7], l*hess[8]-1.0, grad_local[2] };
|
|
|
|
calc_F(p, p0, l, F);
|
|
double delta[4];
|
|
MathExtraLiggghtsNonspherical::GMRES<4,4>(J, F, delta0, delta);
|
|
//printf("iter: %d, point: %e %e %e, mag(delta)=%e\n", iter, p[0],p[1],p[2],LAMMPS_NS::vectorMag4D(delta));
|
|
magDelta = std::max(std::max(fabs(delta[0]),fabs(delta[1])),fabs(delta[2]));
|
|
if(magDelta < tol*size)
|
|
converged = true;
|
|
|
|
double p_new[3] = { p[0] - delta[0],
|
|
p[1] - delta[1],
|
|
p[2] - delta[2] };
|
|
double l_new = l - delta[3];
|
|
double F_new[4];
|
|
res2 = calc_F(p_new, p0, l_new, F_new);
|
|
if(res2 < res1 || converged) {
|
|
LAMMPS_NS::vectorCopy3D(p_new, p);
|
|
l = l_new;
|
|
res1 = res2;
|
|
} else {
|
|
|
|
double a = 0.0;
|
|
double b = 1.0;
|
|
double eps = fabs(b-a);
|
|
double pointa[3], pointb[3];
|
|
double Fa[4], Fb[4];
|
|
double res2a, res2b;
|
|
double la, lb;
|
|
while(eps > 1e-12) {
|
|
double alpha1 = b - (b - a)*PHI_INV;
|
|
double alpha2 = a + (b - a)*PHI_INV;
|
|
|
|
MathExtraLiggghtsNonspherical::yabx3D(p, -alpha1, delta, pointa);
|
|
la = l - alpha1*delta[3];
|
|
res2a = calc_F(pointa, p0, la, Fa);
|
|
|
|
MathExtraLiggghtsNonspherical::yabx3D(p, -alpha2, delta, pointb);
|
|
lb = l - alpha2*delta[3];
|
|
res2b = calc_F(pointb, p0, lb, Fb);
|
|
if(std::min(res2a, res2b) < res1) {
|
|
if(res2a < res2b) {
|
|
res1 = res2a;
|
|
LAMMPS_NS::vectorCopy4D(Fa, F);
|
|
l = la;
|
|
LAMMPS_NS::vectorCopy3D(pointa, p);
|
|
} else {
|
|
res1 = res2b;
|
|
LAMMPS_NS::vectorCopy4D(Fb, F);
|
|
l = lb;
|
|
LAMMPS_NS::vectorCopy3D(pointb, p);
|
|
}
|
|
eps = 0.0;
|
|
} else {
|
|
if(res2a > res2b)
|
|
a = alpha1;
|
|
else
|
|
b = alpha2;
|
|
eps = fabs(b-a);
|
|
}
|
|
}
|
|
}
|
|
|
|
LAMMPS_NS::vectorSubtract3D(p, p0, n);
|
|
MathExtra::cross3(grad_local, n, v);
|
|
shape_function_gradient_local(p, grad_local);
|
|
sine = sqrt(fabs(LAMMPS_NS::vectorDot3D(v,v) / (LAMMPS_NS::vectorDot3D(grad_local, grad_local) * LAMMPS_NS::vectorDot3D(n, n))));
|
|
f = fabs(shape_function_local(p));
|
|
if(sine < tol2 && f < tol)
|
|
converged = true;
|
|
|
|
if(converged)
|
|
break;
|
|
iters ++;
|
|
}
|
|
|
|
if(iters >= max_num_iters - 1) {
|
|
double start_point_temp[3];
|
|
local2global(p, start_point_temp);
|
|
shape_function_gradient_global(start_point_temp, direction_vector);
|
|
LAMMPS_NS::vectorNormalize3D(direction_vector);
|
|
f = shape_function_global(start_point_temp);
|
|
if(f > 0.0)
|
|
LAMMPS_NS::vectorNegate3D(direction_vector);
|
|
surface_line_intersection(max_num_iters, false, start_point_temp, direction_vector, alpha_prev, initial_estimate);
|
|
LAMMPS_NS::vectorCopy3D(initial_estimate, result);
|
|
f = fabs(shape_function_global(result));
|
|
LAMMPS_NS::vectorSubtract3D(start_point, result, n);
|
|
MathExtra::cross3(direction_vector, n, v);
|
|
sine = sqrt(fabs(LAMMPS_NS::vectorDot3D(v,v) / (LAMMPS_NS::vectorDot3D(direction_vector, direction_vector) * LAMMPS_NS::vectorDot3D(n, n))));
|
|
}
|
|
else
|
|
local2global(p, result);
|
|
return LAMMPS_NS::pointDistance(result, start_point);
|
|
}*/
|
|
|
|
//TODO: reduce code duplication
|
|
double Superquadric::surface_line_intersection1(const double *start_point, const double *direction_vector, double *result)
|
|
{
|
|
double start_point_local[3], direction_vector_local[3], point[3];
|
|
double grad_local[3];
|
|
|
|
global2local(start_point, start_point_local);
|
|
LAMMPS_NS::vectorCopy3D(start_point_local, point);
|
|
rotate_global2local(direction_vector, direction_vector_local); //rotate direction vector to the local reference frame
|
|
|
|
double f_der, delta;
|
|
double alpha = 0.0;
|
|
const double eps = 1e-16; //small epsilon
|
|
|
|
double f0 = shape_function_local(start_point_local);
|
|
int num_iters = 10; //max number of Newton iterations
|
|
|
|
for(int i = 0; i < num_iters; i++) {
|
|
if(fabs(f0) < eps*koef)
|
|
break;
|
|
shape_function_gradient_local(point, grad_local);
|
|
f_der = LAMMPS_NS::vectorDot3D(grad_local, direction_vector_local);
|
|
|
|
delta = -f0 / f_der;
|
|
alpha += delta;
|
|
MathExtraLiggghtsNonspherical::yabx3D(point, delta, direction_vector_local, point);
|
|
if(fabs(delta) <= eps)
|
|
break;
|
|
f0 = shape_function_local(point);
|
|
}
|
|
result[0] = start_point[0] + alpha*direction_vector[0];
|
|
result[1] = start_point[1] + alpha*direction_vector[1];
|
|
result[2] = start_point[2] + alpha*direction_vector[2];
|
|
return alpha;
|
|
}
|
|
|
|
void Superquadric::ellipsoid_line_intersection_local(const double *start_point_local, const double *direction_vector_local, double& alpha1, double& alpha2)
|
|
{
|
|
const double x0 = start_point_local[0] * shape_inv[0];
|
|
const double nx = direction_vector_local[0] * shape_inv[0];
|
|
|
|
const double y0 = start_point_local[1] * shape_inv[1];
|
|
const double ny = direction_vector_local[1] * shape_inv[1];
|
|
|
|
const double z0 = start_point_local[2] * shape_inv[2];
|
|
const double nz = direction_vector_local[2] * shape_inv[2];
|
|
|
|
const double a = nx*nx + ny*ny + nz*nz;
|
|
const double b = 2.0*(x0*nx + y0*ny + z0*nz);
|
|
const double c = x0*x0 + y0*y0 + z0*z0 - 1.0;
|
|
const double D = b*b - 4.0*a*c;
|
|
const double two_a_inverted = 0.5 / a;
|
|
if(D < 0) {
|
|
alpha1 = -b * two_a_inverted;
|
|
alpha2 = alpha1;
|
|
} else {
|
|
double sqrtD = sqrt(D);
|
|
alpha1 = (-b + sqrtD) * two_a_inverted;
|
|
alpha2 = (-b - sqrtD) * two_a_inverted;
|
|
}
|
|
}
|
|
|
|
void Superquadric::ellipsoid_line_intersection(const double *start_point, const double *direction_vector, double& alpha1, double& alpha2)
|
|
{
|
|
double start_point_local[3], direction_vector_local[3];
|
|
global2local(start_point, start_point_local);
|
|
rotate_global2local(direction_vector, direction_vector_local);
|
|
|
|
ellipsoid_line_intersection_local(start_point_local, direction_vector_local, alpha1, alpha2);
|
|
}
|
|
#endif
|