79 lines
2.2 KiB
Dart
79 lines
2.2 KiB
Dart
import 'dart:math';
|
|
|
|
/// ========== RK4 for BASHFORTH-ADAMS FORMULATION ==========
|
|
double bashforthAdamsRK4(
|
|
List<List<double>> afRK4Table,
|
|
double fApexCurvature,
|
|
double fShapeFactor,
|
|
double fSign,
|
|
) {
|
|
double A = fApexCurvature;
|
|
double A2 = 2.0 / A;
|
|
double B = fShapeFactor;
|
|
// double KX0 = 1.0;
|
|
// double KT0 = 1.0;
|
|
double inv6 = 1.0 / 6.0;
|
|
double fTotalDiffSquared = 0.0;
|
|
int n = 1;
|
|
|
|
while (n < afRK4Table.length) {
|
|
double x = afRK4Table[n - 1][3]; // predicted x
|
|
double y = afRK4Table[n - 1][1]; // actual y
|
|
double t = afRK4Table[n - 1][4]; // predicted angle
|
|
double dy = afRK4Table[n][2]; // actual y diff
|
|
double AG = 1.0; // AG: Added
|
|
double ydy = y + 0.5 * dy;
|
|
double KX1 = dy * (1.0 / tan(t));
|
|
double KT1 =
|
|
dy * (1.0 / sin(t)) * (A2 - (B * y) - (sin(t) / (AG * x))); // A -> AG
|
|
double KX2 = dy * (1.0 / tan(t + 0.5 * KT1));
|
|
double STKT1 = sin(t + 0.5 * KT1);
|
|
double KT2 =
|
|
dy *
|
|
(1.0 / STKT1) *
|
|
(A2 - (B * ydy) - (STKT1 / (AG * (x + 0.5 * KX1)))); // A -> AG
|
|
double KX3 = dy * (1.0 / tan(t + 0.5 * KT2));
|
|
double STKT2 = sin(t + 0.5 * KT2);
|
|
double KT3 =
|
|
dy *
|
|
(1.0 / STKT2) *
|
|
(A2 - (B * ydy) - (STKT2 / (AG * (x + 0.5 * KX2)))); // A -> AG
|
|
double KX4 = dy * (1.0 / tan(t + KT3));
|
|
double STKT3 = sin(t + KT3);
|
|
double KT4 =
|
|
dy *
|
|
(1.0 / STKT3) *
|
|
(A2 - (B * (y + dy)) - (STKT3 / (AG * (x + KX3)))); // A -> AG
|
|
double KX = (KX1 + 2.0 * (KX2 + KX3) + KX4) * inv6;
|
|
double KT = (KT1 + 2.0 * (KT2 + KT3) + KT4) * inv6;
|
|
|
|
afRK4Table[n][3] = x + KX;
|
|
afRK4Table[n][4] = t + KT;
|
|
fTotalDiffSquared += pow(afRK4Table[n][0] - (fSign * afRK4Table[n][3]), 2);
|
|
n += 1;
|
|
}
|
|
|
|
return fTotalDiffSquared;
|
|
}
|
|
|
|
List<List<double>> makeRK4Table(List<double> edgePoints) {
|
|
List<double> initialValues = List.filled(13, 0.0);
|
|
|
|
List<List<double>> rk4Table = [];
|
|
rk4Table.add(initialValues);
|
|
double yPrev = 0.0;
|
|
|
|
for (var point in edgePoints) {
|
|
List<double> entryValues = List.filled(13, 0.0);
|
|
double x = point[0];
|
|
double y = point[1];
|
|
entryValues[0] = x;
|
|
entryValues[1] = y;
|
|
entryValues[2] = y - yPrev;
|
|
yPrev = y;
|
|
rk4Table.add(entryValues);
|
|
}
|
|
|
|
return rk4Table;
|
|
}
|