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); } }