public IntegrationPoints(IntegrationPoints src)
{
PointCount = src.PointCount;
Ls = null;
if (src.Ls != null)
{
Ls = new double[src.Ls.Length][];
for (int i = 0; i < src.Ls.Length; i++)
{
double[] srcPoint = src.Ls[i];
Ls[i] = new double[srcPoint.Length];
srcPoint.CopyTo(Ls[i], 0);
}
}
Weights = null;
if (src.Weights != null)
{
Weights = new double[src.Weights.Length];
src.Weights.CopyTo(Weights, 0);
}
}
protected void CalcSaintVenantHyperelasticElementAB(
uint feId, IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint quantityId = 0;
System.Diagnostics.Debug.Assert(World.GetDof(quantityId) == 2);
int dof = 2;
int nodeCnt = (int)World.GetNodeCount(quantityId);
double dt = TimeStep;
double beta = NewmarkBeta;
double gamma = NewmarkGamma;
var FV = World.GetFieldValue(ValueId);
TriangleFE triFE = World.GetTriangleFE(quantityId, feId);
Material ma0 = World.GetMaterial(triFE.MaterialId);
if (!(ma0 is SaintVenantHyperelasticMaterial))
{
return;
}
int[] coIds = triFE.NodeCoordIds;
uint elemNodeCnt = triFE.NodeCount;
int[] nodes = new int[elemNodeCnt];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = coIds[iNode];
int nodeId = World.Coord2Node(quantityId, coId);
nodes[iNode] = nodeId;
}
var ma = ma0 as SaintVenantHyperelasticMaterial;
double lambda = ma.LameLambda;
double mu = ma.LameMu;
double rho = ma.MassDensity;
double[] g = { ma.GravityX, ma.GravityY };
double[] sN = triFE.CalcSN();
double[,] sNN = triFE.CalcSNN();
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(TriangleIntegrationPointCount.Point1);
System.Diagnostics.Debug.Assert(ip.Ls.Length == 1);
double[] L = ip.Ls[0];
double[][] Nu = triFE.CalcNu(L);
double detJ = triFE.GetDetJacobian(L);
double weight = ip.Weights[0];
double detJWeight = (1.0 / 2.0) * weight * detJ;
double[,] uu = new double[dof, dof];
for (int iDof = 0; iDof < dof; iDof++)
{
for (int jDof = 0; jDof < dof; jDof++)
{
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int iNodeId = nodes[iNode];
if (iNodeId == -1)
{
continue;
}
uu[iDof, jDof] += U[iNodeId * dof + iDof] * Nu[jDof][iNode];
}
}
}
double[,] e = new double[dof, dof];
for (int iDof = 0; iDof < dof; iDof++)
{
for (int jDof = 0; jDof < dof; jDof++)
{
e[iDof, jDof] = (1.0 / 2.0) * (uu[iDof, jDof] + uu[jDof, iDof]);
for (int kDof = 0; kDof < dof; kDof++)
{
e[iDof, jDof] += (1.0 / 2.0) * uu[kDof, iDof] * uu[kDof, jDof];
}
}
}
double[,,,] b = new double[elemNodeCnt, dof, dof, dof];
{
double[,] f = new double[dof, dof];
for (int iDof = 0; iDof < dof; iDof++)
{
for (int jDof = 0; jDof < dof; jDof++)
{
f[iDof, jDof] = uu[iDof, jDof];
}
f[iDof, iDof] += 1.0;
}
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < dof; iDof++)
{
for (int gDof = 0; gDof < dof; gDof++)
{
for (int hDof = 0; hDof < dof; hDof++)
{
b[iNode, iDof, gDof, hDof] = Nu[hDof][iNode] * f[iDof, gDof];
}
}
}
}
}
double[,] s = new double[dof, dof];
{
double tmp = 0.0;
for (int iDof = 0; iDof < dof; iDof++)
{
for (int jDof = 0; jDof < dof; jDof++)
{
s[iDof, jDof] = 2.0 * mu * e[iDof, jDof];
}
tmp += e[iDof, iDof];
}
for (int iDof = 0; iDof < dof; iDof++)
{
s[iDof, iDof] += lambda * tmp;
}
}
double[,] q = new double[elemNodeCnt, dof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < dof; iDof++)
{
for (int gDof = 0; gDof < dof; gDof++)
{
for (int hDof = 0; hDof < dof; hDof++)
{
q[iNode, iDof] +=
detJWeight * s[gDof, hDof] * b[iNode, iDof, gDof, hDof];
}
}
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = nodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < elemNodeCnt; col++)
{
int colNodeId = nodes[col];
if (colNodeId == -1)
{
continue;
}
int colCoId = coIds[col];
double[] u = FV.GetDoubleValue(colCoId, FieldDerivativeType.Value);
double[] vel = FV.GetDoubleValue(colCoId, FieldDerivativeType.Velocity);
double[] acc = FV.GetDoubleValue(colCoId, FieldDerivativeType.Acceleration);
double[,] k = new double[dof, dof];
double[,] m = new double[dof, dof];
for (int rowDof = 0; rowDof < dof; rowDof++)
{
for (int colDof = 0; colDof < dof; colDof++)
{
{
double tmp1 = 0.0;
double tmp2 = 0.0;
for (int gDof = 0; gDof < dof; gDof++)
{
tmp1 += b[row, rowDof, gDof, gDof];
tmp2 += b[col, colDof, gDof, gDof];
}
k[rowDof, colDof] += detJWeight * lambda * tmp1 * tmp2;
}
{
double tmp = 0.0;
for (int gDof = 0; gDof < dof; gDof++)
{
for (int hDof = 0; hDof < dof; hDof++)
{
tmp +=
b[row, rowDof, gDof, hDof] * b[col, colDof, gDof, hDof] +
b[row, rowDof, gDof, hDof] * b[col, colDof, hDof, gDof];
}
}
k[rowDof, colDof] += detJWeight * mu * tmp;
}
}
}
{
double tmp = 0.0;
for (int gDof = 0; gDof < dof; gDof++)
{
for (int hDof = 0; hDof < dof; hDof++)
{
tmp += s[gDof, hDof] * Nu[hDof][row] * Nu[gDof][col];
}
}
for (int rowDof = 0; rowDof < dof; rowDof++)
{
k[rowDof, rowDof] += detJWeight * tmp;
}
}
m[0, 0] = rho * sNN[row, col];
m[1, 0] = 0.0;
m[0, 1] = 0.0;
m[1, 1] = rho * sNN[row, col];
for (int rowDof = 0; rowDof < dof; rowDof++)
{
for (int colDof = 0; colDof < dof; colDof++)
{
A[rowNodeId * dof + rowDof, colNodeId *dof + colDof] +=
(1.0 / (beta * dt * dt)) * m[rowDof, colDof] +
k[rowDof, colDof];
B[rowNodeId * dof + rowDof] +=
k[rowDof, colDof] * U[colNodeId * dof + colDof] +
m[rowDof, colDof] * (
(1.0 / (beta * dt * dt)) * u[colDof] +
(1.0 / (beta * dt)) * vel[colDof] +
(1.0 / (2.0 * beta) - 1.0) * acc[colDof]);
}
}
}
}
double[,] fg = new double[elemNodeCnt, dof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < dof; iDof++)
{
fg[iNode, iDof] += rho * g[iDof] * sN[iNode];
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = nodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < dof; rowDof++)
{
B[rowNodeId * dof + rowDof] += fg[row, rowDof] - q[row, rowDof];
}
}
}
private void CalcStokesAB(IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint vQuantityId = 0;
uint pQuantityId = 1;
int vDof = 2;
int pDof = 1;
int vNodeCnt = (int)World.GetNodeCount(vQuantityId);
int pNodeCnt = (int)World.GetNodeCount(pQuantityId);
int offset = vNodeCnt * vDof;
IList <uint> feIds = World.GetTriangleFEIds(vQuantityId);
foreach (uint feId in feIds)
{
TriangleFE vTriFE = World.GetTriangleFE(vQuantityId, feId);
TriangleFE pTriFE = World.GetTriangleFE(pQuantityId, feId);
uint vertexCnt = vTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(vTriFE.VertexCoordIds[iVertex] == pTriFE.VertexCoordIds[iVertex]);
}
int[] vCoIds = vTriFE.NodeCoordIds;
uint vElemNodeCnt = vTriFE.NodeCount;
int[] vNodes = new int[vElemNodeCnt];
for (int iNode = 0; iNode < vElemNodeCnt; iNode++)
{
int coId = vCoIds[iNode];
int nodeId = World.Coord2Node(vQuantityId, coId);
vNodes[iNode] = nodeId;
}
int[] pCoIds = pTriFE.NodeCoordIds;
uint pElemNodeCnt = pTriFE.NodeCount;
int[] pNodes = new int[pElemNodeCnt];
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int coId = pCoIds[iNode];
int nodeId = World.Coord2Node(pQuantityId, coId);
pNodes[iNode] = nodeId;
}
Material ma0 = World.GetMaterial(vTriFE.MaterialId);
System.Diagnostics.Debug.Assert(ma0 is NewtonFluidMaterial);
var ma = ma0 as NewtonFluidMaterial;
double rho = ma.MassDensity;
double mu = ma.Mu;
double[] g = { ma.GravityX, ma.GravityY };
double[] vSN = vTriFE.CalcSN();
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);//Point7
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] vN = vTriFE.CalcN(L);
double[][] vNu = vTriFE.CalcNu(L);
double[] vNx = vNu[0];
double[] vNy = vNu[1];
double[] pN = pTriFE.CalcN(L);
double[][] pNu = pTriFE.CalcNu(L);
double[] pNx = pNu[0];
double[] pNy = pNu[1];
double detJ = vTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < vElemNodeCnt; col++)
{
int colNodeId = vNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kvv1 = new double[vDof, vDof];
kvv1[0, 0] = detJWeight * mu * (vNx[row] * vNx[col] +
vNx[row] * vNx[col] + vNy[row] * vNy[col]);
kvv1[0, 1] = detJWeight * mu * vNy[row] * vNx[col];
kvv1[1, 0] = detJWeight * mu * vNx[row] * vNy[col];
kvv1[1, 1] = detJWeight * mu * (vNy[row] * vNy[col] +
vNx[row] * vNx[col] + vNy[row] * vNy[col]);
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
for (int colDof = 0; colDof < vDof; colDof++)
{
A[rowNodeId * vDof + rowDof, colNodeId *vDof + colDof] +=
kvv1[rowDof, colDof];
}
}
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kvp = new double[vDof, pDof];
kvp[0, 0] = -detJWeight * vNx[row] * pN[col];
kvp[1, 0] = -detJWeight * vNy[row] * pN[col];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
A[rowNodeId * vDof + rowDof, offset + colNodeId] += kvp[rowDof, 0];
A[offset + colNodeId, rowNodeId *vDof + rowDof] += kvp[rowDof, 0];
}
}
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
B[rowNodeId * vDof + rowDof] += rho * g[rowDof] * vSN[row];
}
}
}
}
protected void CalcOgdenOriginalRivlinIncompressibleHyperelasticElementAB(
uint feId, IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint uQuantityId = 0;
uint lQuantityId = 1;
System.Diagnostics.Debug.Assert(World.GetDof(uQuantityId) == 2);
System.Diagnostics.Debug.Assert(World.GetDof(lQuantityId) == 1);
int uDof = 2;
int lDof = 1;
int uNodeCnt = (int)World.GetNodeCount(uQuantityId);
int lNodeCnt = (int)World.GetNodeCount(lQuantityId);
//System.Diagnostics.Debug.Assert(uNodeCnt * uDof + lNodeCnt * lDof == A.RowLength);
int offset = GetOffset(lQuantityId);
System.Diagnostics.Debug.Assert(offset == uNodeCnt * uDof);
double dt = TimeStep;
double beta = NewmarkBeta;
double gamma = NewmarkGamma;
var uFV = World.GetFieldValue(UValueId);
TriangleFE uTriFE = World.GetTriangleFE(uQuantityId, feId);
TriangleFE lTriFE = World.GetTriangleFE(lQuantityId, feId);
Material ma0 = World.GetMaterial(uTriFE.MaterialId);
if (!(ma0 is OgdenHyperelasticMaterial))
{
return;
}
uint vertexCnt = uTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(uTriFE.VertexCoordIds[iVertex] == lTriFE.VertexCoordIds[iVertex]);
}
int[] uCoIds = uTriFE.NodeCoordIds;
uint uElemNodeCnt = uTriFE.NodeCount;
int[] uNodes = new int[uElemNodeCnt];
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
int coId = uCoIds[iNode];
int nodeId = World.Coord2Node(uQuantityId, coId);
uNodes[iNode] = nodeId;
}
int[] lCoIds = lTriFE.NodeCoordIds;
uint lElemNodeCnt = lTriFE.NodeCount;
int[] lNodes = new int[lElemNodeCnt];
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
int coId = lCoIds[iNode];
int nodeId = World.Coord2Node(lQuantityId, coId);
lNodes[iNode] = nodeId;
}
var ma = ma0 as OgdenHyperelasticMaterial;
int oOrder = ma.Order;
double[] oMus = ma.Mus;
double[] oAlphas = ma.Alphas;
double rho = ma.MassDensity;
double[] g = { ma.GravityX, ma.GravityY };
double[] uSN = uTriFE.CalcSN();
double[,] uSNN = uTriFE.CalcSNN();
System.Diagnostics.Debug.Assert((int)World.TriIntegrationPointCount >= 3);
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);
System.Diagnostics.Debug.Assert(ip.Ls.Length == (int)World.TriIntegrationPointCount);
double[,] qu = new double[uElemNodeCnt, uDof];
double[] ql = new double[lElemNodeCnt];
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] uN = uTriFE.CalcN(L);
double[] lN = lTriFE.CalcN(L);
double[][] uNu = uTriFE.CalcNu(L);
double detJ = uTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
// 変位の微分
double[,] uu = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
int iNodeId = uNodes[iNode];
if (iNodeId == -1)
{
continue;
}
uu[iDof, jDof] += U[iNodeId * uDof + iDof] * uNu[jDof][iNode];
}
}
}
// ラグランジュの未定乗数
double l = 0;
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
int iNodeId = lNodes[iNode];
if (iNodeId == -1)
{
continue;
}
l += U[offset + iNodeId] * lN[iNode];
}
// Green-Lagrangeのひずみ
double[,] e = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
e[iDof, jDof] = (1.0 / 2.0) * (uu[iDof, jDof] + uu[jDof, iDof]);
for (int kDof = 0; kDof < uDof; kDof++)
{
e[iDof, jDof] += (1.0 / 2.0) * uu[kDof, iDof] * uu[kDof, jDof];
}
}
}
// 変形勾配テンソル
double[,] f = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
f[iDof, jDof] = uu[iDof, jDof];
}
f[iDof, iDof] += 1.0;
}
// 右Cauchy-Green変形テンソル
double[,] c = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
c[iDof, jDof] = uu[iDof, jDof] + uu[jDof, iDof];
for (int kDof = 0; kDof < uDof; kDof++)
{
c[iDof, jDof] += uu[kDof, iDof] * uu[kDof, jDof];
}
}
c[iDof, iDof] += 1.0;
}
// Cのテンソル不変量
double I1 = c[0, 0] + c[1, 1] + 1.0;
double I2 = c[0, 0] * c[1, 1] + c[0, 0] + c[1, 1] - c[0, 1] * c[1, 0];
double I3 = c[0, 0] * c[1, 1] - c[0, 1] * c[1, 0];
double inv13I3 = 1.0 / Math.Pow(I3, 1.0 / 3.0);
double inv23I3 = 1.0 / Math.Pow(I3, 2.0 / 3.0);
double[,] invC = new double[uDof, uDof];
{
double invI3 = 1.0 / I3;
invC[0, 0] = invI3 * c[1, 1];
invC[0, 1] = -invI3 * c[0, 1];
invC[1, 0] = -invI3 * c[1, 0];
invC[1, 1] = invI3 * c[0, 0];
}
// Cの主値の1/2乗と主軸
// Note: これらは3次元
int dim3 = 3;
System.Numerics.Complex[] fLambdas;
System.Numerics.Complex[][] cNormals;
SolvePrincipalValues(c, out fLambdas, out cNormals);
double inv16I3 = 1.0 / Math.Pow(I3, 1.0 / 6.0);
// 主第2Piola-Kirchhoff応力
System.Numerics.Complex[] principalS = new System.Numerics.Complex[uDof];
for (int kDof = 0; kDof < uDof; kDof++)
{
for (int r = 0; r < oOrder; r++)
{
double mu = oMus[r];
double alpha = oAlphas[r];
principalS[kDof] += mu *
System.Numerics.Complex.Pow(fLambdas[kDof], alpha - 2);
}
}
// 第2Piola-Kirchhoff応力
double[,] s = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
for (int kDof = 0; kDof < uDof; kDof++)
{
System.Numerics.Complex sij =
principalS[kDof] * cNormals[kDof][iDof] * cNormals[kDof][jDof];
s[iDof, jDof] += sij.Real;
//System.Diagnostics.Debug.Assert(
// Math.Abs(sij.Imaginary) < IvyFEM.Constants.PrecisionLowerLimit);
}
}
}
// Lagrangeの未定乗数の寄与項
{
double tmp = 2.0 * l * I3;
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
s[iDof, jDof] += tmp * invC[iDof, jDof];
}
}
}
// 主軸構成則テンソル(オリジナルの方)
System.Numerics.Complex[,] principalC21st = new System.Numerics.Complex[uDof, uDof];
System.Numerics.Complex[,] principalC22nd = new System.Numerics.Complex[uDof, uDof];
for (int pDof = 0; pDof < uDof; pDof++)
{
for (int qDof = 0; qDof < uDof; qDof++)
{
for (int r = 0; r < oOrder; r++)
{
double mu = oMus[r];
double alpha = oAlphas[r];
System.Numerics.Complex tmp = 0;
if (pDof == qDof)
{
tmp = mu * (alpha - 2) *
System.Numerics.Complex.Pow(fLambdas[pDof], alpha - 4);
}
principalC21st[pDof, qDof] += tmp;
}
}
}
for (int pDof = 0; pDof < uDof; pDof++)
{
for (int qDof = 0; qDof < uDof; qDof++)
{
for (int r = 0; r < oOrder; r++)
{
double mu = oMus[r];
double alpha = oAlphas[r];
if (pDof != qDof)
{
System.Numerics.Complex tmp = 0;
System.Numerics.Complex diffFLambda = fLambdas[pDof] - fLambdas[qDof];
if (diffFLambda.Magnitude >= IvyFEM.Constants.PrecisionLowerLimit)
{
// λp != λq
System.Numerics.Complex squareFLambdaP = fLambdas[pDof] * fLambdas[pDof];
System.Numerics.Complex squareFLambdaQ = fLambdas[qDof] * fLambdas[qDof];
tmp = (mu /
(squareFLambdaP - squareFLambdaQ)) * (
System.Numerics.Complex.Pow(fLambdas[pDof], alpha - 2) -
System.Numerics.Complex.Pow(fLambdas[qDof], alpha - 2));
}
else
{
// λp == λq
tmp = (1.0 / 2.0) * mu * (alpha - 2.0) *
System.Numerics.Complex.Pow(fLambdas[pDof], alpha - 4);
}
principalC22nd[pDof, qDof] += tmp;
}
}
}
}
// 構成則テンソル（オリジナルの方）
double[,,,] c4 = new double[uDof, uDof, uDof, uDof];
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
for (int pDof = 0; pDof < uDof; pDof++)
{
for (int qDof = 0; qDof < uDof; qDof++)
{
System.Numerics.Complex c4ghef =
principalC21st[pDof, qDof] *
cNormals[pDof][gDof] * cNormals[pDof][hDof] *
cNormals[qDof][eDof] * cNormals[qDof][fDof] +
principalC22nd[pDof, qDof] *
cNormals[pDof][gDof] * cNormals[qDof][hDof] *
(
cNormals[pDof][eDof] * cNormals[qDof][fDof] +
cNormals[qDof][eDof] * cNormals[pDof][fDof]
);
c4[gDof, hDof, eDof, fDof] += c4ghef.Real;
//System.Diagnostics.Debug.Assert(
// Math.Abs(c4ghef.Imaginary) < IvyFEM.Constants.PrecisionLowerLimit);
}
}
}
}
}
}
// 構成則テンソル
double[,,,] barC4 = new double[uDof, uDof, uDof, uDof];
// 圧力構成則テンソル
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
barC4[gDof, hDof, eDof, fDof] +=
4.0 * l * I3 * invC[gDof, hDof] * invC[eDof, fDof] -
2.0 * l * I3 * (
invC[gDof, eDof] * invC[fDof, hDof] +
invC[gDof, fDof] * invC[eDof, hDof]);
}
}
}
}
// 変位構成則テンソル
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
barC4[gDof, hDof, eDof, fDof] +=
(1.0 / 2.0) * (c4[gDof, hDof, eDof, fDof] + c4[gDof, hDof, fDof, eDof]);
}
}
}
}
double[,,,] b = new double[uElemNodeCnt, uDof, uDof, uDof];
{
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
b[iNode, iDof, gDof, hDof] = uNu[hDof][iNode] * f[iDof, gDof];
}
}
}
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < uElemNodeCnt; col++)
{
int colNodeId = uNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kuu = new double[uDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
double tmp1 = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
tmp1 += barC4[gDof, hDof, eDof, fDof] *
b[row, rowDof, eDof, fDof] *
b[col, colDof, gDof, hDof];
}
}
}
}
kuu[rowDof, colDof] += detJWeight * tmp1;
}
}
{
double tmp = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
tmp += s[gDof, hDof] * uNu[gDof][row] * uNu[hDof][col];
}
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
kuu[rowDof, rowDof] += detJWeight * tmp;
}
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
A[rowNodeId * uDof + rowDof, colNodeId *uDof + colDof] +=
kuu[rowDof, colDof];
B[rowNodeId * uDof + rowDof] +=
kuu[rowDof, colDof] * U[colNodeId * uDof + colDof];
}
}
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < lElemNodeCnt; col++)
{
int colNodeId = lNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kul = new double[uDof, lDof];
double[,] klu = new double[lDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
double tmp = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
tmp += invC[gDof, hDof] * b[row, rowDof, gDof, hDof];
}
}
kul[rowDof, 0] +=
detJWeight * tmp * 2.0 * lN[col] * I3;
klu[0, rowDof] +=
detJWeight * tmp * 2.0 * lN[col] * I3;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
A[rowNodeId * uDof + rowDof, offset + colNodeId] += kul[rowDof, 0];
A[offset + colNodeId, rowNodeId *uDof + rowDof] += klu[0, rowDof];
B[rowNodeId * uDof + rowDof] +=
kul[rowDof, 0] * U[offset + colNodeId];
B[offset + colNodeId] +=
klu[0, rowDof] * U[rowNodeId * uDof + rowDof];
}
}
}
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
qu[iNode, iDof] += detJWeight * s[gDof, hDof] * b[iNode, iDof, gDof, hDof];
}
}
}
}
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
ql[iNode] += detJWeight * lN[iNode] * (I3 - 1);
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < uElemNodeCnt; col++)
{
int colNodeId = uNodes[col];
if (colNodeId == -1)
{
continue;
}
int colCoId = uCoIds[col];
double[] u = uFV.GetDoubleValue(colCoId, FieldDerivativeType.Value);
double[] velU = uFV.GetDoubleValue(colCoId, FieldDerivativeType.Velocity);
double[] accU = uFV.GetDoubleValue(colCoId, FieldDerivativeType.Acceleration);
double[,] m = new double[2, 2];
m[0, 0] = rho * uSNN[row, col];
m[1, 0] = 0.0;
m[0, 1] = 0.0;
m[1, 1] = rho * uSNN[row, col];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
A[rowNodeId * uDof + rowDof, colNodeId *uDof + colDof] +=
(1.0 / (beta * dt * dt)) * m[rowDof, colDof];
B[rowNodeId * uDof + rowDof] +=
m[rowDof, colDof] * (
(1.0 / (beta * dt * dt)) * u[colDof] +
(1.0 / (beta * dt)) * velU[colDof] +
(1.0 / (2.0 * beta) - 1.0) * accU[colDof]);
}
}
}
}
double[,] fg = new double[uElemNodeCnt, uDof];
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
fg[iNode, iDof] = rho * g[iDof] * uSN[iNode];
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
B[rowNodeId * uDof + rowDof] += fg[row, rowDof] - qu[row, rowDof];
}
}
for (int row = 0; row < lElemNodeCnt; row++)
{
int rowNodeId = lNodes[row];
if (rowNodeId == -1)
{
continue;
}
B[offset + rowNodeId] += -ql[row];
}
}
private void CalcGLSNavierStokesByPicardAB(IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint vQuantityId = 0;
uint pQuantityId = 1;
int vDof = 2;
int pDof = 1;
int vNodeCnt = (int)World.GetNodeCount(vQuantityId);
int pNodeCnt = (int)World.GetNodeCount(pQuantityId);
int offset = vNodeCnt * vDof;
IList <uint> feIds = World.GetTriangleFEIds(vQuantityId);
foreach (uint feId in feIds)
{
TriangleFE vTriFE = World.GetTriangleFE(vQuantityId, feId);
TriangleFE pTriFE = World.GetTriangleFE(pQuantityId, feId);
uint vertexCnt = vTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(vTriFE.VertexCoordIds[iVertex] == pTriFE.VertexCoordIds[iVertex]);
}
int[] vCoIds = vTriFE.NodeCoordIds;
uint vElemNodeCnt = vTriFE.NodeCount;
int[] vNodes = new int[vElemNodeCnt];
for (int iNode = 0; iNode < vElemNodeCnt; iNode++)
{
int coId = vCoIds[iNode];
int nodeId = World.Coord2Node(vQuantityId, coId);
vNodes[iNode] = nodeId;
}
int[] pCoIds = pTriFE.NodeCoordIds;
uint pElemNodeCnt = pTriFE.NodeCount;
int[] pNodes = new int[pElemNodeCnt];
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int coId = pCoIds[iNode];
int nodeId = World.Coord2Node(pQuantityId, coId);
pNodes[iNode] = nodeId;
}
Material ma0 = World.GetMaterial(vTriFE.MaterialId);
System.Diagnostics.Debug.Assert(ma0 is NewtonFluidMaterial);
var ma = ma0 as NewtonFluidMaterial;
double rho = ma.MassDensity;
double mu = ma.Mu;
double nu = mu / rho;
double[] g = { ma.GravityX, ma.GravityY };
double[][] velos = new double[vElemNodeCnt][];
for (int iNode = 0; iNode < vElemNodeCnt; iNode++)
{
double[] velo = new double[vDof];
int nodeId = vNodes[iNode];
if (nodeId == -1)
{
// 0
}
else
{
for (int iDof = 0; iDof < vDof; iDof++)
{
velo[iDof] = U[nodeId * vDof + iDof];
}
}
velos[iNode] = velo;
}
/*
* double taum = 0;
* double tauc = 0;
* {
* double[] aveVelo = {
* (velos[0][0] + velos[1][0] + velos[2][0]) / 3.0,
* (velos[0][1] + velos[1][1] + velos[2][1]) / 3.0
* };
* double veloNorm = Math.Sqrt(aveVelo[0] * aveVelo[0] + aveVelo[1] * aveVelo[1]);
* double Ae = vTriFE.GetArea();
* double h = 2.0 * Math.Sqrt(Ae / Math.PI);
* double sqinvtaum1 = 0;
* double sqinvtaum2 = (2.0 * 2.0 * veloNorm * veloNorm) / (h * h);
* double sqinvtaum3 = (4.0 * 4.0 * nu * nu) / (h * h * h * h);
* double sqinvtaum = sqinvtaum1 + sqinvtaum2 + sqinvtaum3;
* taum = 1.0 / Math.Sqrt(sqinvtaum);
*
* double re = veloNorm * h / (2.0 * nu);
* if (re < 3.0)
* {
* tauc = (1.0 / 2.0) * h * veloNorm * re / 3.0;
* }
* else
* {
* tauc = (1.0 / 2.0) * h * veloNorm;
* }
* }
*/
double taum = 0;
double tauc = 0;
{
double[] aveVelo =
{
(velos[0][0] + velos[1][0] + velos[2][0]) / 3.0,
(velos[0][1] + velos[1][1] + velos[2][1]) / 3.0
};
double veloNorm = Math.Sqrt(aveVelo[0] * aveVelo[0] + aveVelo[1] * aveVelo[1]);
double[][] Lu = new double[vDof][];
{
double[] a;
double[] b;
double[] c;
vTriFE.CalcTransMatrix(out a, out b, out c);
// Lx
Lu[0] = b;
// Ly
Lu[1] = c;
}
IvyFEM.Lapack.DoubleMatrix GMat = new IvyFEM.Lapack.DoubleMatrix(vDof, vDof);
double[] gVec = new double[vDof];
for (int iDof = 0; iDof < vDof; iDof++)
{
for (int jDof = 0; jDof < vDof; jDof++)
{
for (int kDof = 0; kDof < vDof; kDof++)
{
GMat[iDof, jDof] += Lu[iDof][kDof] * Lu[jDof][kDof];
}
}
}
for (int iDof = 0; iDof < vDof; iDof++)
{
for (int kDof = 0; kDof < vDof; kDof++)
{
gVec[iDof] += Lu[iDof][kDof];
}
}
double sqinvtaum1 = 0;
double sqinvtaum2 = 0;
{
double[] tmpVec = GMat * aveVelo;
sqinvtaum2 = IvyFEM.Lapack.Functions.ddot(aveVelo, tmpVec);
}
double sqinvtaum3 = 0;
{
IvyFEM.Lapack.DoubleMatrix GMatT = new Lapack.DoubleMatrix(GMat);
GMatT.Transpose();
double GMatDoubleDot = IvyFEM.Lapack.DoubleMatrix.DoubleDot(GMat, GMatT);
sqinvtaum3 = 30.0 * nu * nu * GMatDoubleDot;
}
double sqinvtaum = sqinvtaum1 + sqinvtaum2 + sqinvtaum3;
taum = 1.0 / Math.Sqrt(sqinvtaum);
double gDot = IvyFEM.Lapack.Functions.ddot(gVec, gVec);
tauc = 1.0 / (taum * gDot);
}
double[] vSN = vTriFE.CalcSN();
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);//Point7
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] vN = vTriFE.CalcN(L);
double[][] vNu = vTriFE.CalcNu(L);
double[] vNx = vNu[0];
double[] vNy = vNu[1];
double[, ][] vNuv = vTriFE.CalcNuv(L);
double[] pN = pTriFE.CalcN(L);
double[][] pNu = pTriFE.CalcNu(L);
double[] pNx = pNu[0];
double[] pNy = pNu[1];
double detJ = vTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
double[] v = new double[vDof];
double[] vx = new double[vDof];
double[] vy = new double[vDof];
double[] vxx = new double[vDof];
double[] vxy = new double[vDof];
double[] vyx = new double[vDof];
double[] vyy = new double[vDof];
double p = 0;
double px = 0;
double py = 0;
for (int iNode = 0; iNode < vElemNodeCnt; iNode++)
{
int nodeId = vNodes[iNode];
if (nodeId == -1)
{
continue;
}
for (int iDof = 0; iDof < vDof; iDof++)
{
double vValue = U[nodeId * vDof + iDof];
v[iDof] += vValue * vN[iNode];
vx[iDof] += vValue * vNx[iNode];
vy[iDof] += vValue * vNy[iNode];
vxx[iDof] += vValue * vNuv[0, 0][iNode];
vxy[iDof] += vValue * vNuv[0, 1][iNode];
vyx[iDof] += vValue * vNuv[1, 0][iNode];
vyy[iDof] += vValue * vNuv[1, 1][iNode];
}
}
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int nodeId = pNodes[iNode];
if (nodeId == -1)
{
continue;
}
double pValue = U[offset + nodeId];
p += pValue * pN[iNode];
px += pValue * pNx[iNode];
py += pValue * pNy[iNode];
}
double[][] vu = new double[vDof][];
vu[0] = vx;
vu[1] = vy;
double[, ][] vuv = new double[vDof, vDof][];
vuv[0, 0] = vxx;
vuv[0, 1] = vxy;
vuv[1, 0] = vyx;
vuv[1, 1] = vyy;
double[] pu = new double[vDof];
pu[0] = px;
pu[1] = py;
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < vElemNodeCnt; col++)
{
int colNodeId = vNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kvv1 = new double[vDof, vDof];
kvv1[0, 0] = detJWeight * mu * (vNx[row] * vNx[col] +
vNx[row] * vNx[col] + vNy[row] * vNy[col]);
kvv1[0, 1] = detJWeight * mu * vNy[row] * vNx[col];
kvv1[1, 0] = detJWeight * mu * vNx[row] * vNy[col];
kvv1[1, 1] = detJWeight * mu * (vNy[row] * vNy[col] +
vNx[row] * vNx[col] + vNy[row] * vNy[col]);
double[,] kvv2 = new double[vDof, vDof];
//kvv2[0, 0] = detJWeight * rho * vN[row] * (
// vN[col] * vx[0] + v[0] * vNx[col] + v[1] * vNy[col]);
//kvv2[0, 1] = detJWeight * rho * vN[row] * vN[col] * vy[0];
//kvv2[1, 0] = detJWeight * rho * vN[row] * vN[col] * vx[1];
//kvv2[1, 1] = detJWeight * rho * vN[row] * (
// vN[col] * vy[1] + v[0] * vNx[col] + v[1] * vNy[col]);
// Picard
kvv2[0, 0] = detJWeight * rho * vN[row] * (v[0] * vNx[col] + v[1] * vNy[col]);
kvv2[0, 1] = 0;
kvv2[1, 0] = 0;
kvv2[1, 1] = detJWeight * rho * vN[row] * (v[0] * vNx[col] + v[1] * vNy[col]);
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
for (int colDof = 0; colDof < vDof; colDof++)
{
A[rowNodeId * vDof + rowDof, colNodeId *vDof + colDof] +=
kvv1[rowDof, colDof] + kvv2[rowDof, colDof];
//B[rowNodeId * vDof + rowDof] +=
// kvv2[rowDof, colDof] * U[colNodeId * vDof + colDof];
// Picard
// nothing
}
}
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kvp = new double[vDof, pDof];
kvp[0, 0] = -detJWeight * vNx[row] * pN[col];
kvp[1, 0] = -detJWeight * vNy[row] * pN[col];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
A[rowNodeId * vDof + rowDof, offset + colNodeId] += kvp[rowDof, 0];
A[offset + colNodeId, rowNodeId *vDof + rowDof] += kvp[rowDof, 0];
}
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
double[] q2 = new double[vDof];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
//q2[rowDof] = detJWeight * rho * vN[row] * (v[0] * vx[rowDof] + v[1] * vy[rowDof]);
// Picard
// nothing
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
B[rowNodeId * vDof + rowDof] += -q2[rowDof];
}
}
//////////////////////////////////////////////////////////////
// SUPG
double[] rmi = new double[vDof];
double[,,] rmivj = new double[vDof, vDof, vElemNodeCnt];
double[,] rmip = new double[vDof, pElemNodeCnt];
double rc = 0;
double[,] rcvj = new double[vDof, vElemNodeCnt];
for (int iDof = 0; iDof < vDof; iDof++)
{
rmi[iDof] =
-mu * (vuv[0, 0][iDof] + vuv[1, 1][iDof]) +
rho * (v[0] * vx[iDof] + v[1] * vy[iDof]) +
pu[iDof] - rho * g[iDof];
for (int jDof = 0; jDof < vDof; jDof++)
{
for (int jNode = 0; jNode < vElemNodeCnt; jNode++)
{
int jNodeId = vNodes[jNode];
if (jNodeId == -1)
{
continue;
}
rmivj[iDof, jDof, jNode] = 0;
if (iDof == jDof)
{
rmivj[iDof, jDof, jNode] +=
-mu * (vNuv[0, 0][jNode] + vNuv[1, 1][jNode]);
}
//rmivj[iDof, jDof, jNode] +=
// rho * vN[jNode] * vu[jDof][iDof];
// Picard
// nothing
if (iDof == jDof)
{
rmivj[iDof, jDof, jNode] +=
rho * (v[0] * vNu[0][jNode] + v[1] * vNu[1][jNode]);
}
}
}
for (int jNode = 0; jNode < pElemNodeCnt; jNode++)
{
int jNodeId = pNodes[jNode];
if (jNodeId == -1)
{
continue;
}
rmip[iDof, jNode] = pNu[iDof][jNode];
}
}
{
rc = vx[0] + vy[1];
for (int jDof = 0; jDof < vDof; jDof++)
{
for (int jNode = 0; jNode < vElemNodeCnt; jNode++)
{
int jNodeId = vNodes[jNode];
if (jNodeId == -1)
{
continue;
}
rcvj[jDof, jNode] = vNu[jDof][jNode];
}
}
}
// kvv
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < vElemNodeCnt; col++)
{
int colNodeId = vNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kvv1 = new double[vDof, vDof];
double[,] kvv1GLSAdd = new double[vDof, vDof];
double[,] kvv2 = new double[vDof, vDof];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
for (int colDof = 0; colDof < vDof; colDof++)
{
// Picard
kvv1[rowDof, colDof] =
detJWeight * taum * (v[0] * vNu[0][row] + v[1] * vNu[1][row]) *
rmivj[rowDof, colDof, col];
kvv2[rowDof, colDof] =
detJWeight * tauc * rho * vNu[rowDof][row] * rcvj[colDof, col];
}
}
// GLS追加項
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
for (int colDof = 0; colDof < vDof; colDof++)
{
kvv1GLSAdd[rowDof, colDof] =
-detJWeight * (1.0 / rho) * taum * mu * (vNuv[0, 0][row] + vNuv[1, 1][row]) *
rmivj[rowDof, colDof, col];
}
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
for (int colDof = 0; colDof < vDof; colDof++)
{
A[rowNodeId * vDof + rowDof, colNodeId *vDof + colDof] +=
kvv1[rowDof, colDof] + kvv2[rowDof, colDof] + kvv1GLSAdd[rowDof, colDof];
// Picard
// nothing
}
}
}
}
// kvp
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kvp = new double[vDof, pDof];
double[,] kvpGLSAdd = new double[vDof, pDof];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
kvp[rowDof, 0] =
detJWeight * taum *
(v[0] * vNu[0][row] + v[1] * vNu[1][row]) * rmip[rowDof, col];
}
// GLS追加項
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
kvpGLSAdd[rowDof, 0] =
-detJWeight * (1.0 / rho) * taum * mu *
(vNuv[0, 0][row] + vNuv[1, 1][row]) * rmip[rowDof, col];
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
A[rowNodeId * vDof + rowDof, offset + colNodeId] +=
kvp[rowDof, 0] + kvpGLSAdd[rowDof, 0];
// Picard
// nothing
}
}
}
// kpv
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < vElemNodeCnt; col++)
{
int colNodeId = vNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kpv = new double[pDof, vDof];
for (int colDof = 0; colDof < vDof; colDof++)
{
kpv[0, colDof] =
-detJWeight * (1.0 / rho) * taum *
(pNu[0][row] * rmivj[0, colDof, col] + pNu[1][row] * rmivj[1, colDof, col]);
}
for (int colDof = 0; colDof < vDof; colDof++)
{
A[offset + rowNodeId, colNodeId *vDof + colDof] += kpv[0, colDof];
// Picard
// nothing
}
}
}
// kpp
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kpp = new double[pDof, pDof];
kpp[0, 0] =
-detJWeight * (1.0 / rho) * taum *
(pNu[0][row] * rmip[0, col] + pNu[1][row] * rmip[1, col]);
A[offset + rowNodeId, offset + colNodeId] += kpp[0, 0];
// Picard
// nothing
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
double[] qv1 = new double[vDof];
double[] qv2 = new double[vDof];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
// Picard
// nothing
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
B[rowNodeId * vDof + rowDof] += -(qv1[rowDof] + qv2[rowDof]);
}
}
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
double[] qp = new double[pDof];
// Picard
// nothing
B[offset + rowNodeId] += -qp[0];
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
B[rowNodeId * vDof + rowDof] += rho * g[rowDof] * vSN[row];
}
}
}
}
protected void CalcMooneyRivlinHyperelasticElementAB(
uint feId, IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint uQuantityId = 0;
uint lQuantityId = 1;
System.Diagnostics.Debug.Assert(World.GetDof(uQuantityId) == 2);
System.Diagnostics.Debug.Assert(World.GetDof(lQuantityId) == 1);
int uDof = 2;
int lDof = 1;
int uNodeCnt = (int)World.GetNodeCount(uQuantityId);
int lNodeCnt = (int)World.GetNodeCount(lQuantityId);
//System.Diagnostics.Debug.Assert(uNodeCnt * uDof + lNodeCnt * lDof == A.RowLength);
int offset = GetOffset(lQuantityId);
System.Diagnostics.Debug.Assert(offset == uNodeCnt * uDof);
double dt = TimeStep;
double beta = NewmarkBeta;
double gamma = NewmarkGamma;
var uFV = World.GetFieldValue(UValueId);
TriangleFE uTriFE = World.GetTriangleFE(uQuantityId, feId);
TriangleFE lTriFE = World.GetTriangleFE(lQuantityId, feId);
Material ma0 = World.GetMaterial(uTriFE.MaterialId);
if (!(ma0 is MooneyRivlinHyperelasticMaterial))
{
return;
}
uint vertexCnt = uTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(uTriFE.VertexCoordIds[iVertex] == lTriFE.VertexCoordIds[iVertex]);
}
int[] uCoIds = uTriFE.NodeCoordIds;
uint uElemNodeCnt = uTriFE.NodeCount;
int[] uNodes = new int[uElemNodeCnt];
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
int coId = uCoIds[iNode];
int nodeId = World.Coord2Node(uQuantityId, coId);
uNodes[iNode] = nodeId;
}
int[] lCoIds = lTriFE.NodeCoordIds;
uint lElemNodeCnt = lTriFE.NodeCount;
int[] lNodes = new int[lElemNodeCnt];
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
int coId = lCoIds[iNode];
int nodeId = World.Coord2Node(lQuantityId, coId);
lNodes[iNode] = nodeId;
}
var ma = ma0 as MooneyRivlinHyperelasticMaterial;
bool isCompressible = ma.IsCompressible;
double d1 = ma.D1;
double c1 = ma.C1;
double c2 = ma.C2;
double rho = ma.MassDensity;
double[] g = { ma.GravityX, ma.GravityY };
double[] uSN = uTriFE.CalcSN();
double[,] uSNN = uTriFE.CalcSNN();
double[,] lSNN = lTriFE.CalcSNN();
System.Diagnostics.Debug.Assert((int)World.TriIntegrationPointCount >= 3);
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);
System.Diagnostics.Debug.Assert(ip.Ls.Length == (int)World.TriIntegrationPointCount);
double[,] qu = new double[uElemNodeCnt, uDof];
double[] ql = new double[lElemNodeCnt];
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] uN = uTriFE.CalcN(L);
double[] lN = lTriFE.CalcN(L);
double[][] uNu = uTriFE.CalcNu(L);
double detJ = uTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
// 変位の微分
double[,] uu = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
int iNodeId = uNodes[iNode];
if (iNodeId == -1)
{
continue;
}
uu[iDof, jDof] += U[iNodeId * uDof + iDof] * uNu[jDof][iNode];
}
}
}
// ラグランジュの未定乗数
double l = 0;
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
int iNodeId = lNodes[iNode];
if (iNodeId == -1)
{
continue;
}
l += U[offset + iNodeId] * lN[iNode];
}
// Green-Lagrangeのひずみ
double[,] e = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
e[iDof, jDof] = (1.0 / 2.0) * (uu[iDof, jDof] + uu[jDof, iDof]);
for (int kDof = 0; kDof < uDof; kDof++)
{
e[iDof, jDof] += (1.0 / 2.0) * uu[kDof, iDof] * uu[kDof, jDof];
}
}
}
// 右Cauchy-Green変形テンソル
double[,] c = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
c[iDof, jDof] = uu[iDof, jDof] + uu[jDof, iDof];
for (int kDof = 0; kDof < uDof; kDof++)
{
c[iDof, jDof] += uu[kDof, iDof] * uu[kDof, jDof];
}
}
c[iDof, iDof] += 1.0;
}
// Cのテンソル不変量
double I1 = c[0, 0] + c[1, 1] + 1.0;
double I2 = c[0, 0] * c[1, 1] + c[0, 0] + c[1, 1] - c[0, 1] * c[1, 0];
double I3 = c[0, 0] * c[1, 1] - c[0, 1] * c[1, 0];
double inv13I3 = 1.0 / Math.Pow(I3, 1.0 / 3.0);
double inv23I3 = 1.0 / Math.Pow(I3, 2.0 / 3.0);
double[,] invC = new double[uDof, uDof];
{
double invI3 = 1.0 / I3;
invC[0, 0] = invI3 * c[1, 1];
invC[0, 1] = -invI3 * c[0, 1];
invC[1, 0] = -invI3 * c[1, 0];
invC[1, 1] = invI3 * c[0, 0];
}
// 第2Piola-Kirchhoff応力
double[,] s = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
s[iDof, jDof] -=
2.0 * c2 * inv23I3 * c[iDof, jDof] +
(2.0 / 3.0) * (c1 * I1 * inv13I3 + c2 * 2.0 * I2 * inv23I3) * invC[iDof, jDof];
}
}
{
double tmp = 2.0 * c1 * inv13I3 + 2.0 * c2 * inv23I3 * I1;
for (int iDof = 0; iDof < uDof; iDof++)
{
s[iDof, iDof] += tmp;
}
}
// Lagrangeの未定乗数の寄与項
{
double tmp = 2.0 * l * I3;
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
s[iDof, jDof] += tmp * invC[iDof, jDof];
}
}
}
// 構成則テンソル
double[,,,] c4 = new double[uDof, uDof, uDof, uDof];
// 圧力構成則テンソル
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
c4[gDof, hDof, eDof, fDof] +=
4.0 * l * I3 * invC[gDof, hDof] * invC[eDof, fDof] -
2.0 * l * I3 * (
invC[gDof, eDof] * invC[fDof, hDof] +
invC[gDof, fDof] * invC[eDof, hDof]);
}
}
}
}
// 変位構成則テンソル
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
c4[gDof, hDof, eDof, fDof] +=
4.0 * c1 * inv13I3 / 3.0 * (
invC[gDof, hDof] * invC[eDof, fDof] * I1 / 3.0 +
invC[gDof, eDof] * invC[fDof, hDof] * I1 / 2.0 +
invC[gDof, fDof] * invC[eDof, hDof] * I1 / 2.0) +
4.0 * c2 * inv23I3 * 2.0 / 3.0 * (
invC[gDof, hDof] * invC[eDof, fDof] * I2 * (2.0 / 3.0) +
c[gDof, hDof] * invC[eDof, fDof] +
invC[gDof, hDof] * c[eDof, fDof] +
invC[gDof, eDof] * invC[fDof, hDof] * I2 / 2.0 +
invC[gDof, fDof] * invC[eDof, hDof] * I2 / 2.0);
}
}
double tmp = 4.0 * c1 * inv13I3 / 3.0 * invC[gDof, hDof] +
4.0 * c2 * inv23I3 * I1 * (2.0 / 3.0) * invC[gDof, hDof];
for (int eDof = 0; eDof < uDof; eDof++)
{
c4[gDof, hDof, eDof, eDof] -= tmp;
c4[eDof, eDof, gDof, hDof] -= tmp;
}
c4[gDof, gDof, hDof, hDof] += 4.0 * c2 * inv23I3;
c4[gDof, hDof, hDof, gDof] -= 2.0 * c2 * inv23I3;
c4[gDof, hDof, gDof, hDof] -= 2.0 * c2 * inv23I3;
}
}
double[,,,] b = new double[uElemNodeCnt, uDof, uDof, uDof];
{
double[,] f = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
f[iDof, jDof] = uu[iDof, jDof];
}
f[iDof, iDof] += 1.0;
}
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
b[iNode, iDof, gDof, hDof] = uNu[hDof][iNode] * f[iDof, gDof];
}
}
}
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < uElemNodeCnt; col++)
{
int colNodeId = uNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kuu = new double[uDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
double tmp1 = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
tmp1 += c4[gDof, hDof, eDof, fDof] *
b[row, rowDof, eDof, fDof] *
b[col, colDof, gDof, hDof];
}
}
}
}
kuu[rowDof, colDof] += detJWeight * tmp1;
}
}
{
double tmp = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
tmp += s[gDof, hDof] * uNu[gDof][row] * uNu[hDof][col];
}
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
kuu[rowDof, rowDof] += detJWeight * tmp;
}
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
A[rowNodeId * uDof + rowDof, colNodeId *uDof + colDof] +=
kuu[rowDof, colDof];
B[rowNodeId * uDof + rowDof] +=
kuu[rowDof, colDof] * U[colNodeId * uDof + colDof];
}
}
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < lElemNodeCnt; col++)
{
int colNodeId = lNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kul = new double[uDof, lDof];
double[,] klu = new double[lDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
double tmp = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
tmp += invC[gDof, hDof] * b[row, rowDof, gDof, hDof];
}
}
kul[rowDof, 0] +=
detJWeight * tmp * 2.0 * lN[col] * I3;
klu[0, rowDof] +=
detJWeight * tmp * 2.0 * lN[col] * I3;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
A[rowNodeId * uDof + rowDof, offset + colNodeId] += kul[rowDof, 0];
A[offset + colNodeId, rowNodeId *uDof + rowDof] += klu[0, rowDof];
B[rowNodeId * uDof + rowDof] +=
kul[rowDof, 0] * U[offset + colNodeId];
B[offset + colNodeId] +=
klu[0, rowDof] * U[rowNodeId * uDof + rowDof];
}
}
}
// Note: kllは数値積分しなくて求められる
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
qu[iNode, iDof] += detJWeight * s[gDof, hDof] * b[iNode, iDof, gDof, hDof];
}
}
}
}
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
if (isCompressible)
{
ql[iNode] += detJWeight * lN[iNode] * ((I3 - 1) - l / d1);
}
else
{
ql[iNode] += detJWeight * lN[iNode] * (I3 - 1);
}
}
}
if (isCompressible)
{
for (int row = 0; row < lElemNodeCnt; row++)
{
int rowNodeId = lNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < lElemNodeCnt; col++)
{
int colNodeId = lNodes[col];
if (colNodeId == -1)
{
continue;
}
double kll = -(1.0 / d1) * lSNN[row, col];
A[offset + rowNodeId, offset + colNodeId] += kll;
B[offset + rowNodeId] += kll * U[offset + colNodeId];
}
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < uElemNodeCnt; col++)
{
int colNodeId = uNodes[col];
if (colNodeId == -1)
{
continue;
}
int colCoId = uCoIds[col];
double[] u = uFV.GetDoubleValue(colCoId, FieldDerivativeType.Value);
double[] velU = uFV.GetDoubleValue(colCoId, FieldDerivativeType.Velocity);
double[] accU = uFV.GetDoubleValue(colCoId, FieldDerivativeType.Acceleration);
double[,] m = new double[2, 2];
m[0, 0] = rho * uSNN[row, col];
m[1, 0] = 0.0;
m[0, 1] = 0.0;
m[1, 1] = rho * uSNN[row, col];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
A[rowNodeId * uDof + rowDof, colNodeId *uDof + colDof] +=
(1.0 / (beta * dt * dt)) * m[rowDof, colDof];
B[rowNodeId * uDof + rowDof] +=
m[rowDof, colDof] * (
(1.0 / (beta * dt * dt)) * u[colDof] +
(1.0 / (beta * dt)) * velU[colDof] +
(1.0 / (2.0 * beta) - 1.0) * accU[colDof]);
}
}
}
}
double[,] fg = new double[uElemNodeCnt, uDof];
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
fg[iNode, iDof] = rho * g[iDof] * uSN[iNode];
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
B[rowNodeId * uDof + rowDof] += fg[row, rowDof] - qu[row, rowDof];
}
}
for (int row = 0; row < lElemNodeCnt; row++)
{
int rowNodeId = lNodes[row];
if (rowNodeId == -1)
{
continue;
}
B[offset + rowNodeId] += -ql[row];
}
}
private void CalcStdGNavierStokesAB(IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint vQuantityId = 0;
uint pQuantityId = 1;
int vDof = 2;
int pDof = 1;
int vNodeCnt = (int)World.GetNodeCount(vQuantityId);
int pNodeCnt = (int)World.GetNodeCount(pQuantityId);
int offset = vNodeCnt * vDof;
double dt = TimeStep;
double beta = NewmarkBeta;
double gamma = NewmarkGamma;
var FV = World.GetFieldValue(ValueId);
IList <uint> feIds = World.GetTriangleFEIds(vQuantityId);
foreach (uint feId in feIds)
{
TriangleFE vTriFE = World.GetTriangleFE(vQuantityId, feId);
TriangleFE pTriFE = World.GetTriangleFE(pQuantityId, feId);
uint vertexCnt = vTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(vTriFE.VertexCoordIds[iVertex] == pTriFE.VertexCoordIds[iVertex]);
}
int[] vCoIds = vTriFE.NodeCoordIds;
uint vElemNodeCnt = vTriFE.NodeCount;
int[] vNodes = new int[vElemNodeCnt];
for (int iNode = 0; iNode < vElemNodeCnt; iNode++)
{
int coId = vCoIds[iNode];
int nodeId = World.Coord2Node(vQuantityId, coId);
vNodes[iNode] = nodeId;
}
int[] pCoIds = pTriFE.NodeCoordIds;
uint pElemNodeCnt = pTriFE.NodeCount;
int[] pNodes = new int[pElemNodeCnt];
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int coId = pCoIds[iNode];
int nodeId = World.Coord2Node(pQuantityId, coId);
pNodes[iNode] = nodeId;
}
Material ma0 = World.GetMaterial(vTriFE.MaterialId);
System.Diagnostics.Debug.Assert(ma0 is NewtonFluidMaterial);
var ma = ma0 as NewtonFluidMaterial;
double rho = ma.MassDensity;
double mu = ma.Mu;
double[] g = { ma.GravityX, ma.GravityY };
double[] vSN = vTriFE.CalcSN();
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);//Point7
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] vN = vTriFE.CalcN(L);
double[][] vNu = vTriFE.CalcNu(L);
double[] vNx = vNu[0];
double[] vNy = vNu[1];
double[] pN = pTriFE.CalcN(L);
double[][] pNu = pTriFE.CalcNu(L);
double[] pNx = pNu[0];
double[] pNy = pNu[1];
double detJ = vTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
double[] v = new double[vDof];
double[] vx = new double[vDof];
double[] vy = new double[vDof];
for (int iNode = 0; iNode < vElemNodeCnt; iNode++)
{
int nodeId = vNodes[iNode];
if (nodeId == -1)
{
continue;
}
for (int iDof = 0; iDof < vDof; iDof++)
{
double vValue = U[nodeId * vDof + iDof];
v[iDof] += vValue * vN[iNode];
vx[iDof] += vValue * vNx[iNode];
vy[iDof] += vValue * vNy[iNode];
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < vElemNodeCnt; col++)
{
int colNodeId = vNodes[col];
if (colNodeId == -1)
{
continue;
}
int colCoId = vCoIds[col];
double[] u = FV.GetDoubleValue(colCoId, FieldDerivativeType.Value);
double[] vel = FV.GetDoubleValue(colCoId, FieldDerivativeType.Velocity);
double[] acc = FV.GetDoubleValue(colCoId, FieldDerivativeType.Acceleration);
double[,] kvv1 = new double[vDof, vDof];
kvv1[0, 0] = detJWeight * mu * (vNx[row] * vNx[col] +
vNx[row] * vNx[col] + vNy[row] * vNy[col]);
kvv1[0, 1] = detJWeight * mu * vNy[row] * vNx[col];
kvv1[1, 0] = detJWeight * mu * vNx[row] * vNy[col];
kvv1[1, 1] = detJWeight * mu * (vNy[row] * vNy[col] +
vNx[row] * vNx[col] + vNy[row] * vNy[col]);
double[,] kvv2 = new double[vDof, vDof];
kvv2[0, 0] = detJWeight * rho * vN[row] * (
vN[col] * vx[0] + v[0] * vNx[col] + v[1] * vNy[col]);
kvv2[0, 1] = detJWeight * rho * vN[row] * vN[col] * vy[0];
kvv2[1, 0] = detJWeight * rho * vN[row] * vN[col] * vx[1];
kvv2[1, 1] = detJWeight * rho * vN[row] * (
vN[col] * vy[1] + v[0] * vNx[col] + v[1] * vNy[col]);
double[,] m = new double[vDof, vDof];
m[0, 0] = detJWeight * rho * vN[row] * vN[col];
m[1, 1] = detJWeight * rho * vN[row] * vN[col];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
for (int colDof = 0; colDof < vDof; colDof++)
{
A[rowNodeId * vDof + rowDof, colNodeId *vDof + colDof] +=
kvv1[rowDof, colDof] + kvv2[rowDof, colDof] +
(gamma / (beta * dt)) * m[rowDof, colDof];
B[rowNodeId * vDof + rowDof] +=
kvv2[rowDof, colDof] * U[colNodeId * vDof + colDof] +
m[rowDof, colDof] * (
(gamma / (beta * dt)) * u[colDof] -
(1.0 - gamma / beta) * vel[colDof] -
dt * (1.0 - gamma / (2.0 * beta)) * acc[colDof]
);
}
}
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kvp = new double[vDof, pDof];
kvp[0, 0] = -detJWeight * vNx[row] * pN[col];
kvp[1, 0] = -detJWeight * vNy[row] * pN[col];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
A[rowNodeId * vDof + rowDof, offset + colNodeId] += kvp[rowDof, 0];
A[offset + colNodeId, rowNodeId *vDof + rowDof] += kvp[rowDof, 0];
}
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
double[] q2 = new double[vDof];
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
q2[rowDof] = detJWeight * rho * vN[row] * (v[0] * vx[rowDof] + v[1] * vy[rowDof]);
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
B[rowNodeId * vDof + rowDof] += -q2[rowDof];
}
}
}
for (int row = 0; row < vElemNodeCnt; row++)
{
int rowNodeId = vNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < vDof; rowDof++)
{
B[rowNodeId * vDof + rowDof] += rho * g[rowDof] * vSN[row];
}
}
}
}
private void CalcSUPGVorticityAB(IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint wQuantityId = 0;
uint pQuantityId = 1;
int wNodeCnt = (int)World.GetNodeCount(wQuantityId);
int pNodeCnt = (int)World.GetNodeCount(pQuantityId);
int offset = wNodeCnt;
int vDof = 2; // 速度
double dt = TimeStep;
double beta = NewmarkBeta;
double gamma = NewmarkGamma;
var FV = World.GetFieldValue(ValueId);
IList <uint> feIds = World.GetTriangleFEIds(wQuantityId);
foreach (uint feId in feIds)
{
TriangleFE wTriFE = World.GetTriangleFE(wQuantityId, feId);
TriangleFE pTriFE = World.GetTriangleFE(pQuantityId, feId);
uint vertexCnt = wTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(wTriFE.VertexCoordIds[iVertex] == pTriFE.VertexCoordIds[iVertex]);
}
int[] wCoIds = wTriFE.NodeCoordIds;
uint wElemNodeCnt = wTriFE.NodeCount;
int[] wNodes = new int[wElemNodeCnt];
for (int iNode = 0; iNode < wElemNodeCnt; iNode++)
{
int coId = wCoIds[iNode];
int nodeId = World.Coord2Node(wQuantityId, coId);
wNodes[iNode] = nodeId;
}
int[] pCoIds = pTriFE.NodeCoordIds;
uint pElemNodeCnt = pTriFE.NodeCount;
int[] pNodes = new int[pElemNodeCnt];
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int coId = pCoIds[iNode];
int nodeId = World.Coord2Node(pQuantityId, coId);
pNodes[iNode] = nodeId;
}
Material ma0 = World.GetMaterial(wTriFE.MaterialId);
System.Diagnostics.Debug.Assert(ma0 is NewtonFluidMaterial);
var ma = ma0 as NewtonFluidMaterial;
double rho = ma.MassDensity;
double mu = ma.Mu;
double nu = mu / rho;
double[] g = { ma.GravityX, ma.GravityY };
double[][] velos = new double[pElemNodeCnt][];
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int coId = pCoIds[iNode];
double[] velo = new double[vDof];
for (int iDof = 0; iDof < vDof; iDof++)
{
velo[iDof] = CoordV[coId * vDof + iDof];
}
velos[iNode] = velo;
}
double taum = 0;
double tauc = 0;
{
double[] aveVelo =
{
(velos[0][0] + velos[1][0] + velos[2][0]) / 3.0,
(velos[0][1] + velos[1][1] + velos[2][1]) / 3.0
};
double veloNorm = Math.Sqrt(aveVelo[0] * aveVelo[0] + aveVelo[1] * aveVelo[1]);
double[][] Lu = new double[vDof][];
{
double[] a;
double[] b;
double[] c;
wTriFE.CalcTransMatrix(out a, out b, out c);
// Lx
Lu[0] = b;
// Ly
Lu[1] = c;
}
IvyFEM.Lapack.DoubleMatrix GMat = new IvyFEM.Lapack.DoubleMatrix(vDof, vDof);
double[] gVec = new double[vDof];
for (int iDof = 0; iDof < vDof; iDof++)
{
for (int jDof = 0; jDof < vDof; jDof++)
{
for (int kDof = 0; kDof < vDof; kDof++)
{
GMat[iDof, jDof] += Lu[iDof][kDof] * Lu[jDof][kDof];
}
}
}
for (int iDof = 0; iDof < vDof; iDof++)
{
for (int kDof = 0; kDof < vDof; kDof++)
{
gVec[iDof] += Lu[iDof][kDof];
}
}
double sqinvtaum1 = 0;
double sqinvtaum2 = 0;
{
double[] tmpVec = GMat * aveVelo;
sqinvtaum2 = IvyFEM.Lapack.Functions.ddot(aveVelo, tmpVec);
}
double sqinvtaum3 = 0;
{
IvyFEM.Lapack.DoubleMatrix GMatT = new Lapack.DoubleMatrix(GMat);
GMatT.Transpose();
double GMatDoubleDot = IvyFEM.Lapack.DoubleMatrix.DoubleDot(GMat, GMatT);
sqinvtaum3 = nu * nu * GMatDoubleDot;
}
double sqinvtaum = sqinvtaum1 + sqinvtaum2 + sqinvtaum3;
taum = 1.0 / Math.Sqrt(sqinvtaum);
double gDot = IvyFEM.Lapack.Functions.ddot(gVec, gVec);
tauc = 1.0 / (taum * gDot);
}
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);//Point7
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] wN = wTriFE.CalcN(L);
double[][] wNu = wTriFE.CalcNu(L);
double[] wNx = wNu[0];
double[] wNy = wNu[1];
double[, ][] wNuv = wTriFE.CalcNuv(L);
double[] pN = pTriFE.CalcN(L);
double[][] pNu = pTriFE.CalcNu(L);
double[] pNx = pNu[0];
double[] pNy = pNu[1];
double detJ = wTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
double w = 0;
double wx = 0;
double wy = 0;
double wxx = 0;
double wxy = 0;
double wyx = 0;
double wyy = 0;
double velow = 0;
for (int iNode = 0; iNode < wElemNodeCnt; iNode++)
{
int nodeId = wNodes[iNode];
if (nodeId == -1)
{
continue;
}
double wValue = U[nodeId];
w += wValue * wN[iNode];
wx += wValue * wNx[iNode];
wy += wValue * wNy[iNode];
wxx += wValue * wNuv[0, 0][iNode];
wxy += wValue * wNuv[0, 1][iNode];
wyx += wValue * wNuv[1, 0][iNode];
wyy += wValue * wNuv[1, 1][iNode];
int coId = wCoIds[iNode];
double[] prevU = FV.GetDoubleValue(coId, FieldDerivativeType.Value);
double[] prevVel = FV.GetDoubleValue(coId, FieldDerivativeType.Velocity);
double[] prevAcc = FV.GetDoubleValue(coId, FieldDerivativeType.Acceleration);
System.Diagnostics.Debug.Assert(prevU.Length == 1);
System.Diagnostics.Debug.Assert(prevVel.Length == 1);
System.Diagnostics.Debug.Assert(prevAcc.Length == 1);
double vel = 0;
vel = (gamma / (beta * dt)) * (wValue - prevU[0]) +
(1.0 - gamma / beta) * prevVel[0] +
dt * (1.0 - gamma / (2.0 * beta)) * prevAcc[0];
velow += vel * wN[iNode];
}
double p = 0;
double px = 0;
double py = 0;
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int nodeId = pNodes[iNode];
if (nodeId == -1)
{
continue;
}
double pValue = U[offset + nodeId];
p += pValue * pN[iNode];
px += pValue * pNx[iNode];
py += pValue * pNy[iNode];
}
// dg/du
double[] gx = new double[2];
double[] gy = new double[2];
for (int iNode = 0; iNode < wElemNodeCnt; iNode++)
{
int nodeId = wNodes[iNode];
if (nodeId == -1)
{
continue;
}
for (int iDof = 0; iDof < 2; iDof++)
{
double gValue = g[iDof];
gx[iDof] += gValue * wNx[iNode];
gy[iDof] += gValue * wNy[iNode];
}
}
double[] v = new double[2];
v[0] = py;
v[1] = -px;
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < wElemNodeCnt; col++)
{
int colNodeId = wNodes[col];
if (colNodeId == -1)
{
continue;
}
int colCoId = wCoIds[col];
// ω、dω/dt、d2ω/dt2
double[] u = FV.GetDoubleValue(colCoId, FieldDerivativeType.Value);
double[] vel = FV.GetDoubleValue(colCoId, FieldDerivativeType.Velocity);
double[] acc = FV.GetDoubleValue(colCoId, FieldDerivativeType.Acceleration);
System.Diagnostics.Debug.Assert(u.Length == 1);
System.Diagnostics.Debug.Assert(vel.Length == 1);
System.Diagnostics.Debug.Assert(acc.Length == 1);
double kww1 = detJWeight * mu * (wNx[row] * wNx[col] + wNy[row] * wNy[col]);
double kww2 = detJWeight * rho * wN[row] * (v[0] * wNx[col] + v[1] * wNy[col]);
double m = detJWeight * rho * wN[row] * wN[col];
A[rowNodeId, colNodeId] +=
kww1 + kww2 +
(gamma / (beta * dt)) * m;
// v = f(ψ) : kww2 Newton-Raphson
B[rowNodeId] += kww2 * U[colNodeId] +
m * (
(gamma / (beta * dt)) * u[0] -
(1.0 - gamma / beta) * vel[0] -
dt * (1.0 - gamma / (2.0 * beta)) * acc[0]
);
}
}
// v = f(ψ): kwp Newton-Raphson
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double kwp = detJWeight * rho * wN[row] * (pNy[col] * wx - pNx[col] * wy);
A[rowNodeId, offset + colNodeId] += kwp;
B[rowNodeId] += kwp * U[offset + colNodeId];
}
}
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < wElemNodeCnt; col++)
{
int colNodeId = wNodes[col];
if (colNodeId == -1)
{
continue;
}
double kpw = -detJWeight * pN[row] * wN[col];
A[offset + rowNodeId, colNodeId] += kpw;
}
}
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double kpp = detJWeight * (pNx[row] * pNx[col] + pNy[row] * pNy[col]);
A[offset + rowNodeId, offset + colNodeId] += kpp;
}
}
// v = f(ψ): qw Newton-Raphson
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
double qw = detJWeight * rho * wN[row] * (v[0] * wx + v[1] * wy);
B[rowNodeId] += -qw;
}
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
double f = 0;
for (int kNode = 0; kNode < wElemNodeCnt; kNode++)
{
int kNodeId = wNodes[kNode];
if (kNodeId == -1)
{
continue;
}
f += detJWeight * rho * (wNx[kNode] * g[1] - wNy[kNode] * g[0]);
}
B[rowNodeId] += f;
}
//////////////////////////////////////////////////////////////
// SUPG
double rm = 0;
double[] rmw = new double[wElemNodeCnt];
double[] rmp = new double[pElemNodeCnt];
{
rm =
rho * velow +
-mu * (wxx + wyy) +
rho * (v[0] * wx + v[1] * wy) +
-rho * (gx[1] - gy[0]);
for (int jNode = 0; jNode < wElemNodeCnt; jNode++)
{
int jNodeId = wNodes[jNode];
if (jNodeId == -1)
{
continue;
}
rmw[jNode] =
rho * (gamma / (beta * dt)) * wN[jNode] +
-mu * (wNuv[0, 0][jNode] + wNuv[1, 1][jNode]) +
rho * (v[0] * wNu[0][jNode] + v[1] * wNu[1][jNode]);
}
for (int jNode = 0; jNode < pElemNodeCnt; jNode++)
{
int jNodeId = pNodes[jNode];
if (jNodeId == -1)
{
continue;
}
rmp[jNode] = rho * (pNu[1][jNode] * wx - pNu[0][jNode] * wy);
}
}
// kww
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < wElemNodeCnt; col++)
{
int colNodeId = wNodes[col];
if (colNodeId == -1)
{
continue;
}
double kww =
detJWeight * taum * (v[0] * wNu[0][row] + v[1] * wNu[1][row]) * rmw[col];
A[rowNodeId, colNodeId] += kww;
B[rowNodeId] +=
kww * U[colNodeId];
}
}
// kwp
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double kwp =
detJWeight * taum * (pNu[1][col] * wNu[0][row] - pNu[0][col] * wNu[1][row]) * rm +
detJWeight * taum * (v[0] * wNu[0][row] + v[1] * wNu[1][row]) * rmp[col];
A[rowNodeId, offset + colNodeId] += kwp;
B[rowNodeId] +=
kwp * U[offset + colNodeId];
}
}
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
double qw = detJWeight * taum *
(v[0] * wNu[0][row] + v[1] * wNu[1][row]) * rm;
B[rowNodeId] += -qw;
}
}
}
}
private void CalcStdGVorticityAB(IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint wQuantityId = 0;
uint pQuantityId = 1;
int wNodeCnt = (int)World.GetNodeCount(wQuantityId);
int pNodeCnt = (int)World.GetNodeCount(pQuantityId);
int offset = wNodeCnt;
double dt = TimeStep;
double beta = NewmarkBeta;
double gamma = NewmarkGamma;
var FV = World.GetFieldValue(ValueId);
IList <uint> feIds = World.GetTriangleFEIds(wQuantityId);
foreach (uint feId in feIds)
{
TriangleFE wTriFE = World.GetTriangleFE(wQuantityId, feId);
TriangleFE pTriFE = World.GetTriangleFE(pQuantityId, feId);
uint vertexCnt = wTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(wTriFE.VertexCoordIds[iVertex] == pTriFE.VertexCoordIds[iVertex]);
}
int[] wCoIds = wTriFE.NodeCoordIds;
uint wElemNodeCnt = wTriFE.NodeCount;
int[] wNodes = new int[wElemNodeCnt];
for (int iNode = 0; iNode < wElemNodeCnt; iNode++)
{
int coId = wCoIds[iNode];
int nodeId = World.Coord2Node(wQuantityId, coId);
wNodes[iNode] = nodeId;
}
int[] pCoIds = pTriFE.NodeCoordIds;
uint pElemNodeCnt = pTriFE.NodeCount;
int[] pNodes = new int[pElemNodeCnt];
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int coId = pCoIds[iNode];
int nodeId = World.Coord2Node(pQuantityId, coId);
pNodes[iNode] = nodeId;
}
Material ma0 = World.GetMaterial(wTriFE.MaterialId);
System.Diagnostics.Debug.Assert(ma0 is NewtonFluidMaterial);
var ma = ma0 as NewtonFluidMaterial;
double rho = ma.MassDensity;
double mu = ma.Mu;
double[] g = { ma.GravityX, ma.GravityY };
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);//Point7
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] wN = wTriFE.CalcN(L);
double[][] wNu = wTriFE.CalcNu(L);
double[] wNx = wNu[0];
double[] wNy = wNu[1];
double[] pN = pTriFE.CalcN(L);
double[][] pNu = pTriFE.CalcNu(L);
double[] pNx = pNu[0];
double[] pNy = pNu[1];
double detJ = wTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
double w = 0;
double wx = 0;
double wy = 0;
for (int iNode = 0; iNode < wElemNodeCnt; iNode++)
{
int nodeId = wNodes[iNode];
if (nodeId == -1)
{
continue;
}
double wValue = U[nodeId];
w += wValue * wN[iNode];
wx += wValue * wNx[iNode];
wy += wValue * wNy[iNode];
}
double p = 0;
double px = 0;
double py = 0;
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int nodeId = pNodes[iNode];
if (nodeId == -1)
{
continue;
}
double pValue = U[offset + nodeId];
p += pValue * pN[iNode];
px += pValue * pNx[iNode];
py += pValue * pNy[iNode];
}
double[] v = new double[2];
v[0] = py;
v[1] = -px;
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < wElemNodeCnt; col++)
{
int colNodeId = wNodes[col];
if (colNodeId == -1)
{
continue;
}
int colCoId = wCoIds[col];
// ω、dω/dt、d2ω/dt2
double[] u = FV.GetDoubleValue(colCoId, FieldDerivativeType.Value);
double[] vel = FV.GetDoubleValue(colCoId, FieldDerivativeType.Velocity);
double[] acc = FV.GetDoubleValue(colCoId, FieldDerivativeType.Acceleration);
System.Diagnostics.Debug.Assert(u.Length == 1);
System.Diagnostics.Debug.Assert(vel.Length == 1);
System.Diagnostics.Debug.Assert(acc.Length == 1);
double kww1 = detJWeight * mu * (wNx[row] * wNx[col] + wNy[row] * wNy[col]);
double kww2 = detJWeight * rho * wN[row] * (v[0] * wNx[col] + v[1] * wNy[col]);
double m = detJWeight * rho * wN[row] * wN[col];
A[rowNodeId, colNodeId] +=
kww1 + kww2 +
(gamma / (beta * dt)) * m;
// v = f(ψ) : kww2 Newton-Raphson
B[rowNodeId] += kww2 * U[colNodeId] +
m * (
(gamma / (beta * dt)) * u[0] -
(1.0 - gamma / beta) * vel[0] -
dt * (1.0 - gamma / (2.0 * beta)) * acc[0]
);
}
}
// v = f(ψ): kwp Newton-Raphson
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double kwp = detJWeight * rho * wN[row] * (pNy[col] * wx - pNx[col] * wy);
A[rowNodeId, offset + colNodeId] += kwp;
B[rowNodeId] += kwp * U[offset + colNodeId];
}
}
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < wElemNodeCnt; col++)
{
int colNodeId = wNodes[col];
if (colNodeId == -1)
{
continue;
}
double kpw = -detJWeight * pN[row] * wN[col];
A[offset + rowNodeId, colNodeId] += kpw;
}
}
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double kpp = detJWeight * (pNx[row] * pNx[col] + pNy[row] * pNy[col]);
A[offset + rowNodeId, offset + colNodeId] += kpp;
}
}
// v = f(ψ): qw Newton-Raphson
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
double qw = detJWeight * rho * wN[row] * (v[0] * wx + v[1] * wy);
B[rowNodeId] += -qw;
}
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
double f = 0;
for (int kNode = 0; kNode < wElemNodeCnt; kNode++)
{
int kNodeId = wNodes[kNode];
if (kNodeId == -1)
{
continue;
}
f += detJWeight * rho * (wNx[kNode] * g[1] - wNy[kNode] * g[0]);
}
B[rowNodeId] += f;
}
}
}
}
private void CalcStdGVorticityByPicardAB(IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint wQuantityId = 0;
uint pQuantityId = 1;
int wNodeCnt = (int)World.GetNodeCount(wQuantityId);
int pNodeCnt = (int)World.GetNodeCount(pQuantityId);
int offset = wNodeCnt;
IList <uint> feIds = World.GetTriangleFEIds(wQuantityId);
foreach (uint feId in feIds)
{
TriangleFE wTriFE = World.GetTriangleFE(wQuantityId, feId);
TriangleFE pTriFE = World.GetTriangleFE(pQuantityId, feId);
uint vertexCnt = wTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(wTriFE.VertexCoordIds[iVertex] == pTriFE.VertexCoordIds[iVertex]);
}
int[] wCoIds = wTriFE.NodeCoordIds;
uint wElemNodeCnt = wTriFE.NodeCount;
int[] wNodes = new int[wElemNodeCnt];
for (int iNode = 0; iNode < wElemNodeCnt; iNode++)
{
int coId = wCoIds[iNode];
int nodeId = World.Coord2Node(wQuantityId, coId);
wNodes[iNode] = nodeId;
}
int[] pCoIds = pTriFE.NodeCoordIds;
uint pElemNodeCnt = pTriFE.NodeCount;
int[] pNodes = new int[pElemNodeCnt];
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int coId = pCoIds[iNode];
int nodeId = World.Coord2Node(pQuantityId, coId);
pNodes[iNode] = nodeId;
}
Material ma0 = World.GetMaterial(wTriFE.MaterialId);
System.Diagnostics.Debug.Assert(ma0 is NewtonFluidMaterial);
var ma = ma0 as NewtonFluidMaterial;
double rho = ma.MassDensity;
double mu = ma.Mu;
double[] g = { ma.GravityX, ma.GravityY };
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);//Point7
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] wN = wTriFE.CalcN(L);
double[][] wNu = wTriFE.CalcNu(L);
double[] wNx = wNu[0];
double[] wNy = wNu[1];
double[] pN = pTriFE.CalcN(L);
double[][] pNu = pTriFE.CalcNu(L);
double[] pNx = pNu[0];
double[] pNy = pNu[1];
double detJ = wTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
double w = 0;
double wx = 0;
double wy = 0;
for (int iNode = 0; iNode < wElemNodeCnt; iNode++)
{
int nodeId = wNodes[iNode];
if (nodeId == -1)
{
continue;
}
double wValue = U[nodeId];
w += wValue * wN[iNode];
wx += wValue * wNx[iNode];
wy += wValue * wNy[iNode];
}
double p = 0;
double px = 0;
double py = 0;
for (int iNode = 0; iNode < pElemNodeCnt; iNode++)
{
int nodeId = pNodes[iNode];
if (nodeId == -1)
{
continue;
}
double pValue = U[offset + nodeId];
p += pValue * pN[iNode];
px += pValue * pNx[iNode];
py += pValue * pNy[iNode];
}
double[] v = new double[2];
v[0] = py;
v[1] = -px;
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < wElemNodeCnt; col++)
{
int colNodeId = wNodes[col];
if (colNodeId == -1)
{
continue;
}
double kww1 = detJWeight * mu * (wNx[row] * wNx[col] + wNy[row] * wNy[col]);
double kww2 = detJWeight * rho * wN[row] * (v[0] * wNx[col] + v[1] * wNy[col]);
A[rowNodeId, colNodeId] +=
kww1 + kww2;
//Picard
//// v = f(ψ) : kww2 Newton-Raphson
///B[rowNodeId] += kww2 * U[colNodeId];
}
}
// Picard
//// v = f(ψ): kwp Newton-Raphson
//for (int row = 0; row < wElemNodeCnt; row++)
//{
// int rowNodeId = wNodes[row];
// if (rowNodeId == -1)
// {
// continue;
// }
// for (int col = 0; col < pElemNodeCnt; col++)
// {
// int colNodeId = pNodes[col];
// if (colNodeId == -1)
// {
// continue;
// }
//
// double kwp = detJWeight * rho * wN[row] * (pNy[col] * wx - pNx[col] * wy);
// A[rowNodeId, offset + colNodeId] += kwp;
// B[rowNodeId] += kwp * U[offset + colNodeId];
// }
//}
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < wElemNodeCnt; col++)
{
int colNodeId = wNodes[col];
if (colNodeId == -1)
{
continue;
}
double kpw = -detJWeight * pN[row] * wN[col];
A[offset + rowNodeId, colNodeId] += kpw;
}
}
for (int row = 0; row < pElemNodeCnt; row++)
{
int rowNodeId = pNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < pElemNodeCnt; col++)
{
int colNodeId = pNodes[col];
if (colNodeId == -1)
{
continue;
}
double kpp = detJWeight * (pNx[row] * pNx[col] + pNy[row] * pNy[col]);
A[offset + rowNodeId, offset + colNodeId] += kpp;
}
}
// Picard
//// v = f(ψ): qw Newton-Raphson
//for (int row = 0; row < wElemNodeCnt; row++)
//{
// int rowNodeId = wNodes[row];
// if (rowNodeId == -1)
// {
// continue;
// }
// double qw = detJWeight * rho * wN[row] * (v[0] * wx + v[1] * wy);
// B[rowNodeId] += -qw;
//}
for (int row = 0; row < wElemNodeCnt; row++)
{
int rowNodeId = wNodes[row];
if (rowNodeId == -1)
{
continue;
}
double f = 0;
for (int kNode = 0; kNode < wElemNodeCnt; kNode++)
{
int kNodeId = wNodes[kNode];
if (kNodeId == -1)
{
continue;
}
f += detJWeight * rho * (wNx[kNode] * g[1] - wNy[kNode] * g[0]);
}
B[rowNodeId] += f;
}
}
}
}
/*
* private void CalcTwoBodyContactMortarSegmentationQuantityAB(
* uint cQuantityId, IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
* {
* uint uQuantityId = 0;
* System.Diagnostics.Debug.Assert(World.GetFEOrder(uQuantityId) == 1);
* System.Diagnostics.Debug.Assert(World.GetCoordCount(uQuantityId) ==
* World.GetCoordCount(cQuantityId));
* System.Diagnostics.Debug.Assert(World.GetDof(uQuantityId) == 2);
* System.Diagnostics.Debug.Assert(World.GetDof(cQuantityId) == 2);
* int uDof = 2;
* int cDof = 2;
* int uNodeCnt = (int)World.GetNodeCount(uQuantityId);
* int cNodeCnt = (int)World.GetNodeCount(cQuantityId);
* int offset = GetOffset(cQuantityId);
*
* // 線要素の変位を更新
* UpdateLineFEDisplacements(uQuantityId, uDof, cQuantityId);
*
* // 節点法線ベクトルの計算
* Dictionary<int, double[]> co2Normal = GetSlaveLineFECo2Normal(uQuantityId, uDof, cQuantityId);
*
* // MasterからSlaveに垂らした点を計算する
* Dictionary<uint, IList<double>> slaveFEL2s = GetSlavePointFromMasterNodes(
* co2Normal,
* uQuantityId, uDof, cQuantityId);
*
* bool[] lConstraintNodeIds = new bool[cNodeCnt];
* IList<uint> slaveFEIds = World.GetContactSlaveLineFEIds(cQuantityId);
* foreach (uint slaveFEId in slaveFEIds)
* {
* LineFE lineFE = World.GetLineFE(uQuantityId, slaveFEId);
* uint elemNodeCnt = lineFE.NodeCount;
* int[] nodes = new int[elemNodeCnt];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int coId = lineFE.NodeCoordIds[iNode];
* int nodeId = World.Coord2Node(uQuantityId, coId);
* nodes[iNode] = nodeId;
* }
*
* LineFE lLineFE = World.GetLineFE(cQuantityId, slaveFEId);
* int[] lNodes = new int[elemNodeCnt];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int coId = lLineFE.NodeCoordIds[iNode];
* int lNodeId = World.Coord2Node(cQuantityId, coId);
* lNodes[iNode] = lNodeId;
* }
*
* double[][] curNodeCoord = new double[elemNodeCnt][];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int coId = lineFE.NodeCoordIds[iNode];
* double[] coord = World.GetCoord(uQuantityId, coId);
* int iNodeId = nodes[iNode];
* curNodeCoord[iNode] = new double[uDof];
* if (iNodeId == -1)
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* curNodeCoord[iNode][iDof] = coord[iDof];
* }
* }
* else
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* curNodeCoord[iNode][iDof] = coord[iDof] + U[iNodeId * uDof + iDof];
* }
* }
* }
*
* //IntegrationPoints ip = LineFE.GetIntegrationPoints(LineIntegrationPointCount.Point3);
* //System.Diagnostics.Debug.Assert(ip.Ls.Length == 3);
* IntegrationPoints ip = LineFE.GetIntegrationPoints(LineIntegrationPointCount.Point5);
* System.Diagnostics.Debug.Assert(ip.Ls.Length == 5);
*
* IList<double> L2s = new List<double>();
* L2s.Add(0.0);
* if (slaveFEL2s.ContainsKey(slaveFEId))
* {
* foreach (double L2 in slaveFEL2s[slaveFEId])
* {
* L2s.Add(L2);
* }
* }
* L2s.Add(1.0);
* int segCnt = L2s.Count - 1;
* for (int iSeg = 0; iSeg < segCnt; iSeg++)
* {
* double sL2 = L2s[iSeg];
* double eL2 = L2s[iSeg + 1];
* OpenTK.Vector2d sPt = new OpenTK.Vector2d(
* (1.0 - sL2) * curNodeCoord[0][0] + sL2 * curNodeCoord[1][0],
* (1.0 - sL2) * curNodeCoord[0][1] + sL2 * curNodeCoord[1][1]
* );
* OpenTK.Vector2d ePt = new OpenTK.Vector2d(
* (1.0 - eL2) * curNodeCoord[0][0] + eL2 * curNodeCoord[1][0],
* (1.0 - eL2) * curNodeCoord[0][1] + eL2 * curNodeCoord[1][1]
* );
* double segLen = (ePt - sPt).Length;
* for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
* {
* double[] segL = ip.Ls[ipPt];
* double weight = ip.Weights[ipPt];
* double L2 = segL[0] * sL2 + segL[1] * eL2;
* double[] L = { 1.0 - L2, L2 };
* double[] N = lineFE.CalcN(L);
* double[] lN = lLineFE.CalcN(L);
* double detJWeight = (segLen / 2.0) * weight;
* // 現在の位置
* double[] curCoord = new double[uDof];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* curCoord[iDof] += curNodeCoord[iNode][iDof] * N[iNode];
* }
* }
* // 連続な近似法線ベクトルを計算する
* double[] normal = new double[uDof];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int coId = lineFE.NodeCoordIds[iNode];
* double[] nodeNormal = co2Normal[coId];
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* normal[iDof] += nodeNormal[iDof] * N[iNode];
* }
* }
* normal = IvyFEM.Lapack.Utils.NormalizeDoubleVector(normal);
* double[] tangent = { normal[1], -normal[0] };
*
* // 対応するMasterの点を取得する
* uint masterFEId;
* double[] masterL;
* GetMasterLineFEPoint(
* curCoord, normal,
* uQuantityId, uDof, cQuantityId,
* out masterFEId, out masterL);
* if (masterFEId == 0)
* {
* // 対応するMasterの点がない
* continue;
* }
* LineFE masterLineFE = World.GetLineFE(uQuantityId, masterFEId);
* System.Diagnostics.Debug.Assert(masterLineFE.NodeCount == elemNodeCnt);
* double[] masterN = masterLineFE.CalcN(masterL);
* int[] masterNodes = new int[elemNodeCnt];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int coId = masterLineFE.NodeCoordIds[iNode];
* int nodeId = World.Coord2Node(uQuantityId, coId);
* masterNodes[iNode] = nodeId;
* }
* // 現在の位置
* double[] masterCurCoord = new double[uDof];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int coId = masterLineFE.NodeCoordIds[iNode];
* double[] coord = World.GetCoord(uQuantityId, coId);
* int iNodeId = masterNodes[iNode];
* if (iNodeId == -1)
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* masterCurCoord[iDof] += coord[iDof] * masterN[iNode];
* }
* }
* else
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* masterCurCoord[iDof] +=
* (coord[iDof] + U[iNodeId * uDof + iDof]) * masterN[iNode];
* }
* }
* }
*
* // ギャップの計算
* double gap = 0;
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* gap += -normal[iDof] * (curCoord[iDof] - masterCurCoord[iDof]);
* }
*
* // ラグランジュの未定乗数
* double[] l = new double[cDof];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int iNodeId = lNodes[iNode];
* if (iNodeId == -1)
* {
* continue;
* }
* for (int iDof = 0; iDof < cDof; iDof++)
* {
* l[iDof] += U[offset + iNodeId * cDof + iDof] * lN[iNode];
* }
* }
* double ln = 0;
* for (int iDof = 0; iDof < cDof; iDof++)
* {
* ln += normal[iDof] * l[iDof];
* }
* double lt = 0;
* for (int iDof = 0; iDof < cDof; iDof++)
* {
* lt += tangent[iDof] * l[iDof];
* }
*
* // Karush-Kuhn-Tucker条件
* double tolerance = IvyFEM.Linear.Constants.ConvRatioTolerance;
* if (ln <= tolerance &&
* Math.Abs(lt) <= tolerance &&
* gap >= -tolerance)
* {
* // 拘束しない
* continue;
* }
*
* ////////////////////////////////////////
* // これ以降、条件を付加する処理
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* int iNodeId = lNodes[iNode];
* if (iNodeId == -1)
* {
* continue;
* }
* lConstraintNodeIds[iNodeId] = true;
* }
*
* // Slave
* for (int row = 0; row < elemNodeCnt; row++)
* {
* int rowNodeId = nodes[row];
* if (rowNodeId == -1)
* {
* continue;
* }
* for (int col = 0; col < elemNodeCnt; col++)
* {
* int colNodeId = lNodes[col];
* if (colNodeId == -1)
* {
* continue;
* }
*
* double[,] kul = new double[uDof, cDof];
* double[,] klu = new double[cDof, uDof];
* for (int rowDof = 0; rowDof < uDof; rowDof++)
* {
* kul[rowDof, rowDof] +=
* detJWeight * N[row] * lN[col];
* klu[rowDof, rowDof] +=
* detJWeight * N[row] * lN[col];
* }
*
* for (int rowDof = 0; rowDof < uDof; rowDof++)
* {
* for (int colDof = 0; colDof < cDof; colDof++)
* {
* A[rowNodeId * uDof + rowDof, offset + colNodeId * cDof + colDof] +=
* kul[rowDof, colDof];
* A[offset + colNodeId * cDof + colDof, rowNodeId * uDof + rowDof] +=
* klu[colDof, rowDof];
* B[rowNodeId * uDof + rowDof] +=
* kul[rowDof, colDof] * U[offset + colNodeId * cDof + colDof];
* B[offset + colNodeId * cDof + colDof] +=
* klu[colDof, rowDof] * U[rowNodeId * uDof + rowDof];
* }
* }
* }
* }
*
* // Master
* for (int row = 0; row < elemNodeCnt; row++)
* {
* int rowNodeId = masterNodes[row];
* if (rowNodeId == -1)
* {
* continue;
* }
* for (int col = 0; col < elemNodeCnt; col++)
* {
* int colNodeId = lNodes[col];
* if (colNodeId == -1)
* {
* continue;
* }
*
* double[,] kul = new double[uDof, cDof];
* double[,] klu = new double[cDof, uDof];
* for (int rowDof = 0; rowDof < uDof; rowDof++)
* {
* kul[rowDof, rowDof] +=
* -detJWeight * masterN[row] * lN[col];
* klu[rowDof, rowDof] +=
* -detJWeight * masterN[row] * lN[col];
* }
*
* for (int rowDof = 0; rowDof < uDof; rowDof++)
* {
* for (int colDof = 0; colDof < cDof; colDof++)
* {
* A[rowNodeId * uDof + rowDof, offset + colNodeId * cDof + colDof] +=
* kul[rowDof, colDof];
* A[offset + colNodeId * cDof + colDof, rowNodeId * uDof + rowDof] +=
* klu[colDof, rowDof];
* B[rowNodeId * uDof + rowDof] +=
* kul[rowDof, colDof] * U[offset + colNodeId * cDof + colDof];
* B[offset + colNodeId * cDof + colDof] +=
* klu[colDof, rowDof] * U[rowNodeId * uDof + rowDof];
* }
* }
* }
* }
*
* // Slave
* double[,] qu = new double[elemNodeCnt, uDof];
* double[,] ql = new double[elemNodeCnt, cDof];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* qu[iNode, iDof] += detJWeight * l[iDof] * N[iNode];
* }
* }
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* for (int iDof = 0; iDof < cDof; iDof++)
* {
* ql[iNode, iDof] += detJWeight * lN[iNode] * curCoord[iDof];
* }
* }
*
* for (int row = 0; row < elemNodeCnt; row++)
* {
* int rowNodeId = nodes[row];
* if (rowNodeId == -1)
* {
* continue;
* }
* for (int rowDof = 0; rowDof < uDof; rowDof++)
* {
* B[rowNodeId * uDof + rowDof] += -qu[row, rowDof];
* }
* }
* for (int row = 0; row < elemNodeCnt; row++)
* {
* int rowNodeId = lNodes[row];
* if (rowNodeId == -1)
* {
* continue;
* }
* for (int rowDof = 0; rowDof < cDof; rowDof++)
* {
* B[offset + rowNodeId * cDof + rowDof] += -ql[row, rowDof];
* }
* }
*
* // Master
* double[,] masterQu = new double[elemNodeCnt, uDof];
* double[,] masterQl = new double[elemNodeCnt, cDof];
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* masterQu[iNode, iDof] += -detJWeight * l[iDof] * masterN[iNode];
* }
* }
* for (int iNode = 0; iNode < elemNodeCnt; iNode++)
* {
* for (int iDof = 0; iDof < uDof; iDof++)
* {
* masterQl[iNode, iDof] += -detJWeight * lN[iNode] * masterCurCoord[iDof];
* }
* }
*
* for (int row = 0; row < elemNodeCnt; row++)
* {
* int rowNodeId = masterNodes[row];
* if (rowNodeId == -1)
* {
* continue;
* }
* for (int rowDof = 0; rowDof < uDof; rowDof++)
* {
* B[rowNodeId * uDof + rowDof] += -masterQu[row, rowDof];
* }
* }
* for (int row = 0; row < elemNodeCnt; row++)
* {
* int rowNodeId = lNodes[row];
* if (rowNodeId == -1)
* {
* continue;
* }
* for (int rowDof = 0; rowDof < cDof; rowDof++)
* {
* B[offset + rowNodeId * cDof + rowDof] += -masterQl[row, rowDof];
* }
* }
* }
* }
* }
*
* // 条件をセットしなかった節点
* for (int iNodeId = 0; iNodeId < cNodeCnt; iNodeId++)
* {
* if (lConstraintNodeIds[iNodeId])
* {
* continue;
* }
* for (int iDof = 0; iDof < cDof; iDof++)
* {
* A[offset + iNodeId * cDof + iDof, offset + iNodeId * cDof + iDof] = 1.0;
* B[offset + iNodeId * cDof + iDof] = 0;
* }
* }
* }
*/
private void CalcTwoBodyContactMortarSegmentationQuantityAB(
uint cQuantityId, IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint uQuantityId = 0;
System.Diagnostics.Debug.Assert(World.GetFEOrder(uQuantityId) == 1);
System.Diagnostics.Debug.Assert(World.GetCoordCount(uQuantityId) ==
World.GetCoordCount(cQuantityId));
System.Diagnostics.Debug.Assert(World.GetDof(uQuantityId) == 2);
System.Diagnostics.Debug.Assert(World.GetDof(cQuantityId) == 2);
int uDof = 2;
int cDof = 2;
int uNodeCnt = (int)World.GetNodeCount(uQuantityId);
int cNodeCnt = (int)World.GetNodeCount(cQuantityId);
int offset = GetOffset(cQuantityId);
// 線要素の変位を更新
UpdateLineFEDisplacements(uQuantityId, uDof, cQuantityId);
// 節点法線ベクトルの計算
Dictionary <int, double[]> co2Normal = GetSlaveLineFECo2Normal(uQuantityId, uDof, cQuantityId);
// MasterからSlaveに垂らした点を計算する
Dictionary <uint, IList <double> > slaveFEL2s = GetSlavePointFromMasterNodes(
co2Normal,
uQuantityId, uDof, cQuantityId);
bool[] lConstraintNodeIds = new bool[cNodeCnt];
IList <uint> slaveFEIds = World.GetContactSlaveLineFEIds(cQuantityId);
foreach (uint slaveFEId in slaveFEIds)
{
LineFE lineFE = World.GetLineFE(uQuantityId, slaveFEId);
uint elemNodeCnt = lineFE.NodeCount;
int[] nodes = new int[elemNodeCnt];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = lineFE.NodeCoordIds[iNode];
int nodeId = World.Coord2Node(uQuantityId, coId);
nodes[iNode] = nodeId;
}
LineFE lLineFE = World.GetLineFE(cQuantityId, slaveFEId);
int[] lNodes = new int[elemNodeCnt];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = lLineFE.NodeCoordIds[iNode];
int lNodeId = World.Coord2Node(cQuantityId, coId);
lNodes[iNode] = lNodeId;
}
double[][] curNodeCoord = new double[elemNodeCnt][];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = lineFE.NodeCoordIds[iNode];
double[] coord = World.GetCoord(uQuantityId, coId);
int iNodeId = nodes[iNode];
curNodeCoord[iNode] = new double[uDof];
if (iNodeId == -1)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
curNodeCoord[iNode][iDof] = coord[iDof];
}
}
else
{
for (int iDof = 0; iDof < uDof; iDof++)
{
curNodeCoord[iNode][iDof] = coord[iDof] + U[iNodeId * uDof + iDof];
}
}
}
//IntegrationPoints ip = LineFE.GetIntegrationPoints(LineIntegrationPointCount.Point3);
//System.Diagnostics.Debug.Assert(ip.Ls.Length == 3);
IntegrationPoints ip = LineFE.GetIntegrationPoints(LineIntegrationPointCount.Point5);
System.Diagnostics.Debug.Assert(ip.Ls.Length == 5);
IList <double> L2s = new List <double>();
L2s.Add(0.0);
if (slaveFEL2s.ContainsKey(slaveFEId))
{
foreach (double L2 in slaveFEL2s[slaveFEId])
{
L2s.Add(L2);
}
}
L2s.Add(1.0);
int segCnt = L2s.Count - 1;
for (int iSeg = 0; iSeg < segCnt; iSeg++)
{
double sL2 = L2s[iSeg];
double eL2 = L2s[iSeg + 1];
OpenTK.Vector2d sPt = new OpenTK.Vector2d(
(1.0 - sL2) * curNodeCoord[0][0] + sL2 * curNodeCoord[1][0],
(1.0 - sL2) * curNodeCoord[0][1] + sL2 * curNodeCoord[1][1]
);
OpenTK.Vector2d ePt = new OpenTK.Vector2d(
(1.0 - eL2) * curNodeCoord[0][0] + eL2 * curNodeCoord[1][0],
(1.0 - eL2) * curNodeCoord[0][1] + eL2 * curNodeCoord[1][1]
);
double segLen = (ePt - sPt).Length;
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] segL = ip.Ls[ipPt];
double weight = ip.Weights[ipPt];
double L2 = segL[0] * sL2 + segL[1] * eL2;
double[] L = { 1.0 - L2, L2 };
double[] N = lineFE.CalcN(L);
double[] lN = lLineFE.CalcN(L);
double detJWeight = (segLen / 2.0) * weight;
// 現在の位置
double[] curCoord = new double[uDof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
curCoord[iDof] += curNodeCoord[iNode][iDof] * N[iNode];
}
}
// 連続な近似法線ベクトルを計算する
double[] normal = new double[uDof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = lineFE.NodeCoordIds[iNode];
double[] nodeNormal = co2Normal[coId];
for (int iDof = 0; iDof < uDof; iDof++)
{
normal[iDof] += nodeNormal[iDof] * N[iNode];
}
}
normal = IvyFEM.Lapack.Utils.NormalizeDoubleVector(normal);
double[] tangent = { normal[1], -normal[0] };
// 対応するMasterの点を取得する
uint masterFEId;
double[] masterL;
GetMasterLineFEPoint(
curCoord, normal,
uQuantityId, uDof, cQuantityId,
out masterFEId, out masterL);
if (masterFEId == 0)
{
// 対応するMasterの点がない
continue;
}
LineFE masterLineFE = World.GetLineFE(uQuantityId, masterFEId);
System.Diagnostics.Debug.Assert(masterLineFE.NodeCount == elemNodeCnt);
double[] masterN = masterLineFE.CalcN(masterL);
int[] masterNodes = new int[elemNodeCnt];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = masterLineFE.NodeCoordIds[iNode];
int nodeId = World.Coord2Node(uQuantityId, coId);
masterNodes[iNode] = nodeId;
}
// 現在の位置
double[] masterCurCoord = new double[uDof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = masterLineFE.NodeCoordIds[iNode];
double[] coord = World.GetCoord(uQuantityId, coId);
int iNodeId = masterNodes[iNode];
if (iNodeId == -1)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
masterCurCoord[iDof] += coord[iDof] * masterN[iNode];
}
}
else
{
for (int iDof = 0; iDof < uDof; iDof++)
{
masterCurCoord[iDof] +=
(coord[iDof] + U[iNodeId * uDof + iDof]) * masterN[iNode];
}
}
}
// ギャップの計算
double gap = 0;
for (int iDof = 0; iDof < uDof; iDof++)
{
gap += -normal[iDof] * (curCoord[iDof] - masterCurCoord[iDof]);
}
// ラグランジュの未定乗数
double[] l = new double[cDof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int iNodeId = lNodes[iNode];
if (iNodeId == -1)
{
continue;
}
for (int iDof = 0; iDof < cDof; iDof++)
{
l[iDof] += U[offset + iNodeId * cDof + iDof] * lN[iNode];
}
}
// Karush-Kuhn-Tucker条件
double tolerance = IvyFEM.Linear.Constants.ConvRatioTolerance;
if (l[0] <= tolerance &&
Math.Abs(l[1]) <= tolerance &&
gap >= -tolerance)
{
// 拘束しない
continue;
}
////////////////////////////////////////
// これ以降、条件を付加する処理
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int iNodeId = lNodes[iNode];
if (iNodeId == -1)
{
continue;
}
lConstraintNodeIds[iNodeId] = true;
}
// Slave
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = nodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < elemNodeCnt; col++)
{
int colNodeId = lNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kul = new double[uDof, cDof];
double[,] klu = new double[cDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
kul[rowDof, 0] +=
detJWeight * normal[rowDof] * N[row] * lN[col];
klu[0, rowDof] +=
detJWeight * normal[rowDof] * N[row] * lN[col];
kul[rowDof, 1] +=
detJWeight * tangent[rowDof] * N[row] * lN[col];
klu[1, rowDof] +=
detJWeight * tangent[rowDof] * N[row] * lN[col];
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < cDof; colDof++)
{
A[rowNodeId * uDof + rowDof, offset + colNodeId * cDof + colDof] +=
kul[rowDof, colDof];
A[offset + colNodeId * cDof + colDof, rowNodeId *uDof + rowDof] +=
klu[colDof, rowDof];
B[rowNodeId * uDof + rowDof] +=
kul[rowDof, colDof] * U[offset + colNodeId * cDof + colDof];
B[offset + colNodeId * cDof + colDof] +=
klu[colDof, rowDof] * U[rowNodeId * uDof + rowDof];
}
}
}
}
// Master
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = masterNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < elemNodeCnt; col++)
{
int colNodeId = lNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kul = new double[uDof, cDof];
double[,] klu = new double[cDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
kul[rowDof, 0] +=
-detJWeight * normal[rowDof] * masterN[row] * lN[col];
klu[0, rowDof] +=
-detJWeight * normal[rowDof] * masterN[row] * lN[col];
kul[rowDof, 1] +=
-detJWeight * tangent[rowDof] * masterN[row] * lN[col];
klu[1, rowDof] +=
-detJWeight * tangent[rowDof] * masterN[row] * lN[col];
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < cDof; colDof++)
{
A[rowNodeId * uDof + rowDof, offset + colNodeId * cDof + colDof] +=
kul[rowDof, colDof];
A[offset + colNodeId * cDof + colDof, rowNodeId *uDof + rowDof] +=
klu[colDof, rowDof];
B[rowNodeId * uDof + rowDof] +=
kul[rowDof, colDof] * U[offset + colNodeId * cDof + colDof];
B[offset + colNodeId * cDof + colDof] +=
klu[colDof, rowDof] * U[rowNodeId * uDof + rowDof];
}
}
}
}
// Slave
double[,] qu = new double[elemNodeCnt, uDof];
double[,] ql = new double[elemNodeCnt, cDof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
qu[iNode, iDof] +=
detJWeight * (l[0] * normal[iDof] + l[1] * tangent[iDof]) * N[iNode];
}
}
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < cDof; iDof++)
{
ql[iNode, 0] +=
detJWeight * lN[iNode] * normal[iDof] * curCoord[iDof];
ql[iNode, 1] +=
detJWeight * lN[iNode] * tangent[iDof] * curCoord[iDof];
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = nodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
B[rowNodeId * uDof + rowDof] += -qu[row, rowDof];
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = lNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < cDof; rowDof++)
{
B[offset + rowNodeId * cDof + rowDof] += -ql[row, rowDof];
}
}
// Master
double[,] masterQu = new double[elemNodeCnt, uDof];
double[,] masterQl = new double[elemNodeCnt, cDof];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
masterQu[iNode, iDof] +=
-detJWeight * (l[0] * normal[iDof] + l[1] * tangent[iDof]) * masterN[iNode];
}
}
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
masterQl[iNode, 0] +=
-detJWeight * lN[iNode] * normal[iDof] * masterCurCoord[iDof];
masterQl[iNode, 1] +=
-detJWeight * lN[iNode] * tangent[iDof] * masterCurCoord[iDof];
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = masterNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
B[rowNodeId * uDof + rowDof] += -masterQu[row, rowDof];
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = lNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < cDof; rowDof++)
{
B[offset + rowNodeId * cDof + rowDof] += -masterQl[row, rowDof];
}
}
}
}
}
// 条件をセットしなかった節点
for (int iNodeId = 0; iNodeId < cNodeCnt; iNodeId++)
{
if (lConstraintNodeIds[iNodeId])
{
continue;
}
for (int iDof = 0; iDof < cDof; iDof++)
{
A[offset + iNodeId * cDof + iDof, offset + iNodeId * cDof + iDof] = 1.0;
B[offset + iNodeId * cDof + iDof] = 0;
}
}
}
protected void CalcOgdenHyperelasticElementAB(
uint feId, IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint uQuantityId = 0;
uint lQuantityId = 1;
System.Diagnostics.Debug.Assert(World.GetDof(uQuantityId) == 2);
System.Diagnostics.Debug.Assert(World.GetDof(lQuantityId) == 1);
int uDof = 2;
int lDof = 1;
int uNodeCnt = (int)World.GetNodeCount(uQuantityId);
int lNodeCnt = (int)World.GetNodeCount(lQuantityId);
//System.Diagnostics.Debug.Assert(uNodeCnt * uDof + lNodeCnt * lDof == A.RowLength);
int offset = GetOffset(lQuantityId);
System.Diagnostics.Debug.Assert(offset == uNodeCnt * uDof);
TriangleFE uTriFE = World.GetTriangleFE(uQuantityId, feId);
TriangleFE lTriFE = World.GetTriangleFE(lQuantityId, feId);
Material ma0 = World.GetMaterial(uTriFE.MaterialId);
if (!(ma0 is OgdenHyperelasticMaterial))
{
return;
}
uint vertexCnt = uTriFE.VertexCount;
for (int iVertex = 0; iVertex < vertexCnt; iVertex++)
{
System.Diagnostics.Debug.Assert(uTriFE.VertexCoordIds[iVertex] == lTriFE.VertexCoordIds[iVertex]);
}
int[] uCoIds = uTriFE.NodeCoordIds;
uint uElemNodeCnt = uTriFE.NodeCount;
int[] uNodes = new int[uElemNodeCnt];
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
int coId = uCoIds[iNode];
int nodeId = World.Coord2Node(uQuantityId, coId);
uNodes[iNode] = nodeId;
}
int[] lCoIds = lTriFE.NodeCoordIds;
uint lElemNodeCnt = lTriFE.NodeCount;
int[] lNodes = new int[lElemNodeCnt];
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
int coId = lCoIds[iNode];
int nodeId = World.Coord2Node(lQuantityId, coId);
lNodes[iNode] = nodeId;
}
var ma = ma0 as OgdenHyperelasticMaterial;
bool isCompressible = ma.IsCompressible;
double d1 = ma.D1;
int oOrder = ma.Order;
double[] oMus = ma.Mus;
double[] oAlphas = ma.Alphas;
double rho = ma.MassDensity;
double[] g = { ma.GravityX, ma.GravityY };
double[] uSN = uTriFE.CalcSN();
double[,] lSNN = lTriFE.CalcSNN();
System.Diagnostics.Debug.Assert((int)World.TriIntegrationPointCount >= 3);
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);
System.Diagnostics.Debug.Assert(ip.Ls.Length == (int)World.TriIntegrationPointCount);
double[,] qu = new double[uElemNodeCnt, uDof];
double[] ql = new double[lElemNodeCnt];
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] uN = uTriFE.CalcN(L);
double[] lN = lTriFE.CalcN(L);
double[][] uNu = uTriFE.CalcNu(L);
double detJ = uTriFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
// 変位の微分
double[,] uu = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
int iNodeId = uNodes[iNode];
if (iNodeId == -1)
{
continue;
}
uu[iDof, jDof] += U[iNodeId * uDof + iDof] * uNu[jDof][iNode];
}
}
}
// ラグランジュの未定乗数
double l = 0;
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
int iNodeId = lNodes[iNode];
if (iNodeId == -1)
{
continue;
}
l += U[offset + iNodeId] * lN[iNode];
}
// Green-Lagrangeのひずみ
double[,] e = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
e[iDof, jDof] = (1.0 / 2.0) * (uu[iDof, jDof] + uu[jDof, iDof]);
for (int kDof = 0; kDof < uDof; kDof++)
{
e[iDof, jDof] += (1.0 / 2.0) * uu[kDof, iDof] * uu[kDof, jDof];
}
}
}
// 変形勾配テンソル
double[,] f = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
f[iDof, jDof] = uu[iDof, jDof];
}
f[iDof, iDof] += 1.0;
}
// 右Cauchy-Green変形テンソル
double[,] c = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
c[iDof, jDof] = uu[iDof, jDof] + uu[jDof, iDof];
for (int kDof = 0; kDof < uDof; kDof++)
{
c[iDof, jDof] += uu[kDof, iDof] * uu[kDof, jDof];
}
}
c[iDof, iDof] += 1.0;
}
// Cのテンソル不変量
double I1 = c[0, 0] + c[1, 1] + 1.0;
double I2 = c[0, 0] * c[1, 1] + c[0, 0] + c[1, 1] - c[0, 1] * c[1, 0];
double I3 = c[0, 0] * c[1, 1] - c[0, 1] * c[1, 0];
double inv13I3 = 1.0 / Math.Pow(I3, 1.0 / 3.0);
double inv23I3 = 1.0 / Math.Pow(I3, 2.0 / 3.0);
double[,] invC = new double[uDof, uDof];
{
double invI3 = 1.0 / I3;
invC[0, 0] = invI3 * c[1, 1];
invC[0, 1] = -invI3 * c[0, 1];
invC[1, 0] = -invI3 * c[1, 0];
invC[1, 1] = invI3 * c[0, 0];
}
// Cの主値の1/2乗と主軸
// Note: これらは3次元
int dim3 = 3;
System.Numerics.Complex[] fLambdas;
System.Numerics.Complex[][] cNormals;
SolvePrincipalValues(c, out fLambdas, out cNormals);
double inv16I3 = 1.0 / Math.Pow(I3, 1.0 / 6.0);
// 修正Cの主値
System.Numerics.Complex[] barFLambdas = fLambdas.Select(a => a * inv16I3).ToArray();
// 主第2Piola-Kirchhoff応力
System.Numerics.Complex[] principalS = new System.Numerics.Complex[uDof];
/*
* for (int kDof = 0; kDof < uDof; kDof++)
* {
* for (int r = 0; r < oOrder; r++)
* {
* double mu = oMus[r];
* double alpha = oAlphas[r];
* principalS[kDof] += (mu / (fLambdas[kDof] * fLambdas[kDof])) * (
* System.Numerics.Complex.Pow(barFLambdas[kDof], alpha) -
* (1.0 / 3.0) * (
* System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[2], alpha)
* ));
* }
* }
*/
for (int kDof = 0; kDof < uDof; kDof++)
{
for (int r = 0; r < oOrder; r++)
{
double mu = oMus[r];
double alpha = oAlphas[r];
principalS[kDof] += (mu / (fLambdas[kDof] * fLambdas[kDof])) * (
System.Numerics.Complex.Pow(barFLambdas[kDof], alpha) -
(1.0 / 3.0) * (
System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
Math.Pow(I3, -alpha / 6.0)
));
}
}
// 第2Piola-Kirchhoff応力
double[,] s = new double[uDof, uDof];
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
for (int kDof = 0; kDof < uDof; kDof++)
{
System.Numerics.Complex sij =
principalS[kDof] * cNormals[kDof][iDof] * cNormals[kDof][jDof];
s[iDof, jDof] += sij.Real;
//System.Diagnostics.Debug.Assert(
// Math.Abs(sij.Imaginary) < IvyFEM.Constants.PrecisionLowerLimit);
}
}
}
// Lagrangeの未定乗数の寄与項
{
double tmp = 2.0 * l * I3;
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int jDof = 0; jDof < uDof; jDof++)
{
s[iDof, jDof] += tmp * invC[iDof, jDof];
}
}
}
// 主軸構成則テンソル(オリジナルの方)
System.Numerics.Complex[,] principalC21st = new System.Numerics.Complex[uDof, uDof];
System.Numerics.Complex[,] principalC22nd = new System.Numerics.Complex[uDof, uDof];
/*
* for (int pDof = 0; pDof < uDof; pDof++)
* {
* for (int qDof = 0; qDof < uDof; qDof++)
* {
* for (int r = 0; r < oOrder; r++)
* {
* double mu = oMus[r];
* double alpha = oAlphas[r];
* System.Numerics.Complex tmp1 = 0;
* if (pDof == qDof)
* {
* tmp1 = -2.0 * (
* System.Numerics.Complex.Pow(barFLambdas[qDof], alpha) -
* (1.0 / 3.0) * (
* System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[2], alpha)
* ));
* }
* System.Numerics.Complex tmp2 = 0;
* for (int mDof = 0; mDof < dim3; mDof++)
* {
* double dmp = mDof == pDof ? 1 : 0;
* double dmq = mDof == qDof ? 1 : 0;
* tmp2 += alpha *
* System.Numerics.Complex.Pow(barFLambdas[mDof], alpha) *
* (dmp - 1.0 / 3.0) *
* (dmq - 1.0 / 3.0);
* }
* principalC21st[pDof, qDof] +=
* (mu / (fLambdas[pDof] * fLambdas[pDof] * fLambdas[qDof] * fLambdas[qDof])) *
* (tmp1 + tmp2);
* }
* }
* }
* for (int pDof = 0; pDof < uDof; pDof++)
* {
* for (int qDof = 0; qDof < uDof; qDof++)
* {
* for (int r = 0; r < oOrder; r++)
* {
* double mu = oMus[r];
* double alpha = oAlphas[r];
* System.Numerics.Complex tmp = 0;
* if (pDof != qDof)
* {
* System.Numerics.Complex diffFLambda = fLambdas[pDof] - fLambdas[qDof];
* if (diffFLambda.Magnitude >= IvyFEM.Constants.PrecisionLowerLimit)
* {
* // λp != λq
* System.Numerics.Complex squareFLambdaP = fLambdas[pDof] * fLambdas[pDof];
* System.Numerics.Complex squareFLambdaQ = fLambdas[qDof] * fLambdas[qDof];
* tmp = (mu / (squareFLambdaP * squareFLambdaQ)) * (
* (
* squareFLambdaQ * System.Numerics.Complex.Pow(barFLambdas[pDof], alpha) -
* squareFLambdaP * System.Numerics.Complex.Pow(barFLambdas[qDof], alpha)
* ) / (squareFLambdaP - squareFLambdaQ) +
* (1.0 / 3.0) *
* (
* System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[2], alpha)
* ));
* }
* else
* {
* // λp == λq
* tmp = (mu /
* (fLambdas[pDof] * fLambdas[pDof] * fLambdas[pDof] * fLambdas[pDof])) * (
* -((2.0 - alpha) / 2.0) *
* System.Numerics.Complex.Pow(barFLambdas[pDof], alpha) +
* (1.0 / 3.0) * (
* System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
* System.Numerics.Complex.Pow(barFLambdas[2], alpha)
* ));
* }
* principalC22nd[pDof, qDof] += tmp;
* }
* }
* }
* }
*/
uint dim2 = 2;
for (int pDof = 0; pDof < uDof; pDof++)
{
for (int qDof = 0; qDof < uDof; qDof++)
{
for (int r = 0; r < oOrder; r++)
{
double mu = oMus[r];
double alpha = oAlphas[r];
System.Numerics.Complex tmp1 = 0;
if (pDof == qDof)
{
tmp1 = -2.0 * (
System.Numerics.Complex.Pow(barFLambdas[qDof], alpha) -
(1.0 / 3.0) * (
System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
Math.Pow(I3, -alpha / 6.0)
));
}
System.Numerics.Complex tmp2 = 0;
for (int mDof = 0; mDof < dim2; mDof++)
{
double dmp = mDof == pDof ? 1 : 0;
double dmq = mDof == qDof ? 1 : 0;
tmp2 += alpha *
System.Numerics.Complex.Pow(barFLambdas[mDof], alpha) *
(dmp - 1.0 / 3.0) *
(dmq - 1.0 / 3.0);
}
tmp2 += (alpha / 9.0) * Math.Pow(I3, -alpha / 6.0);
principalC21st[pDof, qDof] +=
(mu / (fLambdas[pDof] * fLambdas[pDof] * fLambdas[qDof] * fLambdas[qDof])) *
(tmp1 + tmp2);
}
}
}
for (int pDof = 0; pDof < uDof; pDof++)
{
for (int qDof = 0; qDof < uDof; qDof++)
{
for (int r = 0; r < oOrder; r++)
{
double mu = oMus[r];
double alpha = oAlphas[r];
if (pDof != qDof)
{
System.Numerics.Complex tmp = 0;
System.Numerics.Complex diffFLambda = fLambdas[pDof] - fLambdas[qDof];
if (diffFLambda.Magnitude >= IvyFEM.Constants.PrecisionLowerLimit)
{
// λp != λq
System.Numerics.Complex squareFLambdaP = fLambdas[pDof] * fLambdas[pDof];
System.Numerics.Complex squareFLambdaQ = fLambdas[qDof] * fLambdas[qDof];
tmp = (mu / (squareFLambdaP * squareFLambdaQ)) * (
(
squareFLambdaQ * System.Numerics.Complex.Pow(barFLambdas[pDof], alpha) -
squareFLambdaP * System.Numerics.Complex.Pow(barFLambdas[qDof], alpha)
) / (squareFLambdaP - squareFLambdaQ) +
(1.0 / 3.0) *
(
System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
System.Numerics.Complex.Pow(I3, -alpha / 6.0)
));
}
else
{
// λp == λq
tmp = (mu /
(fLambdas[pDof] * fLambdas[pDof] * fLambdas[pDof] * fLambdas[pDof])) * (
-((2.0 - alpha) / 2.0) *
System.Numerics.Complex.Pow(barFLambdas[pDof], alpha) +
(1.0 / 3.0) * (
System.Numerics.Complex.Pow(barFLambdas[0], alpha) +
System.Numerics.Complex.Pow(barFLambdas[1], alpha) +
System.Numerics.Complex.Pow(I3, -alpha / 6.0)
));
}
principalC22nd[pDof, qDof] += tmp;
}
}
}
}
// 構成則テンソル（オリジナルの方）
double[,,,] c4 = new double[uDof, uDof, uDof, uDof];
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
for (int pDof = 0; pDof < uDof; pDof++)
{
for (int qDof = 0; qDof < uDof; qDof++)
{
System.Numerics.Complex c4ghef =
principalC21st[pDof, qDof] *
cNormals[pDof][gDof] * cNormals[pDof][hDof] *
cNormals[qDof][eDof] * cNormals[qDof][fDof] +
principalC22nd[pDof, qDof] *
cNormals[pDof][gDof] * cNormals[qDof][hDof] *
(
cNormals[pDof][eDof] * cNormals[qDof][fDof] +
cNormals[qDof][eDof] * cNormals[pDof][fDof]
);
c4[gDof, hDof, eDof, fDof] += c4ghef.Real;
//System.Diagnostics.Debug.Assert(
// Math.Abs(c4ghef.Imaginary) < IvyFEM.Constants.PrecisionLowerLimit);
}
}
}
}
}
}
// 構成則テンソル
double[,,,] barC4 = new double[uDof, uDof, uDof, uDof];
// 圧力構成則テンソル
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
barC4[gDof, hDof, eDof, fDof] +=
4.0 * l * I3 * invC[gDof, hDof] * invC[eDof, fDof] -
2.0 * l * I3 * (
invC[gDof, eDof] * invC[fDof, hDof] +
invC[gDof, fDof] * invC[eDof, hDof]);
}
}
}
}
// 変位構成則テンソル
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
barC4[gDof, hDof, eDof, fDof] +=
(1.0 / 2.0) * (c4[gDof, hDof, eDof, fDof] + c4[gDof, hDof, fDof, eDof]);
}
}
}
}
double[,,,] b = new double[uElemNodeCnt, uDof, uDof, uDof];
{
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
b[iNode, iDof, gDof, hDof] = uNu[hDof][iNode] * f[iDof, gDof];
}
}
}
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < uElemNodeCnt; col++)
{
int colNodeId = uNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kuu = new double[uDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
double tmp1 = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
for (int eDof = 0; eDof < uDof; eDof++)
{
for (int fDof = 0; fDof < uDof; fDof++)
{
tmp1 += barC4[gDof, hDof, eDof, fDof] *
b[row, rowDof, eDof, fDof] *
b[col, colDof, gDof, hDof];
}
}
}
}
kuu[rowDof, colDof] += detJWeight * tmp1;
}
}
{
double tmp = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
tmp += s[gDof, hDof] * uNu[gDof][row] * uNu[hDof][col];
}
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
kuu[rowDof, rowDof] += detJWeight * tmp;
}
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
for (int colDof = 0; colDof < uDof; colDof++)
{
A[rowNodeId * uDof + rowDof, colNodeId *uDof + colDof] +=
kuu[rowDof, colDof];
B[rowNodeId * uDof + rowDof] +=
kuu[rowDof, colDof] * U[colNodeId * uDof + colDof];
}
}
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < lElemNodeCnt; col++)
{
int colNodeId = lNodes[col];
if (colNodeId == -1)
{
continue;
}
double[,] kul = new double[uDof, lDof];
double[,] klu = new double[lDof, uDof];
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
double tmp = 0.0;
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
tmp += invC[gDof, hDof] * b[row, rowDof, gDof, hDof];
}
}
kul[rowDof, 0] +=
detJWeight * tmp * 2.0 * lN[col] * I3;
klu[0, rowDof] +=
detJWeight * tmp * 2.0 * lN[col] * I3;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
A[rowNodeId * uDof + rowDof, offset + colNodeId] += kul[rowDof, 0];
A[offset + colNodeId, rowNodeId *uDof + rowDof] += klu[0, rowDof];
B[rowNodeId * uDof + rowDof] +=
kul[rowDof, 0] * U[offset + colNodeId];
B[offset + colNodeId] +=
klu[0, rowDof] * U[rowNodeId * uDof + rowDof];
}
}
}
// Note: kllは数値積分しなくて求められる
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
for (int gDof = 0; gDof < uDof; gDof++)
{
for (int hDof = 0; hDof < uDof; hDof++)
{
qu[iNode, iDof] += detJWeight * s[gDof, hDof] * b[iNode, iDof, gDof, hDof];
}
}
}
}
for (int iNode = 0; iNode < lElemNodeCnt; iNode++)
{
if (isCompressible)
{
ql[iNode] += detJWeight * lN[iNode] * ((I3 - 1) - l / d1);
}
else
{
ql[iNode] += detJWeight * lN[iNode] * (I3 - 1);
}
}
}
if (isCompressible)
{
for (int row = 0; row < lElemNodeCnt; row++)
{
int rowNodeId = lNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < lElemNodeCnt; col++)
{
int colNodeId = lNodes[col];
if (colNodeId == -1)
{
continue;
}
double kll = -(1.0 / d1) * lSNN[row, col];
A[offset + rowNodeId, offset + colNodeId] += kll;
B[offset + rowNodeId] += kll * U[offset + colNodeId];
}
}
}
double[,] fg = new double[uElemNodeCnt, uDof];
for (int iNode = 0; iNode < uElemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < uDof; iDof++)
{
fg[iNode, iDof] = rho * g[iDof] * uSN[iNode];
}
}
for (int row = 0; row < uElemNodeCnt; row++)
{
int rowNodeId = uNodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int rowDof = 0; rowDof < uDof; rowDof++)
{
B[rowNodeId * uDof + rowDof] += fg[row, rowDof] - qu[row, rowDof];
}
}
for (int row = 0; row < lElemNodeCnt; row++)
{
int rowNodeId = lNodes[row];
if (rowNodeId == -1)
{
continue;
}
B[offset + rowNodeId] += -ql[row];
}
}
private void CalcAB(IvyFEM.Linear.DoubleSparseMatrix A, double[] B)
{
uint quantityId = 0;
var veloFV = World.GetFieldValue(VeloValueId);
IList <uint> feIds = World.GetTriangleFEIds(quantityId);
foreach (uint feId in feIds)
{
TriangleFE triFE = World.GetTriangleFE(quantityId, feId);
uint elemNodeCnt = triFE.NodeCount;
int[] nodes = new int[elemNodeCnt];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int coId = triFE.NodeCoordIds[iNode];
int nodeId = World.Coord2Node(quantityId, coId);
nodes[iNode] = nodeId;
}
Material ma0 = World.GetMaterial(triFE.MaterialId);
System.Diagnostics.Debug.Assert(ma0 is DiffusionMaterial);
var ma = ma0 as DiffusionMaterial;
double nu = ma.DiffusionCoef;
double f = ma.F;
double[][] velos = new double[elemNodeCnt][];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
int nodeId = nodes[iNode];
if (nodeId == -1)
{
continue;
}
int coId = World.Node2Coord(quantityId, nodeId);
double[] velo = veloFV.GetDoubleValue(coId, FieldDerivativeType.Value);
velos[iNode] = velo;
}
double tau;
{
double[] aveVelo =
{
(velos[0][0] + velos[1][0] + velos[2][0]) / 3.0,
(velos[0][1] + velos[1][1] + velos[2][1]) / 3.0
};
double veloNorm = Math.Sqrt(aveVelo[0] * aveVelo[0] + aveVelo[1] * aveVelo[1]);
double[] veloDir = { aveVelo[0] / veloNorm, aveVelo[1] / veloNorm };
double h;
{
double[] L = new double[] { 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0 };
double[][] Nu = triFE.CalcNu(L);
double[] Nx = Nu[0];
double[] Ny = Nu[1];
double tmp = 0;
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
tmp += Math.Abs(veloDir[0] * Nx[iNode] + veloDir[1] * Ny[iNode]);
}
h = 2.0 / tmp;
}
double lambda = (1.0 / 2.0) * veloNorm * h / nu;
//tau = (1.0 / 2.0) * (h / veloNorm) * (1.0 / Math.Tanh(lambda) - 1.0 / lambda);
if (nu > 1.0e-20)
{
if (lambda < 3.0)
{
tau = (1.0 / 2.0) * h / veloNorm * lambda / 3.0;
}
else
{
tau = (1.0 / 2.0) * h / veloNorm;
}
}
else
{
tau = (1.0 / 2.0) * h / veloNorm;
}
}
double[] sN = triFE.CalcSN();
IntegrationPoints ip = TriangleFE.GetIntegrationPoints(World.TriIntegrationPointCount);//Point7
for (int ipPt = 0; ipPt < ip.PointCount; ipPt++)
{
double[] L = ip.Ls[ipPt];
double[] N = triFE.CalcN(L);
double[][] Nu = triFE.CalcNu(L);
double[] Nx = Nu[0];
double[] Ny = Nu[1];
double detJ = triFE.GetDetJacobian(L);
double weight = ip.Weights[ipPt];
double detJWeight = (1.0 / 2.0) * weight * detJ;
double[] velo = new double[2];
for (int iNode = 0; iNode < elemNodeCnt; iNode++)
{
for (int iDof = 0; iDof < 2; iDof++)
{
velo[iDof] += N[iNode] * velos[iNode][iDof];
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = nodes[row];
if (rowNodeId == -1)
{
continue;
}
for (int col = 0; col < elemNodeCnt; col++)
{
int colNodeId = nodes[col];
if (colNodeId == -1)
{
continue;
}
double k = detJWeight * nu * (Nx[row] * Nx[col] + Ny[row] * Ny[col]);
double c = detJWeight *
(N[row] + tau * (velo[0] * Nx[row] + velo[1] * Ny[row])) *
(velo[0] * Nx[col] + velo[1] * Ny[col]);
A[rowNodeId, colNodeId] += k + c;
}
}
}
for (int row = 0; row < elemNodeCnt; row++)
{
int rowNodeId = nodes[row];
if (rowNodeId == -1)
{
continue;
}
B[rowNodeId] += f * sN[row];
}
}
}
