nmelone/GoldenCheetah
1
0
Fork 0
mirror of https://github.com/GoldenCheetah/GoldenCheetah.git synced 2026-02-13 16:18:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Optimize splines and bikesim dvdt. (#3349)

Browse Source 
* Optimize splines and bikesim dvdt.

* Trig identity

* Refactor to provide WattsForV.

* Optimize gps route smoothing, spline distance.
This commit is contained in:
ericchristoffersen
2020-03-11 15:15:20 -07:00
committed by GitHub
parent 0676ce1d89
commit 5ec95681f2
6 changed files with 465 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files

174
src/FileIO/FixGPS.cpp
View File

@@ -672,52 +672,168 @@ bool IsReasonableGeoLocation(geolocation *ploc) {
ploc->Alt() >= -1000 && ploc->Alt() < 10000);
}
class SaveState
{
double m_t;
double m_step;
double m_achieved;
bool m_fGoingUp;
public:
SaveState() : m_t(0.), m_step(0.), m_achieved(1000000.), m_fGoingUp(true) {};
void Save(double t, double step, double achieved, bool fGoingUp) {
m_t = t;
m_step = step;
m_achieved = achieved;
m_fGoingUp = fGoingUp;
}
void Restore(double& t, double& step, double &achieved, bool& fGoingUp) {
t = m_t;
step = m_step;
fGoingUp = m_fGoingUp;
achieved = m_achieved;
}
};
class GradientPrecisionHistory
{
static const size_t s_HistSize = 4;
double precisionHistory[s_HistSize];
SaveState saveHistory[s_HistSize];
public:
GradientPrecisionHistory() {
for (int i = 0; i < s_HistSize; i++) precisionHistory[i] = 1000000.;
}
void GetBestState(SaveState& state) {
int lowestIdx = 0;
double lowestValue = precisionHistory[lowestIdx];
for (int i = 1; i < s_HistSize; i++) {
double val = precisionHistory[i];
if (val < lowestValue)
{
lowestIdx = i;
lowestValue = precisionHistory[i];
}
}
state = saveHistory[lowestIdx];
}
bool Push(double newDelta, SaveState state) {
int highestIdx = 0;
double highestValue = precisionHistory[highestIdx];
for (int i = 1; i < s_HistSize; i++) {
double val = precisionHistory[i];
if (val > highestValue) {
highestValue = val;
highestIdx = i;
}
}
// Only dealing in magnitudes here.
newDelta = fabs(newDelta);
// Making progress if new value is less than the biggest value in history.
if (newDelta < highestValue) {
precisionHistory[highestIdx] = newDelta;
saveHistory[highestIdx] = state;
return true;;
}
return false;
}
};
// Gradient descent requires 2x the compute per evaluation but usually seens 7x fewer evalutaions,
// so > 3x speedup.
// Returns delta from epsilon, 0. if perfect resolution.
template <typename T_Curve, typename T_Pos, bool T_GradientDescent>
bool EvalAtTargetDistance(T_Curve &curve, double targetDistance, double epsilon, unsigned sampleCount, double &prevT, T_Pos &pos, unsigned &evalCount)
double EvalAtTargetDistance(T_Curve &curve, double targetDistance, double epsilon, unsigned sampleCount, double &prevT, T_Pos &pos, unsigned &evalCount)
{
static const int s_iGradientDegree = T_GradientDescent ? 1 : 0;
double t = prevT;
double step = 1 / (double)sampleCount; // default step size if gradient descent isn't used.
double step = std::max(0.1, 1 / (double)sampleCount); // default step size if gradient descent isn't used.
bool fGoingUp = true;
bool fGradientPossible = true;
double achievedPrecision = 1000000.;
GradientPrecisionHistory gph;
SaveState gss;
gss.Save(t, step, achievedPrecision,fGoingUp);
while (true) {
evalCount++;
// Fail if no convegence after 100 tries.
if (evalCount > 100)
return false;
return achievedPrecision;
T_Pos evalResults[1 + s_iGradientDegree];
curve.Evaluate(t, s_iGradientDegree, evalResults);
pos = evalResults[0];
double delta = pos[0] - targetDistance;
if (fabs(delta) < epsilon) {
achievedPrecision = pos[0] - targetDistance;
if (fabs(achievedPrecision) < epsilon) {
prevT = t;
break;
}
bool fSuccess = false;
if (T_GradientDescent) {
// Use gradient to predict next sample point.
double evalRise = evalResults[1][0];
double evalStep = -(delta / evalRise);
if (T_GradientDescent && fGradientPossible) {
// Gradient descent can fail if unlucky.
// If suggested next sample point is out of range then fall back
// to binary search.
if (t + evalStep >= 0. && t + evalStep <= 1.) {
step = evalStep;
fSuccess = true;
if (!gph.Push(achievedPrecision, gss)) {
fGradientPossible = false;
} else {
// Try to use gradient to predict next sample point.
double evalRise = evalResults[1][0];
// Abandon gradient descent if slope is too close to zero.
if (evalRise == 0.) {
fGradientPossible = false;
}
else {
int exp;
std::frexp(evalRise, &exp);
if (exp < -10) {
fGradientPossible = false;
}
}
if (fGradientPossible) {
double evalStep = -(achievedPrecision / evalRise);
// Gradient descent can fail if unlucky.
// If suggested next sample point is out of range then fall back
// to binary search.
if (t + evalStep >= 0. && t + evalStep <= 1.) {
gss.Save(t, step, achievedPrecision, fGoingUp);
step = evalStep;
fGoingUp = (step > 0);
fSuccess = true;
}
}
}
// Restore previos step if gradient descent ran off the rails.
if (!fGradientPossible) {
gph.GetBestState(gss);
gss.Restore(t, step, achievedPrecision, fGoingUp);
}
}
if (!fSuccess) {
bool directionChange = (delta > 0) == fGoingUp;
bool directionChange = (achievedPrecision > 0) == fGoingUp;
if (directionChange) {
step /= -2.;
fGoingUp = !fGoingUp;
@@ -727,7 +843,7 @@ bool EvalAtTargetDistance(T_Curve &curve, double targetDistance, double epsilon,
t += step;
}
return true;
return achievedPrecision;
}
// Create new distance/altitude curve
@@ -736,7 +852,11 @@ bool EvalAtTargetDistance(T_Curve &curve, double targetDistance, double epsilon,
template <typename T_Curve, typename T_Pos>
bool InterpolateBSplineCurve(const std::vector<T_Pos> &inControls, std::vector<T_Pos> &outControls, T_Curve &curve, unsigned &evalCountSum)
{
static const double precision = 0.0001; // 10cm
// Tell eval it can stop if it finds this precision.
static const double s_desiredPrecision = 0.0001; // 10cm
// Fail eval if required precision isn't achieved.
static const double s_requiredPrecision = 0.001; // 1m.
outControls.resize(0);
@@ -746,9 +866,11 @@ bool InterpolateBSplineCurve(const std::vector<T_Pos> &inControls, std::vector<T
T_Pos pos;
unsigned evalCount = 0;
bool fConvergence = EvalAtTargetDistance<T_Curve, T_Pos, true>(curve, targetDistance, precision, (unsigned)inControls.size(), t, pos, evalCount);
double obtainedPrecision = EvalAtTargetDistance<T_Curve, T_Pos, true>(curve, targetDistance, s_desiredPrecision, (unsigned)inControls.size(), t, pos, evalCount);
evalCountSum += evalCount;
if (!fConvergence) {
// Fail if required precision not met.
if (fabs(obtainedPrecision) > s_requiredPrecision) {
outControls.resize(0);
return false;
}
@@ -789,6 +911,18 @@ bool smoothAltitude(const std::vector<Vector2<double>> &inControls, unsigned deg
// Spline undefined if degree is less than 3.
if (degree0 < 3) return false;
// First thing, check for monotonaity. This smoothing
// cannot work if distance can decrease. User should
// recompute distance before try again.
double dist = 0.;
for (int i = 0; i < inControls.size(); i++) {
double newDist = inControls[i][0];
if (newDist < dist) {
return false;
}
dist = newDist;
}
// Degree can't exceed control size - 1.
degree0 = std::min(degree0, (unsigned)inControls.size() - 1);

73
src/FileIO/LocationInterpolation.cpp
View File

@@ -192,10 +192,13 @@ xyz SphericalTwoPointInterpolator::InterpolateNext(xyz p0, xyz p1)
void UnitCatmullRomInterpolator::Init(double pm1, double p0, double p1, double p2)
{
std::get<0>(m_p) = pm1;
std::get<1>(m_p) = p0;
std::get<2>(m_p) = p1;
std::get<3>(m_p) = p2;
std::get<0>(m_baseCoefs) = pm1;
std::get<1>(m_baseCoefs) = p0;
std::get<2>(m_baseCoefs) = p1;
std::get<3>(m_baseCoefs) = p2;
m_locCoefs.Invalidate();
m_tanCoefs.Invalidate();
}
UnitCatmullRomInterpolator::UnitCatmullRomInterpolator()
@@ -208,53 +211,27 @@ UnitCatmullRomInterpolator::UnitCatmullRomInterpolator(double pm1, double p0, do
Init(pm1, p0, p1, p2);
}
double UnitCatmullRomInterpolator::T()
{
// Control curvature:
// 0 is standard (straigtest)
// 0.5 is called centripetal
// 1 is chordal (loopiest)
double t = 0.5;
return t;
}
double UnitCatmullRomInterpolator::Location(double u) const
{
double t = T();
double A, B, C, D;
m_locCoefs.Get(m_baseCoefs, A, B, C, D);
double p0 = std::get<0>(m_p);
double p1 = std::get<1>(m_p);
double p2 = std::get<2>(m_p);
double p3 = std::get<3>(m_p);
// Curvature-parameterized CatmullRom equation courtesy of wolfram alpha:
// [1, u, u ^ 2, u ^ 3] * [[0, 1, 0, 0], [-t, 0, t, 0], [2 * t, t - 3, 3 - 2t, -t], [-t, 2 - t, t - 2, t]] * [p0, p1, p2, p3]
double retval = p0 * u * (u * (2 * t - t * u) - t) +
u * (u * (u * (p1 * (-t) + 2 * p1 + p2 * (t - 2) + p3 * t) + p1 * t - 3 * p1 + p2 * (3 - 2 * t) - p3 * t) + p2 * t) + p1;
double retval = (u * u * u) * A +
(u * u) * B +
(u) * C +
D;
return retval;
}
double UnitCatmullRomInterpolator::Tangent(double u) const
{
double t = T();
double A, B, C;
m_tanCoefs.Get(m_baseCoefs, A, B, C);
double p0 = std::get<0>(m_p);
double p1 = std::get<1>(m_p);
double p2 = std::get<2>(m_p);
double p3 = std::get<3>(m_p);
// Curvature-parameterized CatmullRom equation courtesy of wolfram alpha:
// [1, u, u ^ 2, u ^ 3] * [[0, 1, 0, 0], [-t, 0, t, 0], [2 * t, t - 3, 3 - 2t, -t], [-t, 2 - t, t - 2, t]] * [p0, p1, p2, p3]
// d f(u)/du
// Closed form slope for cubic at point u in 0..1.
double retval = 3 * (u *u) * ((p3 + p2 - p1 - p0) * t - 2 * p2 + 2 * p1) +
2 * (u) * ((-p3 - 2 * p2 + p1 + 2 * p0)*t + 3 * p2 - 3 * p1) +
(p2 - p0) * t;
double retval = (u * u) * A +
(u) * B +
C;
return retval;
}
@@ -264,18 +241,10 @@ double UnitCatmullRomInterpolator::Tangent(double u) const
// the distance spline which is monotonic, etc.)
bool UnitCatmullRomInterpolator::Inverse(double r, double &u) const
{
const double t = T();
double a, b, c, d;
m_locCoefs.Get(m_baseCoefs, a, b, c, d);
double p0 = std::get<0>(m_p);
double p1 = std::get<1>(m_p);
double p2 = std::get<2>(m_p);
double p3 = std::get<3>(m_p);
// Normalized form of equation from Location.
double a = ((p3 + p2 - p1 - p0) * t - 2 * p2 + 2 * p1);
double b = ((-p3 - 2 * p2 + p1 + 2 * p0)*t + 3 * p2 - 3 * p1);
double c = (p2 - p0) * t;
double d = p1 - r; // "- r" because root finder expects coefficients for expression that equals zero.
d -= r; // "- r" because root finder solves expressions that equal zero.
Roots roots = BlinnCubicSolver(a, b, c, d);

328
src/FileIO/LocationInterpolation.h
View File

@@ -20,11 +20,18 @@
#define _LOCATION_INTERPOLATION_H
#include <tuple>
#include <ratio>
#include "qwt_math.h"
struct geolocation;
#if defined(_MSC_VER)
#define NOINLINE __declspec(noinline)
#else
#define NOINLINE
#endif
class v3
{
std::tuple<double, double, double> m_t;
@@ -169,15 +176,127 @@ struct SphericalTwoPointInterpolator : public TwoPointInterpolator
xyz InterpolateNext(xyz p0, xyz p1);
};
// Coefficient caching. A large benefit is that by performing compute separate
// from use the use will be inlined. Cache efficiency depends on route point
// distances but typical rides cache succeeds 5-10 times before invalidated.
class LazyCoefficients
{
mutable bool m_CoefsValid;
public:
bool Valid() const { return m_CoefsValid; }
void Validate() const { m_CoefsValid = true; }
void Invalidate() const { m_CoefsValid = false; }
LazyCoefficients() : m_CoefsValid(false) {}
};
template <typename T_SourceCoefType, typename T_Ratio>
class LazyLocationCoefficients : public LazyCoefficients
{
// Cache of computed coefficients for location computation.
mutable std::tuple<double, double, double, double> m_coefs;
// Computes and stores coefficients for cubic splne location calculation.
NOINLINE void Compute(const T_SourceCoefType& base) const
{
// Curvature-parameterized CatmullRom equation courtesy of wolfram alpha:
// [1, u, u ^ 2, u ^ 3] * [[0, 1, 0, 0], [-t, 0, t, 0], [2 * t, t - 3, 3 - 2t, -t], [-t, 2 - t, t - 2, t]] * [p0, p1, p2, p3]
double t = (double)T_Ratio::num / (double)T_Ratio::den;
double p0 = std::get<0>(base);
double p1 = std::get<1>(base);
double p2 = std::get<2>(base);
double p3 = std::get<3>(base);
double A = ((p3 + p2 - p1 - p0) * t - 2 * p2 + 2 * p1);
double B = ((-p3 - 2 * p2 + p1 + 2 * p0) * t + 3 * p2 - 3 * p1);
double C = (p2 - p0) * t;
double D = p1;
std::get<0>(m_coefs) = A;
std::get<1>(m_coefs) = B;
std::get<2>(m_coefs) = C;
std::get<3>(m_coefs) = D;
Validate();
}
public:
void Get(const T_SourceCoefType &base, double &A, double &B, double &C, double &D) const
{
if (!Valid()) {
Compute(base);
}
A = std::get<0>(m_coefs);
B = std::get<1>(m_coefs);
C = std::get<2>(m_coefs);
D = std::get<3>(m_coefs);
}
};
template <typename T_SourceCoefType, typename T_Ratio>
class LazyTangentCoefficients : public LazyCoefficients
{
// Cache of computed coefficients for tangent computation.
mutable std::tuple<double, double, double> m_coefs;
// Computes and stores coefficients for cubic splne tangent calculation.
NOINLINE void Compute(const T_SourceCoefType &base) const
{
// d f(u)/ du of Curvature-parameterized CatmullRom equation courtesy of wolfram alpha:
// [1, u, u ^ 2, u ^ 3] * [[0, 1, 0, 0], [-t, 0, t, 0], [2 * t, t - 3, 3 - 2t, -t], [-t, 2 - t, t - 2, t]] * [p0, p1, p2, p3]
double t = (double)T_Ratio::num / (double)T_Ratio::den;
double p0 = std::get<0>(base);
double p1 = std::get<1>(base);
double p2 = std::get<2>(base);
double p3 = std::get<3>(base);
double A = 3 * ((p3 + p2 - p1 - p0) * t - 2 * p2 + 2 * p1);
double B = 2 * ((-p3 - 2 * p2 + p1 + 2 * p0)*t + 3 * p2 - 3 * p1);
double C = (p2 - p0) * t;
std::get<0>(m_coefs) = A;
std::get<1>(m_coefs) = B;
std::get<2>(m_coefs) = C;
Validate();
}
public:
LazyTangentCoefficients() {};
void Get(const T_SourceCoefType &base, double &A, double &B, double &C) const
{
if (!Valid()) {
Compute(base);
}
A = std::get<0>(m_coefs);
B = std::get<1>(m_coefs);
C = std::get<2>(m_coefs);
}
};
class UnitCatmullRomInterpolator
{
// Optimization Note: Currently everything is computed from control
// parameters in m_p. If someone wished this to run faster the
// roots and coefficients could be precomputed whenever init is called.
// Control curvature:
// 0 is standard (straigtest)
// 0.5 is called centripetal
// 1 is chordal (loopiest)
typedef std::ratio<1, 2> T;
std::tuple<double, double, double, double> m_p;
typedef std::tuple<double, double, double, double> CoefType;
static double T(); // rounding coefficient
CoefType m_baseCoefs;
LazyLocationCoefficients<CoefType, T> m_locCoefs;
LazyTangentCoefficients<CoefType, T> m_tanCoefs;
public:
@@ -359,111 +478,114 @@ public:
m_DidChange = true;
}
bool NeedsUpdate() { return m_DidChange; }
// Update interpolator if input state has changed
// This method synthesizes fake points to allow interpolation on partially filled point queue
// which occurs when there are insufficient points or at the start and end of the interpolation.
void update()
NOINLINE void Update()
{
// Queue changed, update interpolator with new points, interpolate new points if necessary
if (m_DidChange) {
xyz pm1(m_PointWindow.pm1());
xyz p0(m_PointWindow.p0());
xyz p1(m_PointWindow.p1());
xyz p2(m_PointWindow.p2());
xyz pm1(m_PointWindow.pm1());
xyz p0(m_PointWindow.p0());
xyz p1(m_PointWindow.p1());
xyz p2(m_PointWindow.p2());
switch (m_PointWindow.Count()) {
case 4:
// All points are set.
break;
case 3:
// Either pm1 or p2 is missing, interpolate the missing one.
{
xyz *s, *e, *pr;
if (!m_PointWindow.haspm1()) {
s = &p1;
e = &p0;
pr = &pm1;
} else {
s = &p0;
e = &p1;
pr = &p2;
}
*pr = this->InterpolateNext(*s, *e);
}
switch (m_PointWindow.Count()) {
case 4:
// All points are set.
break;
case 2:
{
xyz *pr0, *pr1, *s0, *s1, *e0, *e1;
// pm1 and p2
// p1 and p2
if (!m_PointWindow.haspm1() && !m_PointWindow.hasp0()) {
pr0 = &p0;
s0 = &p2;
e0 = &p1;
pr1 = &pm1;
s1 = &p1;
e1 = pr0;
} else if (!m_PointWindow.haspm1()) {
// interpolate to pm1 and p2
pr0 = &pm1;
s0 = &p1;
e0 = &p0;
pr1 = &p2;
s1 = &p0;
e1 = &p1;
} else {
// interpolate to p1 and p2
pr0 = &p1;
s0 = &pm1;
e0 = &p0;
pr1 = &p2;
s1 = &p0;
e1 = pr0;
}
*pr0 = this->InterpolateNext(*s0, *e0);
*pr1 = this->InterpolateNext(*s1, *e1);
case 3:
// Either pm1 or p2 is missing, interpolate the missing one.
{
xyz *s, *e, *pr;
if (!m_PointWindow.haspm1()) {
s = &p1;
e = &p0;
pr = &pm1;
}
break;
case 1:
// one point is present, propagate it to remaining entries.
{
xyz t = p2;
if (m_PointWindow.hasp1()) t = p1;
else if (m_PointWindow.hasp0()) t = p0;
else if (m_PointWindow.haspm1()) t = pm1;
pm1 = t;
p0 = t;
p1 = t;
p2 = t;
else {
s = &p0;
e = &p1;
pr = &p2;
}
break;
case 0:
{
// no points, default 0.0 is as good as anything.
// or assert?
xyz zero(0, 0, 0);
pm1 = zero;
p0 = zero;
p1 = zero;
p2 = zero;
}
break;
}
m_Interpolator.Init(pm1, p0, p1, p2);
m_DidChange = false;
*pr = this->InterpolateNext(*s, *e);
}
break;
case 2:
{
xyz *pr0, *pr1, *s0, *s1, *e0, *e1;
// pm1 and p2
// p1 and p2
if (!m_PointWindow.haspm1() && !m_PointWindow.hasp0()) {
pr0 = &p0;
s0 = &p2;
e0 = &p1;
pr1 = &pm1;
s1 = &p1;
e1 = pr0;
}
else if (!m_PointWindow.haspm1()) {
// interpolate to pm1 and p2
pr0 = &pm1;
s0 = &p1;
e0 = &p0;
pr1 = &p2;
s1 = &p0;
e1 = &p1;
}
else {
// interpolate to p1 and p2
pr0 = &p1;
s0 = &pm1;
e0 = &p0;
pr1 = &p2;
s1 = &p0;
e1 = pr0;
}
*pr0 = this->InterpolateNext(*s0, *e0);
*pr1 = this->InterpolateNext(*s1, *e1);
}
break;
case 1:
// one point is present, propagate it to remaining entries.
{
xyz t = p2;
if (m_PointWindow.hasp1()) t = p1;
else if (m_PointWindow.hasp0()) t = p0;
else if (m_PointWindow.haspm1()) t = pm1;
pm1 = t;
p0 = t;
p1 = t;
p2 = t;
}
break;
case 0:
{
// no points, default 0.0 is as good as anything.
// or assert?
xyz zero(0, 0, 0);
pm1 = zero;
p0 = zero;
p1 = zero;
p2 = zero;
}
break;
}
m_Interpolator.Init(pm1, p0, p1, p2);
m_DidChange = false;
}
// Interpolate between points[1] and points[2].
@@ -472,7 +594,8 @@ public:
xyz Location(double frac)
{
// Ensure interpolator has current state - synthesize fake points if needed.
update();
if (NeedsUpdate())
Update();
return m_Interpolator.Location(frac);
}
@@ -483,7 +606,8 @@ public:
xyz Tangent(double frac)
{
// Ensure interpolator has current state - synthesize fake points if needed.
update();
if (NeedsUpdate())
Update();
return m_Interpolator.Tangent(frac);
}
@@ -756,7 +880,7 @@ public:
// Step 3
double linearDistance = p2.DistanceFrom(pm1);
if (linearDistance < 0.000001)
if (linearDistance < 0.0001)
break;
// Step 4

74
src/Train/BicycleSim.cpp
View File

@@ -175,43 +175,43 @@ struct MotionStatePair
double T0() const { return m_t0; }
double T1() const { return m_t1; }
double DT() const { return m_t1 - m_t0; }
double CalcV(const BicycleSimState& s, double v, double t) const { return m_pb->V(s, v, t); }
BicycleSimState Interpolate(double t) const
{
return BicycleSimState::Interpolate(m_s0, m_s1, m_t0, m_t1, t);
}
double CalcV(double v, double t) const
{
// Linear interpolate the state info before sampling at t.
BicycleSimState st = this->Interpolate(t);
double newV = m_pb->V(st, v, t);
return newV;
}
double dVdT(double v, double t) const
{
static const double s_step = 0.01;
// Linear interpolate the state info before sampling at t.
BicycleSimState st = this->Interpolate(t);
// Linear interpolate the state info before sampling velocities at t and t+step.
BicycleSimState tn0 = this->Interpolate(t);
double v0 = CalcV(tn0, v, t);
BicycleSimState tn1 = this->Interpolate(t + s_step);
double v1 = CalcV(tn1, v, t + s_step);
double m = (v1 - v0) / s_step;
double m = m_pb->DV(st, v);
return m;
}
};
// Compute new velocity from current state and impulse duration.
double Bicycle::V(const BicycleSimState &simState, // current sim state
double v, // current velocity
double dt) const // duration that state was applied
// Returns power needed to maintain speed v against current simstate.
double Bicycle::WattsForV(const BicycleSimState &simState, double v) const
{
if (dt == 0.0) return v;
double sl = simState.Slope() / 100; // 10% = 0.1
const double CrV = 0.1; // Coefficient for velocity - dependent dynamic rolling resistance, here approximated with 0.1
double CrVn = CrV * cos(sl); // Coefficient for the dynamic rolling resistance, normalized to road inclination; CrVn = CrV*cos(slope)
double Beta = atan(sl);
double cosBeta = cos(Beta);
double sinBeta = sin(Beta);
double cosBeta = 1 / std::sqrt(sl*sl + 1); // cos(atan(sl))
double sinBeta = sl * cosBeta; // sin(atan(sl))
double m = MassKG();
double Frg = s_g0 * m * (m_constants.m_crr * cosBeta + sinBeta);
@@ -221,22 +221,49 @@ double Bicycle::V(const BicycleSimState &simState, // current sim state
const double W = 0; // Windspeed
double wattsForV = m_constants.m_Cm * v * (CdARho / 2 * (v + W)*(v + W) + Frg + v * CrVn);
return wattsForV;
}
// Return ms/s from current state, speed and impulse duration.
// This is linear application of watts over time to determine
// new speed.
double Bicycle::V(const BicycleSimState &simState, // current sim state
double v, // current speed
double dt) const // duration that state was applied
{
// Zero means no impulse, no change.
//Negative means... we're not going there.
if (dt <= 0.0) return v;
double wattsForV = WattsForV(simState, v);
double extraWatts = simState.Watts() - wattsForV;
double j = KEFromV(v);
double newJ = j + (extraWatts * dt);
double newV = VFromKE(newJ);
double j = KEFromV(v); // state and speed to joules
double newJ = j + (extraWatts * dt); // joules + (watts * time == more joules)
double newV = VFromKE(newJ); // new joules back to speed.
return newV;
}
// returns m/s^2 for current state.
double Bicycle::DV(const BicycleSimState &simState, // current sim state
double v) const // current speed
{
double newV = V(simState, v, 1.); // new speed with 1s impulse.
return (newV - v); // implicit divide by 1s.
}
template <typename T>
SpeedDistance
Integrate_EulerDirect(const T &state, double v)
{
double dt = state.DT();
double vt = state.CalcV(state.m_s1, v, dt);
double vt = state.CalcV(v, dt);
SpeedDistance ret = { vt, vt * dt};
return ret;
}
@@ -536,6 +563,7 @@ struct Integrator
Integrate_Hairer8<T>,
Integrate_KahanLi8<T>
};
return fpa[e](state, v);
}
};

12
src/Train/BicycleSim.h
View File

@@ -170,10 +170,16 @@ public:
double KEFromV(double v) const; // Compute kinetic energy of bike and rider (J) at speed v (in m/s)
double VFromKE(double ke) const; // Compute velocity (m/s) from kinetic energy of bike and rider (J)
double WattsForV(const BicycleSimState &simState, double v) const;
// Compute new velocity in m/s after simState applied for dt seconds.
double V(const BicycleSimState &simState, // current sim state
double v, // current velocity (m/s)
double dt) const; // impulse duration
double V(const BicycleSimState &simState, // current sim state
double v, // current velocity (m/s)
double dt) const; // impulse duration
double DV(const BicycleSimState &simState, // current sim state
double v) const; // current velocity
};
#endif // BICYCLESIM_H

8
src/Train/PhysicsUtility.cpp
View File

@@ -47,7 +47,6 @@ double MsToKmh(double ms)
return ms * ((60.0 * 60.0) / 1000.0);
}
// Function to compute the sustainable speed in KM/H from input params and a
// bunch of real world constants.
// Relies upon cubic root finder.
@@ -75,9 +74,10 @@ double computeInstantSpeed(double weightKG,
double m = weightKG;
double Rho = AirDensity(Hnn, T);
double Beta = atan(sl);
double cosBeta = cos(Beta);
double sinBeta = sin(Beta);
double cosBeta = 1 / std::sqrt(sl*sl + 1); // cos(atan(sl))
double sinBeta = sl * cosBeta; // sin(atan(sl))
double Frg = s_g0 * m * (crr * cosBeta + sinBeta);
// Home Rolled cubic solver: