private int psi()
{
FriedChiken.Tick(t); //要素アップデート、勾配の計算
FriedChiken.Tack(t); //マスク等後処理
int itr = 0;
for (int s = 0; s < 50; s++)
{
var ff = ShoNS.Array.DoubleArray.From(FriedChiken.getJacobian().rawData);
var g = ShoNS.Array.DoubleArray.From(FriedChiken.getResidual().rawData).T;
var solver = new ShoNS.Array.Solver(ff);
var _dx = solver.Solve(g);
_dx.CopyTo(dx.rawData, 0);
FriedChiken.x.Subtract(1.0, dx);
FriedChiken.Tick(t); //要素アップデート、勾配の計算
FriedChiken.Tack(t); //マスク等後処理
itr++;
if (FriedChiken.getResidual().norm < 0.0001)
{
break;
}
}
return(itr);
}
protected override void SolveInstance(Grasshopper.Kernel.IGH_DataAccess DA)
{
stw.Stop();
string dbg = stw.ElapsedMilliseconds.ToString();
stw.Reset();
double dist = 0;
if (!DA.GetData(1, ref dist))
{
return;
}
if (_go == false)
{
t = 0;
FriedChiken.clear();
lpS = new List <GH_particleSystem>();
if (fixedPointsGuids != null)
{
foreach (Guid[] gs in fixedPointsGuids)
{
foreach (Guid g in gs)
{
Rhino.RhinoDoc.ActiveDoc.Objects.Delete(g, true);
}
}
fixedPointsGuids = null;
}
if (!DA.GetDataList(0, lpS))
{
return;
}
particleSystem[] _ps = null;
_ps = new particleSystem[lpS.Count];
for (int i = 0; i < lpS.Count; i++)
{
_ps[i] = lpS[i].Value;
}
FriedChiken.addParticleSystems(_ps);
//FriedChiken.begin();
__dist = dist;
lS = new List <Rhino.Geometry.GeometryBase>();
lPC = new List <Rhino.Geometry.PointCloud>();
lfN = new List <NumericalMethodHelper.objects.fixedNodes>();
fixedPointsGuids = new List <Guid[]>();
foreach (GH_particleSystem pS in lpS)
{
if (pS != null)
{
if (pS.UPGR != null)
{
pS.UPGR(dist, 0, 0);
}
}
}
}
else
{
double dt = full.getDt();
if (t == 0)
{
output = new List <string>();
//firstAction(DA);
FriedChiken.clear();
lpS = new List <GH_particleSystem>();
if (!DA.GetDataList(0, lpS))
{
return;
}
particleSystem[] _ps = null;
_ps = new particleSystem[lpS.Count];
for (int i = 0; i < lpS.Count; i++)
{
_ps[i] = lpS[i].Value;
}
FriedChiken.addParticleSystems(_ps);
if (fixedPointsGuids != null)
{
foreach (Guid[] gs in fixedPointsGuids)
{
foreach (Guid g in gs)
{
Rhino.RhinoDoc.ActiveDoc.Objects.Delete(g, true);
}
}
}
FriedChiken.begin();
refX = DoubleArray.From(FriedChiken.x.rawData);
__dist = dist;
if (FriedChiken.numCond > 0)
{
lambda = new vector(FriedChiken.numCond).zeros();
dx = new vector(FriedChiken.q.nElem);
qo = new vector(FriedChiken.q.nElem);
qr = new vector(FriedChiken.numCond).zeros();
}
lS = new List <Rhino.Geometry.GeometryBase>();
lPC = new List <Rhino.Geometry.PointCloud>();
lfN = new List <NumericalMethodHelper.objects.fixedNodes>();
fixedPointsGuids = new List <Guid[]>();
for (int i = 0; i < FriedChiken.particleSystems.Count; i++)
{
for (int j = 0; j < FriedChiken.particleSystems[i].objList.Count; j++)
{
if (FriedChiken.particleSystems[i].objList[j] is mikity.NumericalMethodHelper.objects.fixedNodes)
{
mikity.NumericalMethodHelper.objects.fixedNodes fN = (mikity.NumericalMethodHelper.objects.fixedNodes)FriedChiken.particleSystems[i].objList[j];
lfN.Add(fN);
fixedPointsGuids.Add(new Guid[fN.nNodes]);
Rhino.Geometry.Point3d[] ps = new Rhino.Geometry.Point3d[fN.nNodes];
Guid[] gs = fixedPointsGuids[fixedPointsGuids.Count - 1];
for (int k = 0; k < fN.nNodes; k++)
{
ps[k] = new Rhino.Geometry.Point3d(fN.nodeList[k].getNodes()[0, 0] + dist, fN.nodeList[k].getNodes()[0, 1], fN.nodeList[k].getNodes()[0, 2]);
gs[k] = Rhino.RhinoDoc.ActiveDoc.Objects.AddPoint(ps[k]);
}
Rhino.RhinoDoc.ActiveDoc.Groups.Add(gs);
}
}
}
}
//Computation
while (stw.ElapsedMilliseconds < 25)
{
stw.Start();
FriedChiken.Tick(t); //要素アップデート、勾配の計算
FriedChiken.Tack(t); //マスク等後処理
if (_IF)
{
FriedChiken.omega.zeros();
}
double resid = 0;
if (FriedChiken.numCond > 0)
{
resid = FriedChiken.getResidual().norm;
phi();
}
string tmp = "\t" + t.ToString() + "\t";
tmp += FriedChiken.omega.norm.ToString() + "\t";
if (_geodesic)
{
if (FriedChiken.numCond > 0)
{
varphi();
}
}
var v = DoubleArray.From(FriedChiken.q.rawData);
double normW = FriedChiken.omega.norm;
if (_normalize == true)
{
if (normW != 0)
{
FriedChiken.omega.dividedby(normW);//力を正規化
}
normW = FriedChiken.omega.norm;
}
var a = DoubleArray.From(FriedChiken.omega.rawData);
FriedChiken.omega.MinusTo(FriedChiken.r);//力を加速度に
double norm1 = (v * v.T)[0, 0];
double norm2 = (v * a.T)[0, 0];
double norm3 = (a * a.T)[0, 0];
double f = 0;
if (norm1 * norm3 != 0)
{
f = -norm2 / Math.Sqrt(norm1 * norm3);
}
else if (norm1 == 0)
{
f = 1;
}
else
{
f = -1;
}
double damping = 0;
if (_drift1)
{
damping = Drift1(f);
}
else if (_drift2)
{
damping = Drift2(f);
}
else if (_drift3)
{
damping = Drift3(f);
}
else
{
damping = Drift0(f);
}
//damping = 0;
full.move(f);
dbg = "damping:" + damping.ToString() + "\n" + "dt:" + dt.ToString() + "\n" + "Step#:" + t.ToString();
full.setDbgText(dbg);
FriedChiken.q.times(damping).Add(dt, FriedChiken.r);
double normQ = FriedChiken.q.norm;
double K = normQ * normQ * 0.5;
double P = FriedChiken.energy;
double E = K + P;
int itr = 0;
FriedChiken.x.Add(dt, FriedChiken.q);
if (FriedChiken.numCond > 0)
{
itr = psi();
}
full.addNorm(K, E, itr, normW, resid);
stw.Stop();
t++;
if (_RP == false)
{
break;
}
}
stw.Reset();
//////////////
for (int i = 0; i < lfN.Count; i++)
{
Guid[] gs = fixedPointsGuids[i];
for (int j = 0; j < gs.Count(); j++)
{
Rhino.DocObjects.PointObject obj = (Rhino.DocObjects.PointObject)Rhino.RhinoDoc.ActiveDoc.Objects.Find(gs[j]);
Rhino.Geometry.Point3d p = new Rhino.Geometry.Point3d(obj.PointGeometry.Location.X, obj.PointGeometry.Location.Y, obj.PointGeometry.Location.Z);
if (lfN[i].fixX)
{
lfN[i].nodeList[j].getNodes()[0, 0] = lfN[i].nodeList[j].getNodes()[0, 0] * 0.95 + (p.X - dist) * 0.05; //*
if (dist != __dist)
{
p.X = p.X + dist - __dist;
}
}
else
{
p.X = lfN[i].nodeList[j].getNodes()[0, 0] + dist;
}
if (lfN[i].fixY)
{
lfN[i].nodeList[j].getNodes()[0, 1] = lfN[i].nodeList[j].getNodes()[0, 1] * 0.95 + p.Y * 0.05;//*
}
else
{
p.Y = lfN[i].nodeList[j].getNodes()[0, 1];
}
if (lfN[i].fixZ)
{
lfN[i].nodeList[j].getNodes()[0, 2] = lfN[i].nodeList[j].getNodes()[0, 2] * 0.95 + p.Z * 0.05; //*
}
else
{
p.Z = lfN[i].nodeList[j].getNodes()[0, 2];
}
double x = 0, y = 0, z = 0;
x = p.X - obj.PointGeometry.Location.X;
y = p.Y - obj.PointGeometry.Location.Y;
z = p.Z - obj.PointGeometry.Location.Z;
Rhino.Geometry.Transform tx = Rhino.Geometry.Transform.Translation(x, y, z);
gs[j] = Rhino.RhinoDoc.ActiveDoc.Objects.Transform(gs[j], tx, true);
}
}
if (dist != __dist)
{
__dist = dist;
}
foreach (GH_particleSystem pS in lpS)
{
if (pS != null)
{
if (pS.UPGR != null)
{
pS.UPGR(dist, 0, 0);
}
}
}
}
return;
}
bool keyboardHook_KeyUp(RamGecTools.KeyboardHook.VKeys key)
{
if (DEV)
{
if (key == RamGecTools.KeyboardHook.VKeys.KEY_F)
{
for (int i = 0; i < FriedChiken.nParticles; i++)
{
FriedChiken.x[i * 3 + 0] += (rand.NextDouble() - 0.5) * 50d;
FriedChiken.x[i * 3 + 1] += (rand.NextDouble() - 0.5) * 50d;
FriedChiken.x[i * 3 + 2] += (rand.NextDouble() - 0.5) * 50d;
}
return(true);
}
if (key == RamGecTools.KeyboardHook.VKeys.KEY_E)
{
if (_IF)
{
_IF = false;
full.offIF();
}
else
{
_IF = true;
full.onIF();
}
return(true);
}
if (key == RamGecTools.KeyboardHook.VKeys.KEY_R)
{
if (_RP)
{
_RP = false;
full.offRF();
}
else
{
_RP = true;
full.onRF();
}
return(true);
}
if (key == RamGecTools.KeyboardHook.VKeys.KEY_N)
{
if (_normalize)
{
_normalize = false;
full.offNorm();
}
else
{
_normalize = true;
full.onNorm();
}
return(true);
}
if (key == RamGecTools.KeyboardHook.VKeys.KEY_H)
{
if (_geodesic)
{
_geodesic = false;
full.offGeo();
}
else
{
_geodesic = true;
full.onGeo();
}
return(true);
}
if (key == RamGecTools.KeyboardHook.VKeys.KEY_A)
{
if (_drift1)
{
_drift1 = false;
_drift2 = true;
_drift3 = false;
full.drift2();
full.renewPlot(Drift2);
}
else if (_drift2)
{
_drift1 = false;
_drift2 = false;
_drift3 = true;
full.drift3();
full.renewPlot(Drift3);
}
else if (_drift3)
{
_drift1 = false;
_drift2 = false;
_drift3 = false;
full.drift0();
full.renewPlot(Drift0);
}
else
{
_drift1 = true;
_drift2 = false;
_drift3 = false;
full.drift1();
full.renewPlot(Drift1);
}
return(true);
}
}
if (key == RamGecTools.KeyboardHook.VKeys.ESCAPE)
{
full.resetGo();
full.clearNorm();
_go = false;
timer.Enabled = false;
if (t != 0 && refX != null)
{
t = 0;
for (int i = 0; i < FriedChiken.x.rawData.Length; i++)
{
FriedChiken.x.rawData[i] = refX[i];
}
FriedChiken.Tick(t); //要素アップデート、勾配の計算
FriedChiken.Tack(t); //マスク等後処理
refX = null;
}
ExpireSolution(true);
return(true);
}
if (key == RamGecTools.KeyboardHook.VKeys.KEY_G)
{
if (_go)
{
full.pauseGo();
_go = false;
timer.Enabled = false;
}
else
{
full.onGo();
_go = true;
timer.Enabled = true;
}
return(true);
}
if (key == RamGecTools.KeyboardHook.VKeys.KEY_T)
{
if (_VN)
{
_VN = false;
full.offVN();
}
else
{
_VN = true;
full.onVN();
}
return(true);
}
return(false);
}