public void mosek2(List<leaf> _listLeaf, List<branch> _listBranch, List<node> _listNode, double force)
{
//variable settings
ShoNS.Array.SparseDoubleArray mat = new SparseDoubleArray(_listNode.Count * 3, _listNode.Count * 3);
ShoNS.Array.SparseDoubleArray F = new SparseDoubleArray(_listNode.Count * 3, 1);
ShoNS.Array.SparseDoubleArray xx = new SparseDoubleArray(_listNode.Count*3,1);
ShoNS.Array.SparseDoubleArray shift = new SparseDoubleArray(_listNode.Count*3, _listNode.Count*3);
for (int k = 0; k < _listNode.Count;k++ )
{
var node = _listNode[k];
xx[k * 3 + 0, 0] = node.x;
xx[k * 3 + 1, 0] = node.y;
xx[k * 3 + 2, 0] = node.z;
/* if (node.nodeType == node.type.fx)
{
for (int i = 0; i < node.shareB.Count; i++)
{
var branch = node.shareB[i];
var index = node.numberB[i];
if (branch.branchType == branch.type.fix)
{
node.x = branch.crv.Points[index].Location.X;
node.y = branch.crv.Points[index].Location.Y;
node.z = branch.slice2.height;
xx[k * 3 + 0, 0] = node.x;
xx[k * 3 + 1, 0] = node.y;
xx[k * 3 + 2, 0] = node.z;
}
}
}*/
}
List<int> series=new List<int>();
List<Point3d> origin = new List<Point3d>();
for(int i=0;i<_listNode.Count;i++)
{
series.Add(i);
}
int L1=0;
int L2=_listNode.Count;
for(int i=0;i<_listNode.Count;i++)
{
var node=_listNode[i];
if(node.nodeType==node.type.fx)
{
L2--;
series[i]=L2;
}else{
series[i]=L1;
origin.Add(new Point3d(node.x, node.y, node.z));
L1++;
}
}
for (int i = 0; i < _listNode.Count; i++)
{
shift[i * 3 + 0, series[i] * 3 + 0] = 1;
shift[i * 3 + 1, series[i] * 3 + 1] = 1;
shift[i * 3 + 2, series[i] * 3 + 2] = 1;
F[i * 3 + 2, 0] = -force;//force
}
foreach (var leaf in _listLeaf)
{
foreach (var tup in leaf.tuples)
{
var det = tup.SPK[0, 0] * tup.SPK[1, 1] - tup.SPK[0, 1] * tup.SPK[0, 1];
if (det > 0)
{
for (int i = 0; i < tup.nNode; i++)
{
for (int j = 0; j < tup.nNode; j++)
{
for (int k = 0; k < 3; k++)
{
for (int l = 0; l < 2; l++)
{
for (int m = 0; m < 2; m++)
{
var val = tup.B[l, m, i * 3 + k, j * 3 + k] * tup.SPK[l, m] * tup.refDv * tup.area;
mat[leaf.globalIndex[tup.internalIndex[i]] * 3 + k, leaf.globalIndex[tup.internalIndex[j]] * 3 + k] += val;
}
}
}
}
}
}
}
}
foreach (var branch in _listBranch)
{
foreach (var tup in branch.tuples)
{
for (int i = 0; i < tup.nNode; i++)
{
for (int j = 0; j < tup.nNode; j++)
{
for (int k = 0; k < 3; k++)
{
//if (tup.SPK[0, 0] > 0)
{
var val = tup.B[0, 0, i * 3 + k, j * 3 + k] * tup.SPK[0, 0] * tup.refDv * tup.area;
for (int l = 0; l < 1; l++)
{
for (int m = 0; m < 1; m++)
{
var val2 = tup.B[l, m, i * 3 + k, j * 3 + k] * tup.SPK[l, m] * tup.refDv * tup.area;
mat[branch.globalIndex[tup.internalIndex[i]] * 3 + k, branch.globalIndex[tup.internalIndex[j]] * 3 + k] += val2;
}
}
}
}
}
}
}
}
var reactionForce = mat * xx as SparseDoubleArray;
double max1 = 0;
double max2 = 0;
for (int i = 0; i < _listNode.Count; i++)
{
if (_listNode[i].nodeType == node.type.fr)
{
var nx = reactionForce[i * 3 + 0, 0];
var ny = reactionForce[i * 3 + 1, 0];
var norm = Math.Sqrt(nx * nx + ny * ny);
if (norm > max1) max1 = norm;
}
else
{
var nx = reactionForce[i * 3 + 0, 0];
var ny = reactionForce[i * 3 + 1, 0];
var norm = Math.Sqrt(nx * nx + ny * ny);
if (norm > max2) max2 = norm;
}
}
//System.Windows.Forms.MessageBox.Show(max1.ToString() + ".-." + max2.ToString());
var newMat = (shift.T.Multiply(mat) as SparseDoubleArray).Multiply(shift) as SparseDoubleArray;
var newxx = shift.T.Multiply(xx) as SparseDoubleArray;
var newF = shift.T.Multiply(F) as SparseDoubleArray;
var T = newMat.GetSliceDeep(0, L1 * 3 - 1, 0, L1 * 3 - 1);
var D = newMat.GetSliceDeep(0, L1 * 3 - 1, L1 * 3, _listNode.Count * 3 - 1);
var fx = newxx.GetSliceDeep(L1 * 3, _listNode.Count * 3 - 1, 0, 0);
newF = newF.GetSliceDeep(0, L1 * 3 - 1, 0, 0);
var solve = new SparseLU(T);
//var solve = new SparseSVD(T);
//var _U = solve.U;
//var _V = solve.V;
//var _D = solve.D;
//double tol=0.00000000001;
//for (int i = 0; i < L1*3; i++)
//{
// if (_D[i, i] > tol) _D[i, i] = 1d / _D[i, i];
// else if (_D[i, i] < -tol) _D[i, i] = 1d / _D[i, i];
// else _D[i, i] = 0d;
//}
var df = D * fx as SparseDoubleArray;
var b = DoubleArray.From((-newF - df));
var sol=solve.Solve(b);
//var sol = _V * _D * _U.T*b;
var exSol = new SparseDoubleArray(sol.GetLength(0)+fx.GetLength(0),1);
for (int i = 0; i < L1; i++)
{
exSol[i * 3 + 0, 0] = sol[i * 3 + 0, 0];
exSol[i * 3 + 1, 0] = sol[i * 3 + 1, 0];
exSol[i * 3 + 2, 0] = sol[i * 3 + 2, 0];
}
for (int i = L1; i < _listNode.Count; i++)
{
exSol[i * 3 + 0, 0] = fx[(i - L1) * 3 + 0, 0];
exSol[i * 3 + 1, 0] = fx[(i - L1) * 3 + 1, 0];
exSol[i * 3 + 2, 0] = fx[(i - L1) * 3 + 2, 0];
}
exSol = shift.Multiply(exSol) as SparseDoubleArray;
foreach (var branch in _listBranch)
{
branch.shellCrv = branch.crv.Duplicate() as NurbsCurve;
for (int i = 0; i < branch.N; i++)
{
var P = branch.shellCrv.Points[i].Location;
branch.shellCrv.Points.SetPoint(i, new Point3d(exSol[branch.globalIndex[i] * 3 + 0, 0], exSol[branch.globalIndex[i] * 3 + 1, 0], exSol[branch.globalIndex[i] * 3 + 2, 0]));
//if you don't want to allow movements of x and y coordinates, use the following instead of the above.
//branch.shellCrv.Points.SetPoint(i, new Point3d(P.X, P.Y, exSol[branch.globalIndex[i] * 3 + 2, 0]));
}
}
foreach (var leaf in _listLeaf)
{
leaf.shellSrf = leaf.srf.Duplicate() as NurbsSurface;
for (int i = 0; i < leaf.nU; i++)
{
for (int j = 0; j < leaf.nV; j++)
{
var P = leaf.shellSrf.Points.GetControlPoint(i, j).Location;
leaf.shellSrf.Points.SetControlPoint(i, j, new ControlPoint(exSol[leaf.globalIndex[i + j * leaf.nU] * 3 + 0, 0], exSol[leaf.globalIndex[i + j * leaf.nU] * 3 + 1, 0], exSol[leaf.globalIndex[i + j * leaf.nU] * 3 + 2, 0]));
//if you don't want to allow movements of x and y coordinates, use the following instead of the above.
//leaf.shellSrf.Points.SetControlPoint(i, j, new ControlPoint(P.X, P.Y, exSol[leaf.globalIndex[i + j * leaf.nU] * 3 + 2, 0]));
}
}
}
}
public void computeForceDiagram(List<leaf> _listLeaf, List<node> _listNode)
{
//variable settings
int numvar = _listNode.Count * 2;
int numcon = 0;
double infinity = 0;
numcon += _listNode.Count; //finite element matrix
foreach (var leaf in _listLeaf)
{
leaf.conOffset = numcon;
leaf.varOffset = numvar;
numcon += leaf.tuples.Length * 1; //grad(detF)dx>-detF
}
mosek.boundkey[] bkx = new mosek.boundkey[numvar];
double[] blx = new double[numvar];
double[] bux = new double[numvar];
for (int i = 0; i < _listNode.Count; i++)
{
if (_listNode[i].forceNodeType== node.type.fx)
{
bkx[i * 2 + 0] = mosek.boundkey.fx;
blx[i * 2 + 0] = 0;// _listNode[i].X;
bux[i * 2 + 0] = 0;//_listNode[i].X;
bkx[i * 2 + 1] = mosek.boundkey.fx;
blx[i * 2 + 1] = 0;//_listNode[i].Y;
bux[i * 2 + 1] = 0;//_listNode[i].Y;
}
else
{
bkx[i * 2 + 0] = mosek.boundkey.fr;
blx[i * 2 + 0] = -infinity;
bux[i * 2 + 0] = infinity;
bkx[i * 2 + 1] = mosek.boundkey.fr;
blx[i * 2 + 1] = -infinity;
bux[i * 2 + 1] = infinity;
}
}
for (int t = 0; t < 10; t++)
{
init(_listLeaf);
double[] x = new double[_listNode.Count * 2];
foreach (var leaf in _listLeaf)
{
for (int j = 0; j < leaf.nV; j++)
{
for (int i = 0; i < leaf.nU; i++)
{
int indexX = leaf.globalIndex[i + j * leaf.nU] * 2 + 0;
int indexY = leaf.globalIndex[i + j * leaf.nU] * 2 + 1;
var Q = leaf.forceSrf.Points.GetControlPoint(i, j);
x[indexX] = Q.Location.X;
x[indexY] = Q.Location.Y;
}
}
}
using (mosek.Env env = new mosek.Env())
{
// Create a task object.
using (mosek.Task task = new mosek.Task(env, 0, 0))
{
// Directs the log task stream to the user specified
// method msgclass.streamCB
task.set_Stream(mosek.streamtype.log, new msgclass(""));
task.appendcons(numcon);
task.appendvars(numvar);
for (int j = 0; j < numvar; ++j)
{
task.putvarbound(j, bkx[j], blx[j], bux[j]);
}
ShoNS.Array.SparseDoubleArray mat = new SparseDoubleArray(_listNode.Count, _listNode.Count * 2);
foreach (var leaf in _listLeaf)
{
foreach (var tup in leaf.tuples)
{
for(int i = 0; i < tup.nNode; i++)
{
int indexI = leaf.globalIndex[tup.internalIndex[i]];
for (int j = 0; j < tup.nNode; j++)
{
int indexJx = leaf.globalIndex[tup.internalIndex[j]] * 2 + 0;
int indexJy = leaf.globalIndex[tup.internalIndex[j]] * 2 + 1;
var d0 = tup.d0;
var d1 = tup.d1;
var G1 = tup.refGi[0];
var G2 = tup.refGi[1];
var val0 = (d0[j] * d1[0][i] * G1[1] + d0[j] * d1[1][i] * G2[1]) * tup.refDv * tup.area;
var val1 = -(d0[j] * d1[0][i] * G1[0] + d0[j] * d1[1][i] * G2[0]) * tup.refDv * tup.area;
mat[indexI, indexJx] += val0;
mat[indexI, indexJy] += val1;
}
}
}
}
for (int i = 0; i < _listNode.Count; i++)
{
var val = 0d;
for (int j = 0; j < _listNode.Count * 2; j++)
{
val += mat[i, j] * x[j];
}
task.putconbound(i, mosek.boundkey.fx, -val, -val);
for (int j = 0; j < _listNode.Count*2; j++)
{
task.putaij(i, j, mat[i, j]);
}
}
foreach (var leaf in _listLeaf)
{
int offsetC = 0;
foreach (var tup in leaf.tuples)
{
//compute current detF
//x,y; referece, X,Y; forceDiagram
var FXx = tup.gi[0][0] * tup.refGi[0][0] + tup.gi[1][0] * tup.refGi[1][0];
var FYy = tup.gi[0][1] * tup.refGi[0][1] + tup.gi[1][1] * tup.refGi[1][1];
var FXy = tup.gi[0][0] * tup.refGi[0][1] + tup.gi[1][0] * tup.refGi[1][1];
var FYx = tup.gi[0][1] * tup.refGi[0][0] + tup.gi[1][1] * tup.refGi[1][0];
var detF = FXx * FYy- FXy * FYx;
task.putconbound(leaf.conOffset + offsetC, mosek.boundkey.lo, -detF,infinity);
var G1 = tup.refGi[0];
var G2 = tup.refGi[1];
for (int i = 0; i < tup.nNode; i++)
{
int indexX = leaf.globalIndex[tup.internalIndex[i]] * 2 + 0;
int indexY = leaf.globalIndex[tup.internalIndex[i]] * 2 + 1;
double val1 = (tup.d1[0][i] * G1[0] + tup.d1[1][i] * G2[0]) * FYy;
double val2 = (tup.d1[0][i] * G1[1] + tup.d1[1][i] * G2[1]) * FYx;
task.putaij(leaf.conOffset + offsetC, indexX, -val2);
val1 = (tup.d1[0][i] * G1[1] + tup.d1[1][i] * G2[1]) * FXx;
val2 = (tup.d1[0][i] * G1[0] + tup.d1[1][i] * G2[0]) * FXy;
task.putaij(leaf.conOffset + offsetC, indexY, val1- val2);
}
offsetC ++;
}
}
for (int i = 0; i < _listNode.Count; i++)
{
task.putqobjij(i * 2 + 0, i * 2 + 0, 1);
task.putqobjij(i * 2 + 1, i * 2 + 1, 1);
}
task.optimize();
// Print a summary containing information
// about the solution for debugging purposes
task.solutionsummary(mosek.streamtype.msg);
mosek.solsta solsta;
task.getsolsta(mosek.soltype.itr, out solsta);
double[] dx = new double[numvar];
task.getxx(mosek.soltype.itr, // Basic solution.
dx);
switch (solsta)
{
case mosek.solsta.optimal:
System.Windows.Forms.MessageBox.Show("Optimal primal solution\n");
break;
case mosek.solsta.near_optimal:
System.Windows.Forms.MessageBox.Show("Near Optimal primal solution\n");
break;
case mosek.solsta.dual_infeas_cer:
System.Windows.Forms.MessageBox.Show("Primal or dual infeasibility.\n");
break;
case mosek.solsta.prim_infeas_cer:
System.Windows.Forms.MessageBox.Show("Primal or dual infeasibility.\n");
break;
case mosek.solsta.near_dual_infeas_cer:
System.Windows.Forms.MessageBox.Show("Primal or dual infeasibility.\n");
break;
case mosek.solsta.near_prim_infeas_cer:
System.Windows.Forms.MessageBox.Show("Primal or dual infeasibility.\n");
break;
case mosek.solsta.unknown:
System.Windows.Forms.MessageBox.Show("Unknown solution status\n");
break;
default:
System.Windows.Forms.MessageBox.Show("Other solution status\n");
break;
}
foreach (var leaf in _listLeaf)
{
for (int j = 0; j < leaf.nV; j++)
{
for (int i = 0; i < leaf.nU; i++)
{
int indexX = leaf.globalIndex[i + j * leaf.nU] * 2 + 0;
int indexY = leaf.globalIndex[i + j * leaf.nU] * 2 + 1;
var Q = leaf.forceSrf.Points.GetControlPoint(i, j);
var P = new Point3d(Q.Location.X + dx[indexX] * 1d, Q.Location.Y + dx[indexY] * 1d, 0);
leaf.forceSrf.Points.SetControlPoint(i, j, P);
}
}
}
}
}
ExpirePreview(true);
}
}