public void AddResult(SolPoint sol)
{
timeList.Add(sol.T);
xPosList.Add(sol.X[0]);
yPosList.Add(sol.X[1]);
zPosList.Add(sol.X[2]);
xVelList.Add(sol.X[3]);
yVelList.Add(sol.X[4]);
zVelList.Add(sol.X[5]);
xAngList.Add(sol.X[6]);
yAngList.Add(sol.X[7]);
zAngList.Add(sol.X[8]);
xQuatList.Add(sol.X[9]);
yQuatList.Add(sol.X[10]);
zQuatList.Add(sol.X[11]);
wQuatList.Add(sol.X[12]);
var testArr = new double[14];
testArr[0] = sol.T;
for (int i = 0; i <= 12; i++)
{
testArr[i + 1] = sol.X[i];
}
allResult.Add(testArr);
}
public void SynchMeTo(SolPoint sp)
{
var v = sp.X;
var t = sp.T;
SynchMeTo(t, ref v);
}
static void Main(string[] args)
{
var mp = new MaterialObjectNewtow()
{
Name = "ball"
};
mp.Mass.Ix = 10;
mp.Mass.Iy = 10;
mp.Mass.Iz = 10;
mp.AddForce(new ForceCenter(10, new Position3D(0, -1, 0, "Dir"), null, "G"));
mp.AddForce(new Force(1, new Position3D(0, 1, 0, "Dir"), new Position3D(1, 0, 0, "FPoint"), null));
mp.AddForce(new Force(1, new Position3D(0, -1, 0, "Dir"), new Position3D(-1, 0, 0, "FPoint"), null));
//mp.AddMoment(new ForceCenter(2,new Position3D(1,0,0,"Dir"),null,"Moment"));
//mp.Vel.X = 2;
mp.Vel.Y = 100;
//mp.Vel.Z = -0.1;
var x0 = mp.Rebuild();
var solve = Ode.RK45(0, mp.Rebuild(), mp.f);
var res = mp.GetAllParamsValues(0, x0);
for (int i = 0; i < res.Length; i++)
{
Console.WriteLine($"{mp.AllParamsNames[i]} = \t{res[i]}");
}
SolPoint sp = new SolPoint();
string diffNames = " V = [ ";
foreach (var diffpar in mp.DiffArr)
{
diffNames += $"{diffpar.FullName}, ";
}
diffNames += " ];";
Console.WriteLine(diffNames);
foreach (var item in solve.SolveFromToStep(0, 21, 1))
{
Console.WriteLine($"t = {item.T}, {item.X.ToString("G3"):-7}");
sp = item;
}
res = mp.GetAllParamsValues(sp);
for (int i = 0; i < res.Length; i++)
{
Console.WriteLine($"{mp.AllParamsNames[i]} = \t{res[i]}");
}
//==============================================================================
//var mp = new MaterialPoint() { Name = "ball" };
//mp.AddForce(new ForceCenter(10,new Position3D(0,-1,0,"Dir"),null));
//mp.Vel.X = 2;
//mp.Vel.Y = 100;
//mp.Vel.Z = -0.1;
//var x0 = mp.Rebuild();
//var solve = Ode.RK45(0,mp.Rebuild(),mp.f);
//var res = mp.GetAllParamsValues(0,x0);
//for(int i = 0; i < res.Length; i++) {
// Console.WriteLine($"{mp.AllParamsNames[i]} = \t{res[i]}");
//}
//SolPoint sp = new SolPoint();
//string diffNames = " V = [ ";
//foreach(var diffpar in mp.DiffArr) {
// diffNames += $"{diffpar.FullName}, ";
//}
//diffNames += " ];";
//Console.WriteLine( diffNames);
//foreach(var item in solve.SolveFromToStep(0,21,1)) {
// Console.WriteLine($"t = {item.T}, \tV = {item.X}");
// sp = item;
//}
//res = mp.GetAllParamsValues(sp);
//for(int i = 0; i < res.Length; i++) {
// Console.WriteLine($"{mp.AllParamsNames[i]} = \t{res[i]}");
//}
//var dm = new ScnObjDummy() { Name = "Rocket" };
//dm.AddChild(new Position3D(new Vector3D(3,77,0)));
//dm.AddChild(new MassPoint(10));
//dm.AddChild(new Force3D(new Vector3D(0,-8,4)));
//dm.AddLaw(new NewtonLaw4MatPoint3D());
//var x0 = dm.Rebuild();
//var solve = Ode.RK45(0,dm.Rebuild(),dm.f);
//var res = dm.GetAllParamsValues(0,x0);
//for(int i = 0; i < res.Length; i++) {
// Console.WriteLine($"{dm.AllParamsNames[i]} = \t{res[i]}");
//}
//SolPoint sp = new SolPoint();
//foreach(var item in solve.SolveFromToStep(0,100,1)) {
// Console.WriteLine($"t = {item.T}, \tV = {item.X}");
// sp = item;
//}
//res = dm.GetAllParamsValues(sp);
//for(int i = 0; i < res.Length; i++) {
// Console.WriteLine($"{dm.AllParamsNames[i]} = \t{res[i]}");
//}
//==========================
//var sw = new Stopwatch();
//var pos = new Position3D(0,0,0,"pos");
//var pos_ = new Position3D(12,17,-22,"dposDt");
//pos.AddChild(pos_);
//pos.AddDiffVect(pos_);
//var interpX = new InterpXY();
//interpX.Add(0,0);
//interpX.Add(2,1);
//interpX.Add(6,-1);
//interpX.Add(8,0);
//pos_.pX.SealInterp(interpX);
//var solve = Ode.RK45(0,pos.Rebuild(),pos.f,0.01);
//SolPoint sp = new SolPoint();
//sw.Start();
//foreach(var item in solve.SolveFromToStep(0,10000,1000)) {
// Console.WriteLine($"t = {item.T}, \tV = {item.X}");
// sp = item;
//}
//sw.Stop();
//Console.WriteLine($"{sw.ElapsedMilliseconds}");
//var res = pos.GetAllParamsValues(sp);
//for(int i = 0; i < res.Length; i++) {
// Console.WriteLine($"{pos.AllParamsNames[i]} = \t{res[i]}");
//}
//var ww = new ScnPrm("ww", null, 1);
//ww.SetVal(33);
//Console.WriteLine($"ww = {ww.GetVal(1)}");
//var obj = new ScnObjDummy();
//obj.AddParam(ww);
//var dww = new ScnPrmConst("_",null,2);
//obj.AddDiffPropToParam(ww,dww);
//obj.Rebuild();
//foreach(var item in obj.DiffArr) {
// Console.WriteLine($"{item.MyDiff.Name} = {item.MyDiff.GetVal(0d)}");
//}
Console.ReadLine();
}
public Vector GetAllParamsValues(SolPoint sp)
{
return(GetAllParamsValues(sp.T, sp.X));
}
public SolPoint StepUpNplus1(double dt, ref SolPoint spN)
{
SynchMeTo(spN);
return(StepUpNplus1(dt, false));
}
Integrate(Func <double, double, Vector, Func <double, Vector, Vector>, IEnumerable <SolPoint> > Solver,
State initialState, double initialTime, double finalTime, Func <double, Vector, Vector> Flux,
SampleValue sample, List <ReportEntry> reports, Noise noise, bool nonTrivialSolution, Style style)
{
double redrawTick = initialTime; double redrawStep = (finalTime - initialTime) / 50;
double densityTick = initialTime; double densityStep = (finalTime - initialTime) / 1000;
int pointsCounter = 0;
int renderedCounter = 0;
double lastTime = finalTime;
State lastState = null;
if (initialState.NaN())
{
Gui.Log("Initial state contains NaN.");
return(lastTime, lastState);
}
KChartHandler.ChartClearData(style);
(string[] series, string[] seriesLNA) = GenerateSeries(reports, noise, style);
KChartHandler.LegendUpdate(style);
KScoreHandler.ScoreUpdate();
IEnumerable <SolPoint> solution = SolutionGererator(Solver, initialState, initialTime, finalTime, Flux, nonTrivialSolution, style);
List <TriggerEntry> triggers = sample.Triggers(style);
bool[] triggered = new bool[triggers.Count]; for (int i = 0; i < triggers.Count; i++)
{
triggered[i] = false;
}
// BEGIN foreach (SolPoint solPoint in solution) -- done by hand to catch exceptions in MoveNext()
SolPoint solPoint = new SolPoint(initialTime, initialState.Clone().ToArray());
bool hasSolPoint = false;
var enumerator = solution.GetEnumerator();
do
{
// Handle triggers first, they can apply to the initial state
if (triggers.Count > 0)
{
State state = null; // allocated on need from solPoint
State modifiedState = null; // allocated on need from state
for (int i = 0; i < triggers.Count; i++)
{
if (triggered[i] == false)
{
TriggerEntry trigger = triggers[i];
if (state == null)
{
state = new State(sample.Count(), lna: noise != Noise.None).InitAll(solPoint.X);
}
if (trigger.condition.ObserveBool(sample, solPoint.T, state, Flux, style))
{
if (modifiedState == null)
{
modifiedState = state.Clone();
}
double rawValue = trigger.assignment.ObserveMean(sample, solPoint.T, state, Flux, style);
double assignment = trigger.sample.stateMap.NormalizeDimension(trigger.target, rawValue, trigger.dimension, trigger.sample.Volume(), style);
int index = sample.stateMap.IndexOf(trigger.target.symbol);
modifiedState.SetMean(index, assignment);
if (noise != Noise.None && trigger.assignmentVariance != null)
{
double rawValueVariance = trigger.assignmentVariance.ObserveMean(sample, solPoint.T, state, Flux, style);
double assignmentVariance = trigger.sample.stateMap.NormalizeDimension(trigger.target, rawValueVariance, trigger.dimension, trigger.sample.Volume(), style);
modifiedState.SetCovar(index, index, assignmentVariance);
}
triggered[i] = true;
}
}
}
if (modifiedState != null) //restart the solver
{
State newState = modifiedState; // new State(sample.Count(), lna: noise != Noise.None).InitAll(modifiedState.ToArray());
solution = SolutionGererator(Solver, newState, solPoint.T, finalTime, Flux, nonTrivialSolution, style);
enumerator = solution.GetEnumerator();
}
}
try {
if (!enumerator.MoveNext())
{
break;
}
solPoint = enumerator.Current; // get next step of integration from solver
hasSolPoint = true;
}
catch (ConstantEvaluation e) { // stop simulation but allow execution to proceed
Gui.Log("Simulation stopped and ignored: cannot evaluate constant '" + e.Message + "'");
return(lastTime, lastState);
}
catch (Error e) { throw new Error(e.Message); }
catch (Exception e) { KChartHandler.ChartUpdate(style, false); throw new Error("ODE Solver FAILED: " + e.Message); }
pointsCounter++;
// LOOP BODY of foreach (SolPoint solPoint in solution):
if (!Exec.IsExecuting())
{
KChartHandler.ChartUpdate(style); throw new ExecutionEnded("");
} // break;
if (style.chartOutput) // Plot the new solution point
{
if (solPoint.T >= densityTick) // avoid drawing too many points
{
State state = new State(sample.Count(), lna: noise != Noise.None).InitAll(solPoint.X);
for (int i = 0; i < reports.Count; i++)
{
if (series[i] != null) // if a series was actually generated from this report
// generate deterministic series
{
if ((noise == Noise.None && reports[i].flow.HasDeterministicValue()) ||
(noise != Noise.None && reports[i].flow.HasStochasticMean()))
{
double mean = reports[i].flow.ObserveMean(sample, solPoint.T, state, Flux, style);
KChartHandler.ChartAddPoint(series[i], solPoint.T, mean, 0.0, Noise.None);
}
// generate LNA-dependent series
if (noise != Noise.None && reports[i].flow.HasStochasticVariance() && !reports[i].flow.HasNullVariance())
{
double mean = reports[i].flow.ObserveMean(sample, solPoint.T, state, Flux, style);
double variance = reports[i].flow.ObserveVariance(sample, solPoint.T, state, style);
KChartHandler.ChartAddPoint(seriesLNA[i], solPoint.T, mean, variance, noise);
}
}
}
renderedCounter++;
densityTick += densityStep;
}
if (solPoint.T >= redrawTick) // avoid redrawing the plot too often
{
KChartHandler.ChartUpdate(style, incremental: true);
redrawTick += redrawStep;
}
}
lastTime = solPoint.T;
// END foreach (SolPoint solPoint in solution)
} while (true);
if (hasSolPoint)
{
lastState = new State(sample.Count(), lna: noise != Noise.None).InitAll(solPoint.X);
}
KChartHandler.ChartUpdate(style, incremental: false);
return(lastTime, lastState);
}
