// Declarations that hold values from read records & declarations for processing state information
// CalculateMachineSpeed calculates the speed of the machine in meters per second
private double CalculateMachineSpeed()
{
XYZ CentrePointFrom;
XYZ CentrePointTo;
if (!_LeftPoint.IsNull && !_RightPoint.IsNull && !DataLeft.IsNull && !DataRight.IsNull)
{
CentrePointFrom = (_LeftPoint + _RightPoint) * 0.5;
CentrePointTo = (DataLeft + DataRight) * 0.5;
}
else
if (!_LeftTrackPoint.IsNull && !_RightTrackPoint.IsNull && !DataTrackLeft.IsNull && !DataTrackRight.IsNull)
{
CentrePointFrom = (_LeftTrackPoint + _RightTrackPoint) * 0.5;
CentrePointTo = (DataTrackLeft + DataTrackRight) * 0.5;
}
else
if (!_LeftWheelPoint.IsNull && !_RightWheelPoint.IsNull && !DataWheelLeft.IsNull && !DataWheelRight.IsNull)
{
CentrePointFrom = (_LeftWheelPoint + _RightWheelPoint) * 0.5;
CentrePointTo = (DataWheelLeft + DataWheelRight) * 0.5;
}
else
{
return(Consts.NullDouble);
}
double DistanceTraveled = XYZ.Get3DLength(CentrePointFrom, CentrePointTo); // meters converted to kilometers...
double TravelTime = (_DataTime - _DataTimePrevious).TotalMilliseconds / 1000; // milliseconds converted to seconds...
return(TravelTime > 0 ? DistanceTraveled / TravelTime : 0.0);
}
public void Test_XYZTests_Get3DLength()
{
XYZ a = new XYZ(0, 0, 0);
XYZ b = new XYZ(1, 1, 1);
Assert.True(Math.Abs(XYZ.Get3DLength(a, b) - Math.Sqrt(3)) < 0.00001, "3D Length not as expected");
a = new XYZ(1, 2, 3);
b = new XYZ(3, 4, 5);
Assert.True(Math.Abs(XYZ.Get3DLength(a, b) - Math.Sqrt(12)) < 0.00001, "3D Length not as expected");
}
