private double PredictCollision(Vector myDV)
{
Vector myPos = s.S;
Vector myV = s.V;
Vector targetPos = s.T;
var modTv = Math.Sqrt(Physics.mu / s.TargetOrbitR);
Vector targetV = new Vector(targetPos.y, -targetPos.x).Norm() * modTv;
//векторное произведение [v x pos] должно быть > 0 (подгонка - у нас все время движение против часовой стрелки)
if (targetV.x * targetPos.y - targetV.y * targetPos.x < 0)
{
targetV *= -1;
}
//вычисляю полупериод Хофмана - настолько нам нужно заглянуть в будущее
var targetT = 2 * Math.PI * s.TargetOrbitR / modTv;
double tmp = (s.CurrentOrbitR + s.TargetOrbitR) / (2 * s.TargetOrbitR);
var dT = targetT * Math.Sqrt(tmp * tmp * tmp);
for (int t = 0; t < dT; t++)
{
Vector nextPos, nextV;
Physics.Forecast(myPos, myV, t == 0 ? myDV : Vector.Zero, out nextPos, out nextV);
myPos = nextPos;
myV = nextV;
Physics.Forecast(targetPos, targetV, Vector.Zero, out nextPos, out nextV);
targetPos = nextPos;
targetV = nextV;
}
var dist = (targetPos - myPos).Len();
SolverLogger.Log(string.Format("Predicted collision distance: {0:F0}", dist));
return(dist);
}
public bool IsEnd()
{
bool end = solver.State != null && solver.State.Score != 0.0;
if (end)
{
SolverLogger.Log("{0} {1}", stepCount, solver.State);
}
return(end);
}
public override Vector CalculateDV()
{
// double desirableV = Math.Sqrt(Physics.mu / s.S.Len());
// SolverLogger.Log("OrbitV = " + desirableV + " RealV = " + s.V.Len());
// SolverLogger.Log("R = " + s.S.Len());
// var targetOrbit = Physics.CalculateOrbit(s.T, s.TV);
// SolverLogger.Log("TARGET ORBIT ANGLE = " + targetOrbit.TransformAngle);
dvs.MoveNext();
if (dvs.Current.Len() > 0)
{
SolverLogger.Log("IMPULSE " + dvs.Current);
}
return(dvs.Current);
}
public Vector CalcLagrangeMultipliers(Feti1FlexibilityMatrix flexibility, IFetiPreconditioner preconditioner,
Feti1Projection projection, Vector disconnectedDisplacements, Vector rigidBodyModesWork, double globalForcesNorm,
SolverLogger logger)
{
int systemOrder = flexibility.Order;
PcgMatrix pcgMatrix = DefinePcgMatrix(flexibility, projection);
PcgPreconditioner pcgPreconditioner = DefinePcgPreconditioner(preconditioner, projection);
// λ0 = Q * G * inv(G^T * Q * G) * e
Vector lagrangesParticular = projection.CalcParticularLagrangeMultipliers(rigidBodyModesWork);
// Calculate rhs of the projected interface system: rhs = P^T * (d - F * λ0)
var r0 = flexibility.Multiply(lagrangesParticular);
r0.LinearCombinationIntoThis(-1.0, disconnectedDisplacements, 1.0);
var pcgRhs = Vector.CreateZero(systemOrder);
projection.ProjectVector(r0, pcgRhs, true);
// Solve the interface problem using PCG algorithm
var pcgBuilder = new PcgAlgorithm.Builder();
pcgBuilder.MaxIterationsProvider = maxIterationsProvider;
pcgBuilder.ResidualTolerance = pcgConvergenceTolerance;
pcgBuilder.Convergence = pcgConvergenceStrategyFactory.CreateConvergenceStrategy(globalForcesNorm);
PcgAlgorithm pcg = pcgBuilder.Build();
var lagrangesBar = Vector.CreateZero(systemOrder);
IterativeStatistics stats = pcg.Solve(pcgMatrix, pcgPreconditioner, pcgRhs, lagrangesBar, true,
() => Vector.CreateZero(systemOrder));
if (!stats.HasConverged)
{
throw new IterativeSolverNotConvergedException(Feti1Solver.name + " did not converge to a solution. PCG"
+ $" algorithm run for {stats.NumIterationsRequired} iterations and the residual norm ratio was"
+ $" {stats.ResidualNormRatioEstimation}");
}
// Calculate the actual lagrange multipliers from the separation formula: λ = λ0 + P * λbar
var lagranges = Vector.CreateZero(systemOrder);
projection.ProjectVector(lagrangesBar, lagranges, false);
lagranges.AddIntoThis(lagrangesParticular);
// Log statistics about PCG execution
logger.LogIterativeAlgorithm(stats.NumIterationsRequired, stats.ResidualNormRatioEstimation);
return(lagranges);
}
public override Vector CalculateDV()
{
SolverLogger.Log("Current : V = " + s.V + " POS = " + s.S);
Vector nextV;
Vector nextPos;
Physics.Forecast(s.S, s.V, Vector.Zero, out nextPos, out nextV);
SolverLogger.Log("Forecast: V = " + nextV + " POS = " + nextPos);
double r0 = Math.Sqrt(s.Sx * s.Sx + s.Sy * s.Sy);
double r1 = s.TargetOrbitR;
double desirableV;
if (algoState == HohmannAlgoState.ReadyToJump)
{
//desirableV = GetDvForFirstJump(r1, r0);
//algoState = HohmannAlgoState.Jumping;
//var dv = GetDV(desirableV);
//avk
algoState = HohmannAlgoState.Jumping;
desirableV = Math.Sqrt(2 * Physics.mu * r1 / (r0 * (r0 + r1)));
var dv = (1 - desirableV / s.V.Len()) * s.V;
SolverLogger.Log("IMPULSE 1 " + dv.x + ", " + dv.y + "\r\n");
return(dv);
}
if ((Math.Abs(r0 - r1) < 500) && algoState == HohmannAlgoState.Jumping)
{
algoState = HohmannAlgoState.Finishing;
desirableV = GetDvForSecondJump(nextPos.Len()) * 0.71;
var dv = GetDV(desirableV);
//avk
//algoState = HohmannAlgoState.Finishing;
//desirableV = Math.Sqrt(Physics.mu / r0);
//var desirableVector = new Vector(desirableV * s.Sy / s.S.Len(), -desirableV * s.Sx / s.S.Len());
//var dv = s.V - desirableVector;
SolverLogger.Log("IMPULSE 2 " + dv.x + ", " + dv.y + "\r\n");
return(dv);
}
return(new Vector(0, 0));
}
private Vector GetDV(double desirableV)
{
Vector nextV;
Vector nextPos;
Physics.Forecast(s.S, s.V, Vector.Zero, out nextPos, out nextV);
var nextR = nextPos.Len();
var desirableVector = new Vector(desirableV * nextPos.y / nextR, -desirableV * nextPos.x / nextR);
SolverLogger.Log("DesirableVector: " + desirableVector.x + ", " + desirableVector.y);
var vector = new Vector(nextV.x - desirableVector.x, nextV.y - desirableVector.y);
SolverLogger.Log("DV: " + vector.x + ", " + vector.y);
//return new Vector(0, 0);
if (s.Fuel < 5 || vector.x * vector.x + vector.y * vector.y < 1)
{
return(new Vector(0, 0));
}
return(vector);
}
public Vector CalcLagrangeMultipliers(Feti1FlexibilityMatrix flexibility, IFetiPreconditioner preconditioner,
Feti1Projection projection, Vector disconnectedDisplacements, Vector rigidBodyModesWork, double globalForcesNorm,
SolverLogger logger)
{
// PCPG starts from the particular lagrange multipliers: λ0 = Q * G * inv(G^T * Q * G) * e
Vector lagranges = projection.CalcParticularLagrangeMultipliers(rigidBodyModesWork);
IFetiPcgConvergence pcpgConvergenceStrategy =
pcgConvergenceStrategyFactory.CreateConvergenceStrategy(globalForcesNorm);
var pcpg = new PcpgAlgorithm(maxIterationsProvider, pcpgConvergenceTolerance, pcpgConvergenceStrategy);
IterativeStatistics stats =
pcpg.Solve(flexibility, preconditioner, projection, disconnectedDisplacements, lagranges);
if (!stats.HasConverged)
{
throw new IterativeSolverNotConvergedException(Feti1Solver.name + " did not converge to a solution. PCPG"
+ $" algorithm run for {stats.NumIterationsRequired} iterations and the residual norm ratio was"
+ $" {stats.ResidualNormRatioEstimation}");
}
// Log statistics about PCG execution
logger.LogIterativeAlgorithm(stats.NumIterationsRequired, stats.ResidualNormRatioEstimation);
return(lagranges);
}
public static void TestInterfaceProblemSolution()
{
// Setup the model and solver
Model model = Quads4x4MappingMatricesTests.CreateModel();
Dictionary <int, HashSet <INode> > cornerNodes = Quads4x4MappingMatricesTests.DefineCornerNodes(model);
var fetiMatrices = new DenseFetiDPSubdomainMatrixManager.Factory();
var cornerNodeSelection = new UsedDefinedCornerNodes(cornerNodes);
var solver = new FetiDPSolver.Builder(cornerNodeSelection, fetiMatrices).BuildSolver(model);
model.ConnectDataStructures();
solver.OrderDofs(false);
solver.Initialize();
// Mock the stiffness matrices and force vectors
Dictionary <int, IFetiDPSubdomainMatrixManager> matrixManagers = MockStiffnesses();
Dictionary <int, IFetiSubdomainMatrixManager> matrixManagersPreconditioning = MockStiffnessesForPreconditioning();
Dictionary <int, Matrix> Krr = MatricesKrr;
Vector dr = VectorDr;
// Access private fields of FetiDPSolver
FieldInfo fi = typeof(FetiDPSolver).GetField("lagrangeEnumerator", BindingFlags.NonPublic | BindingFlags.Instance);
var lagrangeEnumerator = (FetiDPLagrangeMultipliersEnumerator)fi.GetValue(solver);
fi = typeof(FetiDPSolver).GetField("dofSeparator", BindingFlags.NonPublic | BindingFlags.Instance);
var dofSeparator = (FetiDPDofSeparator)fi.GetValue(solver);
// Hardcoded coarse problem matrix and rhs
var coarseSolver = new DenseFetiDPCoarseProblemSolver(model.Subdomains);
Vector globalFcStar = VectorFcStar;
Matrix inverseKccStar = MatrixKccStar.Invert(); // It must be set as a private field using reflection.
fi = typeof(DenseFetiDPCoarseProblemSolver).GetField("inverseGlobalKccStar",
BindingFlags.NonPublic | BindingFlags.Instance);
fi.SetValue(coarseSolver, inverseKccStar);
// Dirichlet preconditioner
var precondFactory = new DirichletPreconditioner.Factory();
var repackagedKrr = new Dictionary <int, IMatrixView>();
foreach (var idMatrixPair in Krr)
{
repackagedKrr[idMatrixPair.Key] = idMatrixPair.Value;
}
var stiffnessDistribution = new HomogeneousStiffnessDistribution(model, dofSeparator);
stiffnessDistribution.Update(null);
IFetiPreconditioner preconditioner = precondFactory.CreatePreconditioner(model,
stiffnessDistribution, dofSeparator, lagrangeEnumerator, matrixManagersPreconditioning);
// Solve the interface problem
FetiDPInterfaceProblemSolver interfaceSolver = new FetiDPInterfaceProblemSolver.Builder().Build();
var flexibility = new FetiDPFlexibilityMatrix(dofSeparator, lagrangeEnumerator, matrixManagers);
var logger = new SolverLogger("mock FETI-DP");
(Vector lagranges, Vector uc) = interfaceSolver.SolveInterfaceProblem(
flexibility, preconditioner, coarseSolver, globalFcStar, dr, GlobalForcesNorm, logger);
// Check against expected solution
double tol = 1E-7;
Assert.True(SolutionLagrangeMultipliers.Equals(lagranges, tol));
Assert.True(SolutionCornerDisplacements.Equals(uc, tol));
}
public override Vector CalculateDV()
{
//проверить, что крутимся в одну сторону
Vector nextV;
Vector nextPos;
Physics.Forecast(s.S, s.V, Vector.Zero, out nextPos, out nextV);
double r0 = s.CurrentOrbitR;
double r1 = s.TargetOrbitR;
if (algoState == HohmannAlgoState.ReadyToJump)
{
double tmp = (r0 + r1) / (2 * r1);
var tau = Math.Sqrt(tmp * tmp * tmp);
double desiredPhi = r1 > r0 ? Math.PI * (1 - tau) : Math.PI * (tau - 1);
var thetaS = s.S.PolarAngle;
if (thetaS < 0)
{
thetaS += 2 * Math.PI;
}
var thetaT = s.T.PolarAngle;
if (thetaT < 0)
{
thetaT += 2 * Math.PI;
}
var actualPhi = r1 > r0 ? thetaS - thetaT : thetaT - thetaS;
if (actualPhi < 0)
{
actualPhi += 2 * Math.PI;
}
SolverLogger.Log(string.Format("DesiredPhi: {0}, ActualPhi: {1}, Diff=: {2}", desiredPhi * 180 / Math.PI,
actualPhi * 180 / Math.PI, desiredPhi - actualPhi));
if (algoState == HohmannAlgoState.ReadyToJump && Math.Abs(desiredPhi - actualPhi) < Eps)
{
SolverLogger.Log(string.Format("My POS: {0}, V: {1}", s.S, s.V));
SolverLogger.Log(string.Format("Target POS: {0}", s.T));
algoState = HohmannAlgoState.Jumping;
double desirableV = Math.Sqrt(2 * Physics.mu * r1 / (r0 * (r0 + r1)));
//var dv = GetDV(desirableV);
return((1 - desirableV / s.V.Len()) * s.V);
}
}
if (algoState == HohmannAlgoState.Jumping)
{
if (((int)s.ST.Len() < 500))
{
algoState = HohmannAlgoState.Finishing;
return(GetStabilizingJump(s.V, s.S));
}
else
{
SolverLogger.Log("DISTANCE TO = " + s.ST.Len());
}
}
if (algoState == HohmannAlgoState.Finishing)
{
double myOrbit = s.S.Len();
double myOrbitV = Math.Sqrt(Physics.mu / myOrbit);
double myV = s.V.Len();
double hisOrbitV = Math.Sqrt(Physics.mu / s.TargetOrbitR);
SolverLogger.Log(string.Format("Gagarin: {0} Orbit={1} V={2} OrbitV={3}", s.S, myOrbit, myV, myOrbitV));
SolverLogger.Log(string.Format("Target : {0} Orbit={1} V={2}", s.T, s.TargetOrbitR, hisOrbitV));
SolverLogger.Log(string.Format("Summary: ErrV = {0} ErrOrbit={1} DistanceToTarget={2}",
s.V.Len() - myOrbitV,
myOrbit - s.TargetOrbitR,
s.ST.Len()
));
}
return(new Vector(0, 0));
}
public (Vector lagrangeMultipliers, Vector cornerDisplacements) SolveInterfaceProblem(FetiDPFlexibilityMatrix flexibility,
IFetiPreconditioner preconditioner, IFetiDPCoarseProblemSolver coarseProblemSolver,
Vector globalFcStar, Vector dr, double globalForcesNorm, SolverLogger logger)
{
int systemOrder = flexibility.Order;
// Matrix, preconditioner & rhs
var pcgMatrix = new InterfaceProblemMatrix(flexibility, coarseProblemSolver);
var pcgPreconditioner = new InterfaceProblemPreconditioner(preconditioner);
Vector pcgRhs = CreateInterfaceProblemRhs(flexibility, coarseProblemSolver, globalFcStar, dr);
// Solve the interface problem using PCG algorithm
var pcgBuilder = new PcgAlgorithm.Builder();
pcgBuilder.MaxIterationsProvider = maxIterationsProvider;
pcgBuilder.ResidualTolerance = pcgConvergenceTolerance;
pcgBuilder.Convergence = pcgConvergenceStrategyFactory.CreateConvergenceStrategy(globalForcesNorm);
PcgAlgorithm pcg = pcgBuilder.Build(); //TODO: perhaps use the pcg from the previous analysis if it has reorthogonalization.
var lagranges = Vector.CreateZero(systemOrder);
IterativeStatistics stats = pcg.Solve(pcgMatrix, pcgPreconditioner, pcgRhs, lagranges, true,
() => Vector.CreateZero(systemOrder));
// Log statistics about PCG execution
if (!stats.HasConverged)
{
throw new IterativeSolverNotConvergedException(FetiDPSolver.name + " did not converge to a solution. PCG"
+ $" algorithm run for {stats.NumIterationsRequired} iterations and the residual norm ratio was"
+ $" {stats.ResidualNormRatioEstimation}");
}
logger.LogIterativeAlgorithm(stats.NumIterationsRequired, stats.ResidualNormRatioEstimation);
// Calculate corner displacements: uc = inv(KccStar) * (fcStar + FIrc^T * lagranges)
Vector uc = flexibility.MultiplyTransposedFIrc(lagranges);
uc.AddIntoThis(globalFcStar);
uc = coarseProblemSolver.MultiplyInverseCoarseProblemMatrixTimes(uc);
return(lagranges, uc);
}
public Vector CalculateDv2()
{
//проверить, что крутимся в одну сторону
Vector nextV;
Vector nextPos;
Physics.Forecast(s.S, s.V, Vector.Zero, out nextPos, out nextV);
var r0 = s.CurrentOrbitR;
var r1 = s.TargetOrbitR;
var distance = s.ST.Len();
if (distance < minDistance_)
{
minDistance_ = distance;
SolverLogger.Log(string.Format("Min DistanceToTarget={0:F0}, CurrentR={1:F0}, TargetR={2:F0}", minDistance_, r0, r1));
}
if (algoState == HohmannAlgoState.ReadyToJump)
{
double tmp = (r0 + r1) / (2 * r1);
var tau = Math.Sqrt(tmp * tmp * tmp);
double desiredPhi = r1 > r0 ? Math.PI * (1 - tau) : Math.PI * (tau - 1);
var thetaS = s.S.PolarAngle;
if (thetaS < 0)
{
thetaS += 2 * Math.PI;
}
var thetaT = s.T.PolarAngle;
if (thetaT < 0)
{
thetaT += 2 * Math.PI;
}
var actualPhi = r1 > r0 ? thetaS - thetaT : thetaT - thetaS;
if (actualPhi < 0)
{
actualPhi += 2 * Math.PI;
}
bool jump = false;
var difference = desiredPhi - actualPhi;
if (prevDifference_.HasValue)
{
jump = prevDifference_ * difference < 0;
}
prevDifference_ = difference;
if (jump)
//if (Math.Abs(desiredPhi - actualPhi) < 0.01)
{
//SolverLogger.Log(string.Format("My POS: {0}, V: {1}", s.S, s.V));
//SolverLogger.Log(string.Format("Target POS: {0}", s.T));
var desirableV = GetDvForFirstJump(r1, r0);
var dv = GetDV(desirableV);
//var desirableV = Math.Sqrt(2 * Physics.mu * r1 / (r0 * (r0 + r1)));
//var dv = (1 - desirableV / s.V.Len()) * s.V;
if (PredictCollision(dv) < 1000)
{
SolverLogger.Log("IMPULSE 1 " + dv.x + ", " + dv.y + "\r\n");
algoState = HohmannAlgoState.Jumping;
return(dv);
}
}
}
if (algoState == HohmannAlgoState.Jumping && (Math.Abs(r0 - r1) < 10))
//if (algoState == HohmannAlgoState.Jumping && distance < 1000)
{
//algoState = HohmannAlgoState.Finishing;
//var desirableV = GetDvForSecondJump(nextPos.Len()) * 0.71;
//var dv = GetDV(desirableV);
algoState = HohmannAlgoState.Finishing;
var desirableV = Math.Sqrt(Physics.mu / r1);
var dv = (1 - desirableV / s.V.Len()) * s.V;
SolverLogger.Log("IMPULSE 2 " + dv.x + ", " + dv.y + "\r\n");
return(dv);
}
return(new Vector(0, 0));
}
private IEnumerable <Vector> GetSolutions()
{
yield return(Physics.GetStabilizingJump(s.V, s.S));
yield return(Physics.GetStabilizingJump(s.V, s.S));
var orbit = Physics.CalculateOrbit(s.T, s.TV);
var polar = Normalize(s.S.PolarAngle);
var tangle = Normalize(orbit.TransformAngle + Math.PI);
while (Math.Abs(polar - tangle) > EPS)
{
polar = Normalize(s.S.PolarAngle);
tangle = Normalize(orbit.TransformAngle + Math.PI);
SolverLogger.Log("Polar = " + polar);
SolverLogger.Log("Orbit = " + tangle);
yield return(new Vector(0, 0));
}
var perigeyDistance = orbit.a * (1 - orbit.e);
var phi = (s.T.PolarAngle - orbit.TransformAngle);
var area = orbit.S(phi); //phi * s.T.Len2() / 2;
var waitTime = area / CalculateH(s.T, s.T - s.TV);
var tempR = 13736813.23;
yield return(Physics.CalculateHohmannJump(s.S, s.V, tempR));
polar = Normalize(s.S.PolarAngle);
tangle = Normalize(orbit.TransformAngle);
while (Math.Abs(polar - tangle) > EPS)
{
polar = Normalize(s.S.PolarAngle);
tangle = Normalize(orbit.TransformAngle);
yield return(Vector.Zero);
}
yield return(Physics.CalculateHohmannJump(s.S, s.V, perigeyDistance) * 1.00001);
polar = Normalize(s.S.PolarAngle);
tangle = Normalize(orbit.TransformAngle + Math.PI);
while (Math.Abs(polar - tangle) > 0.01)
{
polar = Normalize(s.S.PolarAngle);
tangle = Normalize(orbit.TransformAngle + Math.PI);
// SolverLogger.Log("Polar = " + polar);
// SolverLogger.Log("Orbit = " + tangle);
yield return(new Vector(0, 0));
}
SolverLogger.Log("DISTANCE = " + (s.S - s.T).Len());
var dv = s.V - s.TV;
SolverLogger.Log("DV = " + dv);
var d = (s.T.PolarAngle - s.S.PolarAngle);
SolverLogger.Log("O = " + d);
var vt = Physics.mu * (1 + orbit.e) / CalculateH(s.T, s.T - s.TV) / 2;
yield return(dv); // *0.999939;
//yield return s.V - Physics.GetTangentVector(s.S, vt);
var len = (s.S - s.T).Len();
int count = 0;
int maxCount = 0;
while (true)
{
len = (s.S - s.T).Len();
if (len < 1000)
{
d = (s.T.PolarAngle - s.S.PolarAngle);
SolverLogger.Log("O = " + d);
SolverLogger.Log("DISTANCE = " + len + "\tcount = " + count + "\t maxCount = " + maxCount + " o=" + d + " MER = " + s.S.Len() + " hisR=" + s.T.Len());
count++;
}
else
{
count = 0;
}
if (maxCount < count)
{
maxCount = count;
}
yield return(Vector.Zero);
}
}
