public double[,] SolveODESystem(double[] y, double[] x, ODESolverData data)
{
double eps = 0.00001;
double h = 0;
alglib.odesolverstate s;
int m;
double[] xtbl;
double[,] ytbl;
alglib.odesolverreport rep;
alglib.odesolverrkck(y, x, eps, h, out s);
alglib.odesolversolve(s, ODESolverFunction, data);
alglib.odesolverresults(s, out m, out xtbl, out ytbl, out rep);
return ytbl;
}
private void DispatcherTimerTick(object sender, EventArgs e)
{
time += TimeDelta;
var odeSolverData = new ODESolverData(cubeTensor, invertCubeTensor);
var ODEVector = new double[] { time - TimeDelta, time };
var yNew = SolveODESystem(yODEVector, ODEVector, odeSolverData);
for (int i = 0; i < 3; i++)
yODEVector[i] = yNew[1, i];
var quat = new Quaternion(yNew[1, 3], yNew[1, 4], yNew[1, 5], yNew[1, 6]);
quat.Normalize();
//FrameStartCube.Transform = new RotateTransform3D(new QuaternionRotation3D(quat));
var tg = new Transform3DGroup();
tg.Children.Add(new RotateTransform3D(new QuaternionRotation3D(quat)));
tg.Children.Add(new RotateTransform3D(new QuaternionRotation3D(
new Quaternion(CurrentQuaternionX, CurrentQuaternionY, CurrentQuaternionZ, CurrentQuaternionW))));
FrameStartCube.Transform = tg;
diagonalArrow.Transform = FrameStartCube.Transform;
var diagonalRotation = MatrixVectorMultiply(diagonalArrow.Transform.Value, diagonalArrow.Direction);
diagonalRotation.Normalize();
var endPoint = new Point3D(diagonalRotation.X * cubeDiagonalLength,
diagonalRotation.Y * cubeDiagonalLength, diagonalRotation.Z * cubeDiagonalLength);
trajectoryPoints.Add(endPoint);
if(time - TimeDelta > 0)
trajectoryPoints.Add(endPoint);
if (trajectoryPoints.Count > TrajectoryLength * 2 - 1)
{
trajectoryPoints.RemoveAt(0);
trajectoryPoints.RemoveAt(0);
}
yODEVector[3] = quat.X;
yODEVector[4] = quat.Y;
yODEVector[5] = quat.Z;
yODEVector[6] = quat.W;
}