From 944ba7cfd8f0977aca43bb20b61c36f8d857bd4a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jacob=20St=C3=B8ren?= <jacob.storen@ceetronsolutions.com>
Date: Mon, 17 Sep 2018 14:00:14 +0200
Subject: [PATCH] #3252 Tried updating jacobi. Not quite sccessful
---
.../Application/Tools/RiaSCurveCalculator.cpp | 132 +++++++++++-------
1 file changed, 84 insertions(+), 48 deletions(-)
diff --git a/ApplicationCode/Application/Tools/RiaSCurveCalculator.cpp b/ApplicationCode/Application/Tools/RiaSCurveCalculator.cpp
index 560e3a8e43..5f06b074c2 100644
--- a/ApplicationCode/Application/Tools/RiaSCurveCalculator.cpp
+++ b/ApplicationCode/Application/Tools/RiaSCurveCalculator.cpp
@@ -701,6 +701,35 @@ RiaSCurveCalculator RiaSCurveCalculator::fromTangentsAndLength(cvf::Vec3d p1, do
return curveFromControlPoints;
}
+//--------------------------------------------------------------------------------------------------
+///
+/// Needs to calculate J^-1 * [R1_error, R2_error]
+/// | dR1_dq1 dR1_dq2 | 1 | dR2_dq2 -dR1_dq2 |
+/// J = | | J^-1 = ---------------------------------- | |
+/// | dR2_dq1 dR2_dq2 | dR1_dq1*dR2_dq2 - dR1_dq2*dR2_dq1 | -dR2_dq1 dR1_dq1 |
+///
+/// | q1_step | | R1_Error |
+/// | | = - J^-1 | |
+/// | q2_step | | R2_Error |
+///
+//--------------------------------------------------------------------------------------------------
+void calculateNewStepsFromJacobi(double dR1_dq1, double dR1_dq2,
+ double dR2_dq1, double dR2_dq2,
+ double R1_error,
+ double R2_error,
+ double * newStepq1,
+ double * newStepq2)
+{
+ double invJacobiScale = 1.0/ (dR1_dq1*dR2_dq2 - dR2_dq1*dR1_dq2);
+ double invJacobi_R1q1 = invJacobiScale * dR2_dq2;
+ double invJacobi_R1q2 = invJacobiScale * -dR1_dq2;
+ double invJacobi_R2q1 = invJacobiScale * -dR2_dq1;
+ double invJacobi_R2q2 = invJacobiScale * dR1_dq1;
+
+ (*newStepq1) = - (invJacobi_R1q1 * R1_error + invJacobi_R1q2 * R2_error);
+ (*newStepq2) = - (invJacobi_R2q1 * R1_error + invJacobi_R2q2 * R2_error) ;
+}
+
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
@@ -721,8 +750,6 @@ void RiaSCurveCalculator::initializeWithoutSolveSpace(cvf::Vec3d p1, double azi1
double initialq1q2 = 0.2 * p1p2Length;
double deltaPos = initialq1q2 + delta;
-
-
RiaSCurveCalculator ev_0 = RiaSCurveCalculator::fromTangentsAndLength(p1, azi1, inc1, initialq1q2,
p2, azi2, inc2, initialq1q2);
RiaSCurveCalculator ev_dq1 = RiaSCurveCalculator::fromTangentsAndLength(p1, azi1, inc1, deltaPos,
@@ -740,48 +767,36 @@ void RiaSCurveCalculator::initializeWithoutSolveSpace(cvf::Vec3d p1, double azi1
double R1_error = r1 - ev_0.firstRadius();
double R2_error = r2 - ev_0.secondRadius();
- // Needs to calculate J^-1 * [R1_error, R2_error]
- // | dR1_dq1 dR1_dq2 | 1 | dR2_dq2 -dR1_dq2 |
- // J = | | J^-1 = ---------------------------------- | |
- // | dR2_dq1 dR2_dq2 | dR1_dq1*dR2_dq2 - dR1_dq2*dR2_dq1 | -dR2_dq1 dR1_dq1 |
- //
- // | q1_step | | R1_Error |
- // | | = - J^-1 | |
- // | q2_step | | R2_Error |
- //
- // (R_error_n-Rerror_n-1) - Jn-1*dq
- // J_n = Jn-1 + --------------------------------- (dq)T
- // | dqn |^2
- //
+ double q1StepJ;
+ double q2StepJ;
-
- double invJacobiScale = 1.0/ (dR1_dq1*dR2_dq2 - dR2_dq1*dR1_dq2);
- double invJacobi_R1q1 = invJacobiScale * dR2_dq2;
- double invJacobi_R1q2 = invJacobiScale * -dR1_dq2;
- double invJacobi_R2q1 = invJacobiScale * -dR2_dq1;
- double invJacobi_R2q2 = invJacobiScale * dR1_dq1;
-
- double q1StepJ = - (invJacobi_R1q1 * R1_error + invJacobi_R1q2 * R2_error);
- double q2StepJ = - (invJacobi_R2q1 * R1_error + invJacobi_R2q2 * R2_error) ;
-
-
- double q1R1Step = -R1_error/dR1_dq1;
- //double q1R2Step = R2_error/dR2_dq1;
- //double q2R1Step = R1_error/dR1_dq2;
- double q2R2Step = -R2_error/dR2_dq2;
+ calculateNewStepsFromJacobi(dR1_dq1, dR1_dq2,
+ dR2_dq1, dR2_dq2,
+ R1_error, R2_error,
+ &q1StepJ, &q2StepJ);
+ #define USE_JACOBI_UPDATE
+ #ifdef USE_JACOBI_UPDATE
+ double q1Step = q1StepJ;
+ double q2Step = q2StepJ;
+ #else
+ double q1Step = -R1_error/dR1_dq1;
+ double q2Step = -R2_error/dR2_dq2;
+ #endif
double q1 = initialq1q2;
double q2 = initialq1q2;
const int maxIterations = 40;
const double maxError = 0.01;
+ const double closeError = 40*maxError;
const double maxStepSize = 1.0e9;
const double maxLengthToQ = 1.0e10;
std::cout << std::endl;
std::cout << "Targets: R1, R2: " << r1 << " , " << r2 << std::endl;
std::cout << "Jacobi Steps: " << q1StepJ << ", " << q2StepJ << std::endl;
- std::cout << 0 << ": " << q1R1Step << " , " << q2R2Step << " : " << q1 << " , " << q2 << " | "
+ std::cout << 0 << ": " << q1Step << " , " << q2Step
+ << " : " << q1 << " , " << q2 << " | "
<< ev_0.isOk() << " : " << ev_0.firstRadius() << " , " << ev_0.secondRadius() << std::endl;
for (int iter = 1; iter < maxIterations; ++iter)
@@ -789,41 +804,62 @@ void RiaSCurveCalculator::initializeWithoutSolveSpace(cvf::Vec3d p1, double azi1
std::string q1R1StepCorrMarker;
std::string q2R2StepCorrMarker;
- if (q1 + q1R1Step < 0) { q1R1Step = -0.9*q1; q1R1StepCorrMarker = "*";}
- if (q2 + q2R2Step < 0) { q2R2Step = -0.9*q2; q2R2StepCorrMarker = "*"; }
+ if (q1 + q1Step < 0) { q1Step = -0.9*q1; q1R1StepCorrMarker = "*";}
+ if (q2 + q2Step < 0) { q2Step = -0.9*q2; q2R2StepCorrMarker = "*"; }
- q1 += q1R1Step;
- q2 += q2R2Step;
+ q1 += q1Step;
+ q2 += q2Step;
if (fabs(q1) > maxLengthToQ || fabs(q2) > maxLengthToQ) break;
- std::cout << iter << ": " << q1R1Step << q1R1StepCorrMarker << " , " << q2R2Step<< q2R2StepCorrMarker ;
+ std::cout << iter << ": " << q1Step << q1R1StepCorrMarker << " , " << q2Step<< q2R2StepCorrMarker ;
RiaSCurveCalculator ev_1 = RiaSCurveCalculator::fromTangentsAndLength(p1, azi1, inc1, q1,
p2, azi2, inc2, q2);
- std::cout << " : " << q1 << " , " << q2 << " | " << ev_1.isOk() << " : " << ev_1.firstRadius() << " , " << ev_1.secondRadius() << std::endl;
+ std::cout << " : " << q1 << " , " << q2 << " | "
+ << ev_1.isOk() << " : " << ev_1.firstRadius() << " , " << ev_1.secondRadius() << std::endl;
- /// Update Jacobi using Broyden
- //double dR1_error = (r1 - ev_1.firstRadius()) - R1_error;
- //double dR2_error = (r2 - ev_1.secondRadius()) - R2_error;
- //
- //dR1_dq1 = dR1_error - ;
+ #ifdef USE_JACOBI_UPDATE
+ /// Update Jacobi using Broyden
+ // (R_error_n-Rerror_n-1) - Jn-1*dq
+ // J_n = Jn-1 + --------------------------------- (dq)T
+ // | dqn |^2
+ //
+
+ double dR1_error = (r1 - ev_1.firstRadius()) - R1_error;
+ double dR2_error = (r2 - ev_1.secondRadius()) - R2_error;
+ R1_error = r1 - ev_1.firstRadius();
+ R2_error = r2 - ev_1.secondRadius();
+
+ double stepNormScale = 1.0/(q1Step*q1Step + q2Step*q2Step);
+
+ dR1_dq1 = dR1_dq1 + stepNormScale * (q1Step * (dR1_error - q1Step * dR1_dq1 + q2Step * dR1_dq2) );
+ dR1_dq2 = dR1_dq2 + stepNormScale * (q2Step * (dR1_error - q1Step * dR1_dq1 + q2Step * dR1_dq2) );
+ dR2_dq1 = dR2_dq1 + stepNormScale * (q1Step * (dR2_error - q1Step * dR2_dq1 + q2Step * dR2_dq2) );
+ dR2_dq2 = dR2_dq2 + stepNormScale * (q2Step * (dR2_error - q1Step * dR2_dq1 + q2Step * dR2_dq2) );
+
+ calculateNewStepsFromJacobi(dR1_dq1, dR1_dq2,
+ dR2_dq1, dR2_dq2,
+ R1_error, R2_error,
+ &q1Step, &q2Step);
+
+ #else
+ dR1_dq1 = ((r1 - ev_1.firstRadius()) - (r1 - ev_0.firstRadius()))/q1Step;
+ dR2_dq2 = ((r2 - ev_1.secondRadius()) - (r2 - ev_0.secondRadius()))/q2Step;
R1_error = r1 - ev_1.firstRadius();
R2_error = r2 - ev_1.secondRadius();
- dR1_dq1 = ((r1 - ev_1.firstRadius()) - (r1 - ev_0.firstRadius()))/q1R1Step;
- dR2_dq2 = ((r2 - ev_1.secondRadius()) - (r2 - ev_0.secondRadius()))/q2R2Step;
-
- q1R1Step = -R1_error/dR1_dq1;
- q2R2Step = -R2_error/dR2_dq2;
+ q1Step = -R1_error/dR1_dq1;
+ q2Step = -R2_error/dR2_dq2;
+ #endif
ev_0 = ev_1;
if ( ( fabs(R1_error) < maxError && fabs(R2_error) < maxError)
- || fabs(q1R1Step) > maxStepSize || fabs(q2R2Step) > maxStepSize )
+ || fabs(q1Step) > maxStepSize || fabs(q2Step) > maxStepSize )
{
break;
}