double taylorDistanceSq1D(const GradientBasis *gb, const fullMatrix<double> &nodesXYZ,
const std::vector<SVector3> &tanCAD)
{
const int nV = nodesXYZ.size1();
fullMatrix<double> dxyzdX(nV, 3);
gb->getGradientsFromNodes(nodesXYZ, &dxyzdX, 0, 0);
// const double dx = nodesXYZ(1, 0) - nodesXYZ(0, 0), dy = nodesXYZ(1, 1) - nodesXYZ(0, 1),
// dz = nodesXYZ(1, 2) - nodesXYZ(0, 2), h = 0.5*sqrt(dx*dx+dy*dy+dz*dz)/double(nV-1);
double distSq = 0.;
for (int i=0; i<nV; i++) {
SVector3 tanMesh(dxyzdX(i, 0), dxyzdX(i, 1), dxyzdX(i, 2));
const double h = 0.25*tanMesh.normalize(); // Half of "local edge length"
SVector3 diff = (dot(tanCAD[i], tanMesh) > 0) ?
tanCAD[i] - tanMesh : tanCAD[i] + tanMesh;
distSq += h*h*diff.normSq();
}
return distSq;
}
double taylorDistanceSq2D(const GradientBasis *gb, const fullMatrix<double> &nodesXYZ,
const std::vector<SVector3> &normCAD)
{
const int nV = nodesXYZ.size1();
fullMatrix<double> dxyzdX(nV, 3), dxyzdY(nV, 3);
gb->getGradientsFromNodes(nodesXYZ, &dxyzdX, &dxyzdY, 0);
double distSq = 0.;
for (int i=0; i<nV; i++) {
const double nz = dxyzdX(i, 0) * dxyzdY(i, 1) - dxyzdX(i, 1) * dxyzdY(i, 0);
const double ny = -dxyzdX(i, 0) * dxyzdY(i, 2) + dxyzdX(i, 2) * dxyzdY(i, 0);
const double nx = dxyzdX(i, 1) * dxyzdY(i, 2) - dxyzdX(i, 2) * dxyzdY(i, 1);
SVector3 normMesh(nx, ny, nz);
const double h = 0.25*sqrt(normMesh.normalize()); // Half of sqrt of "local area", to be adjusted w.r.t. el. type?
SVector3 diff = (dot(normCAD[i], normMesh) > 0) ?
normCAD[i] - normMesh : normCAD[i] + normMesh;
distSq += h*h*diff.normSq();
}
return distSq;
}
// Calculate (signed) Jacobian and its gradients for one element, with normal vectors to straight element
// for regularization. Evaluation points depend on the given matrices for shape function gradients.
void JacobianBasis::getSignedJacAndGradientsGeneral(int nJacNodes, const fullMatrix<double> &gSMatX,
const fullMatrix<double> &gSMatY,
const fullMatrix<double> &gSMatZ,
const fullMatrix<double> &nodesXYZ,
const fullMatrix<double> &normals,
fullMatrix<double> &JDJ) const
{
switch (_dim) {
case 0 : {
for (int i = 0; i < nJacNodes; i++) {
for (int j = 0; j < numMapNodes; j++) {
JDJ (i,j) = 0.;
JDJ (i,j+1*numMapNodes) = 0.;
JDJ (i,j+2*numMapNodes) = 0.;
}
JDJ(i,3*numMapNodes) = 1.;
}
break;
}
case 1 : {
fullMatrix<double> dxyzdX(nJacNodes,3), dxyzdY(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = normals(0,0), &dydY = normals(0,1), &dzdY = normals(0,2);
const double &dxdZ = normals(1,0), &dydZ = normals(1,1), &dzdZ = normals(1,2);
for (int j = 0; j < numMapNodes; j++) {
const double &dPhidX = gSMatX(i,j);
JDJ (i,j) = dPhidX * dydY * dzdZ + dPhidX * dzdY * dydZ;
JDJ (i,j+1*numMapNodes) = dPhidX * dzdY * dxdZ - dPhidX * dxdY * dzdZ;
JDJ (i,j+2*numMapNodes) = dPhidX * dxdY * dydZ - dPhidX * dydY * dxdZ;
}
JDJ(i,3*numMapNodes) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
break;
}
case 2 : {
fullMatrix<double> dxyzdX(nJacNodes,3), dxyzdY(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
gSMatY.mult(nodesXYZ, dxyzdY);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = dxyzdY(i,0), &dydY = dxyzdY(i,1), &dzdY = dxyzdY(i,2);
const double &dxdZ = normals(0,0), &dydZ = normals(0,1), &dzdZ = normals(0,2);
for (int j = 0; j < numMapNodes; j++) {
const double &dPhidX = gSMatX(i,j);
const double &dPhidY = gSMatY(i,j);
JDJ (i,j) =
dPhidX * dydY * dzdZ + dzdX * dPhidY * dydZ +
dPhidX * dzdY * dydZ - dydX * dPhidY * dzdZ;
JDJ (i,j+1*numMapNodes) =
dxdX * dPhidY * dzdZ +
dPhidX * dzdY * dxdZ - dzdX * dPhidY * dxdZ
- dPhidX * dxdY * dzdZ;
JDJ (i,j+2*numMapNodes) =
dPhidX * dxdY * dydZ +
dydX * dPhidY * dxdZ - dPhidX * dydY * dxdZ -
dxdX * dPhidY * dydZ;
}
JDJ(i,3*numMapNodes) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
break;
}
case 3 : {
fullMatrix<double> dxyzdX(nJacNodes,3), dxyzdY(nJacNodes,3), dxyzdZ(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
gSMatY.mult(nodesXYZ, dxyzdY);
gSMatZ.mult(nodesXYZ, dxyzdZ);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = dxyzdY(i,0), &dydY = dxyzdY(i,1), &dzdY = dxyzdY(i,2);
const double &dxdZ = dxyzdZ(i,0), &dydZ = dxyzdZ(i,1), &dzdZ = dxyzdZ(i,2);
for (int j = 0; j < numMapNodes; j++) {
const double &dPhidX = gSMatX(i,j);
const double &dPhidY = gSMatY(i,j);
const double &dPhidZ = gSMatZ(i,j);
JDJ (i,j) =
dPhidX * dydY * dzdZ + dzdX * dPhidY * dydZ +
dydX * dzdY * dPhidZ - dzdX * dydY * dPhidZ -
dPhidX * dzdY * dydZ - dydX * dPhidY * dzdZ;
JDJ (i,j+1*numMapNodes) =
dxdX * dPhidY * dzdZ + dzdX * dxdY * dPhidZ +
dPhidX * dzdY * dxdZ - dzdX * dPhidY * dxdZ -
dxdX * dzdY * dPhidZ - dPhidX * dxdY * dzdZ;
JDJ (i,j+2*numMapNodes) =
dxdX * dydY * dPhidZ + dPhidX * dxdY * dydZ +
dydX * dPhidY * dxdZ - dPhidX * dydY * dxdZ -
dxdX * dPhidY * dydZ - dydX * dxdY * dPhidZ;
}
JDJ(i,3*numMapNodes) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
break;
}
}
}
inline void JacobianBasis::getJacobianGeneral(int nJacNodes, const fullMatrix<double> &gSMatX,
const fullMatrix<double> &gSMatY, const fullMatrix<double> &gSMatZ,
const fullMatrix<double> &nodesXYZ, fullVector<double> &jacobian) const
{
switch (_dim) {
case 0 : {
for (int i = 0; i < nJacNodes; i++) jacobian(i) = 1.;
break;
}
case 1 : {
fullMatrix<double> normals(2,3);
const double invScale = getPrimNormals1D(nodesXYZ,normals);
if (scaling) {
const double scale = 1./invScale;
normals(0,0) *= scale; normals(0,1) *= scale; normals(0,2) *= scale; // Faster to scale 1 normal than afterwards
}
fullMatrix<double> dxyzdX(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = normals(0,0), &dydY = normals(0,1), &dzdY = normals(0,2);
const double &dxdZ = normals(1,0), &dydZ = normals(1,1), &dzdZ = normals(1,2);
jacobian(i) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
break;
}
case 2 : {
fullMatrix<double> normal(1,3);
const double invScale = getPrimNormal2D(nodesXYZ,normal);
if (scaling) {
const double scale = 1./invScale;
normal(0,0) *= scale; normal(0,1) *= scale; normal(0,2) *= scale; // Faster to scale normal than afterwards
}
fullMatrix<double> dxyzdX(nJacNodes,3), dxyzdY(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
gSMatY.mult(nodesXYZ, dxyzdY);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = dxyzdY(i,0), &dydY = dxyzdY(i,1), &dzdY = dxyzdY(i,2);
const double &dxdZ = normal(0,0), &dydZ = normal(0,1), &dzdZ = normal(0,2);
jacobian(i) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
break;
}
case 3 : {
fullMatrix<double> dum;
fullMatrix<double> dxyzdX(nJacNodes,3), dxyzdY(nJacNodes,3), dxyzdZ(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
gSMatY.mult(nodesXYZ, dxyzdY);
gSMatZ.mult(nodesXYZ, dxyzdZ);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = dxyzdY(i,0), &dydY = dxyzdY(i,1), &dzdY = dxyzdY(i,2);
const double &dxdZ = dxyzdZ(i,0), &dydZ = dxyzdZ(i,1), &dzdZ = dxyzdZ(i,2);
jacobian(i) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
if (scaling) {
const double scale = 1./getPrimJac3D(nodesXYZ);
jacobian.scale(scale);
}
break;
}
}
}
