private double calculateDistance(List <Point3D> point3D)
{
double d = 0;
for (int i = 0; i < point3D.Count - 1; i++)
{
d += DistanceCalculator.CalcDistance(point3D[i], point3D[i + 1]);
}
return(d);
}
public void Update(object sender, OnTickEventArgs eventArgs)
{
if (!Waypoints.Any())
{
return;
}
float speed = LocationModel.Parent.UnitModel.Speed;
if (DistanceCalculator.DiagonalDistance(Waypoints.Peek(), LocationModel.FloatCoords) < speed)
{
// Arrived near destination
LocationModel.FloatCoords = Waypoints.Dequeue();
if (!Waypoints.Any())
{
MoveComplete?.Invoke(this, new EventArgsWithPayload <FloatCoords>(LocationModel.FloatCoords));
}
return;
}
float xDifference = (float)DistanceCalculator.CalcDistance(LocationModel.FloatCoords.x, Waypoints.Peek().x);
float yDifference = (float)DistanceCalculator.CalcDistance(LocationModel.FloatCoords.y, Waypoints.Peek().y);
// calculate new coords
float diagonalDifference = (float)DistanceCalculator.Pythagoras(xDifference, yDifference);
float v = diagonalDifference / speed;
FloatCoords difference = new FloatCoords
{
x = xDifference / v,
y = yDifference / v
};
difference.x = Waypoints.Peek().x < LocationModel.FloatCoords.x ? -difference.x : difference.x;
difference.y = Waypoints.Peek().y < LocationModel.FloatCoords.y ? -difference.y : difference.y;
LocationModel.FloatCoords = new FloatCoords()
{
x = LocationModel.FloatCoords.x + difference.x,
y = LocationModel.FloatCoords.y + difference.y
};
}
static void Main()
{
Point3D p = new Point3D(-7, -4, 3);
Console.WriteLine(p);
Point3D q = new Point3D(17, 6, 2.5);
Console.WriteLine(q);
Console.WriteLine(DistanceCalculator.CalcDistance(p, q));
Path path = new Path(p, q, Point3D.StartingPoint);
path.WriteToFile("path.txt");
Path pathFromFIle = Path.ReadFromFile("path.txt");
Console.WriteLine(pathFromFIle);
}
//public IEnumerable<float> Get()
//{
// return new float[] { 7.512f, 4.435f};
//}
// GET api/values/5
public float Get(float x1, float y1, float x2, float y2)
{
return(DistanceCalculator.CalcDistance(new Point(x1, y1), new Point(x2, y2)));
}
private static void InsertCorrectedGps(DataTable gpsRawTable, InsertConfig.GpsCorrection correction)
{
DataTable correctedGpsTable = DataTableUtil.GetCorrectedGpsTable();
#region インデックスが 0 の場合
DataRow firstRow = correctedGpsTable.NewRow();
CopyRawDataToCorrectedRow(firstRow, gpsRawTable.Rows[0]);
firstRow.SetField(CorrectedGpsDao.ColumnDistanceDifference, 0);
firstRow.SetField(CorrectedGpsDao.ColumnSpeed, 0);
firstRow.SetField(CorrectedGpsDao.ColumnHeading, 0);
correctedGpsTable.Rows.Add(firstRow);
#endregion
for (int i = 1; i < gpsRawTable.Rows.Count - 1; i++)
{
DataRow row = correctedGpsTable.NewRow();
CopyRawDataToCorrectedRow(row, gpsRawTable.Rows[i]);
// 距離の算出
row.SetField(CorrectedGpsDao.ColumnDistanceDifference, DistanceCalculator.CalcDistance(
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLongitude)));
// 速度の算出
row.SetField(CorrectedGpsDao.ColumnSpeed, SpeedCalculator.CalcSpeed(
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i - 1].Field <DateTime>(AndroidGpsRawDao.ColumnJst),
gpsRawTable.Rows[i + 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i + 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i + 1].Field <DateTime>(AndroidGpsRawDao.ColumnJst),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLongitude)));
//速度が1km以上になったらHEADINGを更新する(停止時に1つ1つ計算するとHEADINDが暴れるため)
if (row.Field <Single>(CorrectedGpsDao.ColumnSpeed) > 1.0)
{
row.SetField(CorrectedGpsDao.ColumnHeading, HeadingCalculator.CalcHeading(
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLongitude)));
}
else
{
row.SetField(CorrectedGpsDao.ColumnHeading, correctedGpsTable.Rows[i - 1].Field <Single>(CorrectedGpsDao.ColumnHeading));
}
correctedGpsTable.Rows.Add(row);
}
#region インデックスが最後の場合
DataRow lastRow = correctedGpsTable.NewRow();
CopyRawDataToCorrectedRow(lastRow, gpsRawTable.Rows[gpsRawTable.Rows.Count - 1]);
lastRow.SetField(CorrectedGpsDao.ColumnDistanceDifference, 0);
lastRow.SetField(CorrectedGpsDao.ColumnSpeed, 0);
lastRow.SetField(CorrectedGpsDao.ColumnHeading, 0);
correctedGpsTable.Rows.Add(lastRow);
#endregion
// ファイルごとの挿入なので主キー違反があっても挿入されないだけ
if (correction == InsertConfig.GpsCorrection.SpeedLPFMapMatching)//速度にローパスフィルタを適用
{
DataTable correctedGpsSpeedLPFTable = LowPassFilter.speedLowPassFilter(correctedGpsTable, 0.05);
CorrectedGpsSpeedLPF005MMDao.Insert(correctedGpsSpeedLPFTable);
}
else if (correction == InsertConfig.GpsCorrection.MapMatching)
{
CorrectedGPSMMDao.Insert(correctedGpsTable);
}
else
{
CorrectedGpsDao.Insert(correctedGpsTable);
}
}
private static void MakeCorrectedGpsForDoppler(DataTable gpsRawTable, InsertConfig.GpsCorrection correction)
{
DataTable correctedGpsTable = DataTableUtil.GetCorrectedGpsDopplerTable();
#region インデックスが 0 の場合
DataRow firstRow = correctedGpsTable.NewRow();
CopyRawDataDopplerToCorrectedRow(firstRow, gpsRawTable.Rows[0]);
firstRow.SetField(CorrectedGpsDao.ColumnDistanceDifference, 0);
//firstRow.SetField(CorrectedGpsDao.ColumnSpeed, 0);
firstRow.SetField(CorrectedGpsDao.ColumnHeading, 0);
var meshAndAltitude = AltitudeCalculator.GetInstance().CalcAltitude(
gpsRawTable.Rows[0].Field <double>(CorrectedGpsDao.ColumnLatitude),
gpsRawTable.Rows[0].Field <double>(CorrectedGpsDao.ColumnLongitude));
firstRow.SetField(CorrectedGpsDao.ColumnTerrainAltitude, meshAndAltitude.Item2);
var linkAndTheta = LinkMatcher.GetInstance().MatchLink(
firstRow.Field <double>(CorrectedGpsDao.ColumnLatitude),
firstRow.Field <double>(CorrectedGpsDao.ColumnLongitude),
0f);
firstRow.SetField(CorrectedGpsDao.ColumnLinkId, linkAndTheta.Item1);
firstRow.SetField(CorrectedGpsDao.ColumnRoadTheta, linkAndTheta.Item2);
correctedGpsTable.Rows.Add(firstRow);
#endregion
#region インデックスがiの場合
for (int i = 1; i < gpsRawTable.Rows.Count - 1; i++)
{
DataRow row = correctedGpsTable.NewRow();
CopyRawDataDopplerToCorrectedRow(row, gpsRawTable.Rows[i]);
// 距離の算出
row.SetField(CorrectedGpsDao.ColumnDistanceDifference, DistanceCalculator.CalcDistance(
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLongitude)));
meshAndAltitude = AltitudeCalculator.GetInstance().CalcAltitude(
gpsRawTable.Rows[i].Field <double>(CorrectedGpsDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(CorrectedGpsDao.ColumnLongitude));
row.SetField(CorrectedGpsDao.ColumnTerrainAltitude, meshAndAltitude.Item2);
// 速度の算出
//row.SetField(CorrectedGpsDao.ColumnSpeed, SpeedCalculator.CalcSpeed(
// gpsRawTable.Rows[i - 1].Field<double>(AndroidGpsRawDao.ColumnLatitude),
// gpsRawTable.Rows[i - 1].Field<double>(AndroidGpsRawDao.ColumnLongitude),
// gpsRawTable.Rows[i - 1].Field<DateTime>(AndroidGpsRawDao.ColumnJst),
// gpsRawTable.Rows[i + 1].Field<double>(AndroidGpsRawDao.ColumnLatitude),
// gpsRawTable.Rows[i + 1].Field<double>(AndroidGpsRawDao.ColumnLongitude),
// gpsRawTable.Rows[i + 1].Field<DateTime>(AndroidGpsRawDao.ColumnJst),
// gpsRawTable.Rows[i].Field<double>(AndroidGpsRawDao.ColumnLatitude),
// gpsRawTable.Rows[i].Field<double>(AndroidGpsRawDao.ColumnLongitude)));
//速度が1km以上になったらHEADINGを更新する(停止時に1つ1つ計算するとHEADINDが暴れるため)
if (row.Field <Single?>(CorrectedGpsDao.ColumnSpeed) > 1.0)
{
row.SetField(CorrectedGpsDao.ColumnHeading, HeadingCalculator.CalcHeading(
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLongitude)));
}
else
{
row.SetField(CorrectedGpsDao.ColumnHeading, correctedGpsTable.Rows[i - 1].Field <double>(CorrectedGpsDao.ColumnHeading));
}
linkAndTheta = LinkMatcher.GetInstance().MatchLink(
row.Field <double>(CorrectedGpsDao.ColumnLatitude),
row.Field <double>(CorrectedGpsDao.ColumnLongitude),
Convert.ToSingle(row.Field <double>(CorrectedGpsDao.ColumnHeading))
);
row.SetField(CorrectedGpsDao.ColumnLinkId, linkAndTheta.Item1);
row.SetField(CorrectedGpsDao.ColumnRoadTheta, linkAndTheta.Item2);
correctedGpsTable.Rows.Add(row);
}
#endregion
#region インデックスが最後の場合
DataRow lastRow = correctedGpsTable.NewRow();
CopyRawDataDopplerToCorrectedRow(lastRow, gpsRawTable.Rows[gpsRawTable.Rows.Count - 1]);
lastRow.SetField(CorrectedGpsDao.ColumnDistanceDifference, 0);
lastRow.SetField(CorrectedGpsDao.ColumnSpeed, 0);
lastRow.SetField(CorrectedGpsDao.ColumnHeading, 0);
meshAndAltitude = AltitudeCalculator.GetInstance().CalcAltitude(
gpsRawTable.Rows[gpsRawTable.Rows.Count - 1].Field <double>(CorrectedGpsDao.ColumnLatitude),
gpsRawTable.Rows[gpsRawTable.Rows.Count - 1].Field <double>(CorrectedGpsDao.ColumnLongitude));
lastRow.SetField(CorrectedGpsDao.ColumnTerrainAltitude, meshAndAltitude.Item2);
linkAndTheta = LinkMatcher.GetInstance().MatchLink(
firstRow.Field <double>(CorrectedGpsDao.ColumnLatitude),
firstRow.Field <double>(CorrectedGpsDao.ColumnLongitude),
0f);
lastRow.SetField(CorrectedGpsDao.ColumnLinkId, linkAndTheta.Item1);
lastRow.SetField(CorrectedGpsDao.ColumnRoadTheta, linkAndTheta.Item2);
correctedGpsTable.Rows.Add(lastRow);
#endregion
// ファイルごとの挿入なので主キー違反があっても挿入されないだけ
if (correction == InsertConfig.GpsCorrection.DopplerSpeed)
{
CorrectedGPSDopplerDao.Insert(correctedGpsTable);
}
else if (correction == InsertConfig.GpsCorrection.DopplerNotMM)
{
CorrectedGpsDopplerNotMMDao.Insert(correctedGpsTable);
}
}
public static void InsertCorrectedGps(InsertDatum datum, InsertConfig.GpsCorrection correction)
{
var tripsTable = TripsDao.Get(datum);
foreach (DataRow tripsRow in tripsTable.Rows)
{
DataTable gpsRawTable =
AndroidGpsRawDao.Get(tripsRow.Field <DateTime>(TripsDao.ColumnStartTime),
tripsRow.Field <DateTime>(TripsDao.ColumnEndTime), datum);
if (gpsRawTable.Rows.Count == 0)
{
return;
}
DataTable correctedGpsTable = DataTableUtil.GetCorrectedGpsTable();
#region インデックスが 0 の場合
DataRow firstRow = correctedGpsTable.NewRow();
CopyRawDataToCorrectedRow(firstRow, gpsRawTable.Rows[0]);
firstRow.SetField(CorrectedGpsDao.ColumnDistanceDifference, 0);
//firstRow.SetField(CorrectedGpsDao.ColumnSpeed, 0);
firstRow.SetField(CorrectedGpsDao.ColumnHeading, 0);
var meshAndAltitude = AltitudeCalculator.GetInstance().CalcAltitude(
gpsRawTable.Rows[0].Field <double>(CorrectedGpsDao.ColumnLatitude),
gpsRawTable.Rows[0].Field <double>(CorrectedGpsDao.ColumnLongitude));
firstRow.SetField(CorrectedGpsDao.ColumnTerrainAltitude, meshAndAltitude.Item2);
var linkAndTheta = LinkMatcher.GetInstance().MatchLink(
firstRow.Field <double>(CorrectedGpsDao.ColumnLatitude),
firstRow.Field <double>(CorrectedGpsDao.ColumnLongitude),
0f, tripsRow.Field <string>(TripsDao.ColumnTripDirection), datum);
firstRow.SetField(CorrectedGpsDao.ColumnLinkId, linkAndTheta.Item1);
firstRow.SetField(CorrectedGpsDao.ColumnRoadTheta, linkAndTheta.Item2);
correctedGpsTable.Rows.Add(firstRow);
#endregion
for (int i = 1; i < gpsRawTable.Rows.Count - 1; i++)
{
DataRow row = correctedGpsTable.NewRow();
CopyRawDataToCorrectedRow(row, gpsRawTable.Rows[i]);
// 距離の算出
row.SetField(CorrectedGpsDao.ColumnDistanceDifference, DistanceCalculator.CalcDistance(
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLongitude)));
meshAndAltitude = AltitudeCalculator.GetInstance().CalcAltitude(
gpsRawTable.Rows[i].Field <double>(CorrectedGpsDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(CorrectedGpsDao.ColumnLongitude));
row.SetField(CorrectedGpsDao.ColumnTerrainAltitude, meshAndAltitude.Item2);
// 速度の算出
//row.SetField(CorrectedGpsDao.ColumnSpeed, SpeedCalculator.CalcSpeed(
// gpsRawTable.Rows[i - 1].Field<double>(AndroidGpsRawDao.ColumnLatitude),
// gpsRawTable.Rows[i - 1].Field<double>(AndroidGpsRawDao.ColumnLongitude),
// gpsRawTable.Rows[i - 1].Field<DateTime>(AndroidGpsRawDao.ColumnJst),
// gpsRawTable.Rows[i + 1].Field<double>(AndroidGpsRawDao.ColumnLatitude),
// gpsRawTable.Rows[i + 1].Field<double>(AndroidGpsRawDao.ColumnLongitude),
// gpsRawTable.Rows[i + 1].Field<DateTime>(AndroidGpsRawDao.ColumnJst),
// gpsRawTable.Rows[i].Field<double>(AndroidGpsRawDao.ColumnLatitude),
// gpsRawTable.Rows[i].Field<double>(AndroidGpsRawDao.ColumnLongitude)));
//速度が1km以上になったらHEADINGを更新する(停止時に1つ1つ計算するとHEADINDが暴れるため)
if (row.Field <Single?>(CorrectedGpsDao.ColumnSpeed) > 1.0)
{
row.SetField(CorrectedGpsDao.ColumnHeading, HeadingCalculator.CalcHeading(
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i - 1].Field <double>(AndroidGpsRawDao.ColumnLongitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLatitude),
gpsRawTable.Rows[i].Field <double>(AndroidGpsRawDao.ColumnLongitude)));
}
else
{
row.SetField(CorrectedGpsDao.ColumnHeading, correctedGpsTable.Rows[i - 1].Field <double>(CorrectedGpsDao.ColumnHeading));
}
linkAndTheta = LinkMatcher.GetInstance().MatchLink(
row.Field <double>(CorrectedGpsDao.ColumnLatitude),
row.Field <double>(CorrectedGpsDao.ColumnLongitude),
Convert.ToSingle(row.Field <double>(CorrectedGpsDao.ColumnHeading))
, tripsRow.Field <string>(TripsDao.ColumnTripDirection), datum
);
row.SetField(CorrectedGpsDao.ColumnLinkId, linkAndTheta.Item1);
row.SetField(CorrectedGpsDao.ColumnRoadTheta, linkAndTheta.Item2);
correctedGpsTable.Rows.Add(row);
}
#region インデックスが最後の場合
DataRow lastRow = correctedGpsTable.NewRow();
CopyRawDataToCorrectedRow(lastRow, gpsRawTable.Rows[gpsRawTable.Rows.Count - 1]);
lastRow.SetField(CorrectedGpsDao.ColumnDistanceDifference, 0);
lastRow.SetField(CorrectedGpsDao.ColumnSpeed, 0);
lastRow.SetField(CorrectedGpsDao.ColumnHeading, 0);
meshAndAltitude = AltitudeCalculator.GetInstance().CalcAltitude(
gpsRawTable.Rows[gpsRawTable.Rows.Count - 1].Field <double>(CorrectedGpsDao.ColumnLatitude),
gpsRawTable.Rows[gpsRawTable.Rows.Count - 1].Field <double>(CorrectedGpsDao.ColumnLongitude));
lastRow.SetField(CorrectedGpsDao.ColumnTerrainAltitude, meshAndAltitude.Item2);
linkAndTheta = LinkMatcher.GetInstance().MatchLink(
firstRow.Field <double>(CorrectedGpsDao.ColumnLatitude),
firstRow.Field <double>(CorrectedGpsDao.ColumnLongitude),
0f, tripsRow.Field <string>(TripsDao.ColumnTripDirection), datum);
lastRow.SetField(CorrectedGpsDao.ColumnLinkId, linkAndTheta.Item1);
lastRow.SetField(CorrectedGpsDao.ColumnRoadTheta, linkAndTheta.Item2);
correctedGpsTable.Rows.Add(lastRow);
#endregion
// ファイルごとの挿入なので主キー違反があっても挿入されないだけ
if (correction == InsertConfig.GpsCorrection.SpeedLPFMapMatching)//速度にローパスフィルタを適用
{
DataTable correctedGpsSpeedLPFTable = LowPassFilter.speedLowPassFilter(correctedGpsTable, 0.05);
CorrectedGpsSpeedLPF005MMDao.Insert(correctedGpsSpeedLPFTable);
}
else if (correction == InsertConfig.GpsCorrection.MapMatching)
{
CorrectedGPSMMDao.Insert(correctedGpsTable);
}
else
{
CorrectedGpsDao.Insert(correctedGpsTable);
}
}
}
public static double CalcLostEnergy(IList <Position> positions)
{
Debug.WriteLine("List Count: " + positions.Count);
AltitudeDatum altitudeBefore;
double speedBefore = 0;
double airSum = 0;
double rollingSum = 0;
double convertLossSum = 0;
double regeneLossSum = 0;
double lostEnergy = 0;
altitudeBefore = AltitudeCalculator.CalcAltitude(positions[1].Latitude, positions[1].Longitude);
for (int i = 3; i < positions.Count; i++)
{
var positionBefore = positions[i - 3];
var positionCurrent = positions[i - 2];
var positionAfter = positions[i - 1];
var distanceDiff = DistanceCalculator.CalcDistance(positionBefore.Latitude,
positionBefore.Longitude,
positionCurrent.Latitude,
positionCurrent.Longitude);
//Debug.WriteLine("DistanceDiff: " + distanceDiff);
// meter per sec
var speed = SpeedCalculator.CalcSpeed(positionBefore.Latitude,
positionBefore.Longitude,
positionBefore.Timestamp.DateTime,
positionAfter.Latitude,
positionAfter.Longitude,
positionAfter.Timestamp.DateTime,
positionCurrent.Latitude,
positionCurrent.Longitude) / 3.6;
//Debug.WriteLine("Speed: " + speed * 3.6);
if (i == 3)
{
speedBefore = speed;
}
var altitude = AltitudeCalculator.CalcAltitude(positionCurrent.Latitude, positionCurrent.Longitude);
double altitudeDiff = 0;
if (altitude != null && altitudeBefore != null)
{
altitudeDiff = altitude.Altitude - altitudeBefore.Altitude;
}
altitudeBefore = altitude;
//Debug.WriteLine("AltitudeDiff: " + altitudeDiff);
double airResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
airResistancePower = AirResistanceCalculator.CalcPower(
Constants.Rho, Car.GetLeaf().CdValue, Car.GetLeaf().FrontalProjectedArea, speed, speed);
}
//Debug.WriteLine("AirResistace: " + airResistancePower);
airSum += airResistancePower;
double rollingResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
rollingResistancePower = RollingResistanceCalculator.CalcPower(
Constants.Myu, Car.GetLeaf().Weight, Math.Atan(altitudeDiff / distanceDiff), speed);
}
//Debug.WriteLine("rollingResistancePower: " + rollingResistancePower);
rollingSum += rollingResistancePower;
double climbingResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
climbingResistancePower = ClimbingResistanceCalculator.CalcPowerPreVer(
Car.GetLeaf().Weight, altitudeDiff);
}
//Debug.WriteLine("climbingResistancePower: " + climbingResistancePower);
double accResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
accResistancePower = AccResistanceCalculator.CalcPower(
speedBefore,
positionBefore.Timestamp.DateTime,
speed,
positionCurrent.Timestamp.DateTime,
Car.GetLeaf().Weight);
}
//Debug.WriteLine("accResistancePower: " + accResistancePower);
double drivingResistancePower =
airResistancePower + rollingResistancePower + climbingResistancePower + accResistancePower;
double torque = 0;
if (drivingResistancePower > 0 && speed > 0)
{
torque = drivingResistancePower * 1000 * 3600 / speed * Car.GetLeaf().TireRadius /
Car.GetLeaf().ReductionRatio;
}
//Debug.WriteLine("torque: " + torque);
var efficiency = EfficiencyCalculator.CalcEfficiency(Car.GetLeaf(), speed, torque).Efficiency;
//Debug.WriteLine("efficiency: " + efficiency);
double convertLoss = ConvertLossCalculator.CalcEnergyPreVer(
drivingResistancePower, speed, efficiency);
//Debug.WriteLine("convertLoss: " + convertLoss);
convertLossSum += Math.Abs(convertLoss);
double regeneEnergy = RegeneEnergyCalculator.CalcEnergy(drivingResistancePower,
speed, Car.GetLeaf(), efficiency);
//Debug.WriteLine("regeneEnergy: " + regeneEnergy);
double regeneLoss = RegeneLossCalculator.CalcEnergyPreVer(drivingResistancePower, speed, efficiency);
//Debug.WriteLine($"{positionCurrent.Timestamp.DateTime}: {regeneLoss}, {efficiency}");
regeneLossSum += Math.Abs(regeneLoss);
lostEnergy += LostEnergyCalculator.CalcEnergy(convertLoss, regeneLoss, airResistancePower,
rollingResistancePower);
//Debug.WriteLine("LostEnergy: " + lostEnergy);
speedBefore = speed;
//var consumedEnergy = ConsumedEnergyCaluculator.CalcEnergy(drivingResistancePower, Car.GetLeaf(), speed, efficiency);
//Debug.WriteLine($"Efficiency: {efficiency}, CalcEfficiency: {(consumedEnergy / convertLoss) * 100}");
//LostEnergyList.Add(lostEnergy);
}
Debug.WriteLine("LostEnergy: " + lostEnergy);
Debug.WriteLine("Air: " + airSum);
Debug.WriteLine("Rolling: " + rollingSum);
Debug.WriteLine("Convert: " + convertLossSum);
Debug.WriteLine("Regene: " + regeneLossSum);
return(lostEnergy);
}
private void CalcEcolog(int count)
{
if (count == 2)
{
_altitudeBefore = AltitudeCalculator.CalcAltitude(PositionCollection[count - 1].Latitude, PositionCollection[count - 1].Longitude);
//Debug.WriteLine("AltitudeBefore: " + _altitudeBefore.Altitude);
}
else if (PositionCollection.Count > 2)
{
var positionBefore = PositionCollection[count - 3];
var positionCurrent = PositionCollection[count - 2];
var positionAfter = PositionCollection[count - 1];
var distanceDiff = DistanceCalculator.CalcDistance(positionBefore.Latitude,
positionBefore.Longitude,
positionCurrent.Latitude,
positionCurrent.Longitude);
//Debug.WriteLine("DistanceDiff: " + distanceDiff);
// meter per sec
var speed = SpeedCalculator.CalcSpeed(positionBefore.Latitude,
positionBefore.Longitude,
positionBefore.Timestamp.DateTime,
positionAfter.Latitude,
positionAfter.Longitude,
positionAfter.Timestamp.DateTime,
positionCurrent.Latitude,
positionCurrent.Longitude) / 3.6;
//Debug.WriteLine("Speed: " + speed * 3.6);
if (count == 3)
{
_speedBefore = speed;
}
var altitude = AltitudeCalculator.CalcAltitude(positionCurrent.Latitude, positionCurrent.Longitude);
//var altitude = new AltitudeDatum { Altitude = 40 };
double altitudeDiff = 0;
if (altitude != null && _altitudeBefore != null)
{
altitudeDiff = altitude.Altitude - _altitudeBefore.Altitude;
}
_altitudeBefore = altitude;
//Debug.WriteLine("AltitudeDiff: " + altitudeDiff);
double airResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
airResistancePower = AirResistanceCalculator.CalcPower(
Constants.Rho, Car.GetLeaf().CdValue, Car.GetLeaf().FrontalProjectedArea, speed, speed);
}
//Debug.WriteLine("AirResistace: " + airResistancePower);
double rollingResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
rollingResistancePower = RollingResistanceCalculator.CalcPower(
Constants.Myu, Car.GetLeaf().Weight, Math.Atan(altitudeDiff / distanceDiff), speed);
}
//Debug.WriteLine("rollingResistancePower: " + rollingResistancePower);
double climbingResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
climbingResistancePower = ClimbingResistanceCalculator.CalcPowerPreVer(
Car.GetLeaf().Weight, altitudeDiff);
}
//Debug.WriteLine("climbingResistancePower: " + climbingResistancePower);
double accResistancePower = 0;
if (speed > 1.0 / 3.6 && distanceDiff > 0)
{
accResistancePower = AccResistanceCalculator.CalcPower(
_speedBefore,
positionBefore.Timestamp.DateTime,
speed,
positionCurrent.Timestamp.DateTime,
Car.GetLeaf().Weight);
}
//Debug.WriteLine("accResistancePower: " + accResistancePower);
double drivingResistancePower =
airResistancePower + rollingResistancePower + climbingResistancePower + accResistancePower;
double torque = 0;
if (drivingResistancePower > 0 && speed > 0)
{
torque = drivingResistancePower * 1000 * 3600 / speed * Car.GetLeaf().TireRadius /
Car.GetLeaf().ReductionRatio;
}
//Debug.WriteLine("torque: " + torque);
var efficiency = EfficiencyCalculator.CalcEfficiency(Car.GetLeaf(), speed, torque).Efficiency;
//var efficiency = 90;
//Debug.WriteLine("efficiency: " + efficiency);
double convertLoss = ConvertLossCalculator.CalcEnergyPreVer(
drivingResistancePower, speed, efficiency);
//Debug.WriteLine("convertLoss: " + convertLoss);
double regeneEnergy = RegeneEnergyCalculator.CalcEnergy(drivingResistancePower,
speed, Car.GetLeaf(), efficiency);
//Debug.WriteLine("regeneEnergy: " + regeneEnergy);
double regeneLoss = RegeneLossCalculator.CalcEnergyPreVer(drivingResistancePower, speed, efficiency);
//Debug.WriteLine($"{positionCurrent.Timestamp.DateTime}: {regeneLoss}, {efficiency}");
double lostEnergy = LostEnergyCalculator.CalcEnergy(convertLoss, regeneLoss, airResistancePower,
rollingResistancePower);
//Debug.WriteLine("LostEnergy: " + lostEnergy);
_speedBefore = speed;
//var consumedEnergy = ConsumedEnergyCaluculator.CalcEnergy(drivingResistancePower, Car.GetLeaf(), speed, efficiency);
//Debug.WriteLine($"Efficiency: {efficiency}, CalcEfficiency: {(consumedEnergy / convertLoss) * 100}");
LostEnergyList.Add(lostEnergy);
AirResistanceList.Add(airResistancePower);
RollingResistanceList.Add(rollingResistancePower);
ConvertLossList.Add(Math.Abs(convertLoss));
RegeneLossList.Add(Math.Abs(regeneLoss));
}
}
public static ECOLOGTuple calculateEcologTuple(GPSTuple previusGPS, GPSTuple currentGPSTuple, DataTable roadLinks, Car car)
{
// ECOLOGTupleの空のインスタンスを作成
ECOLOGTuple retTuple = new ECOLOGTuple();
// GPSTupleから現在地、GPS時刻、速度を取得。
// 端末時刻は時刻同期がちゃんと取れてる必要があるので、GPS時刻を使うのが無難な気がする。
retTuple.Jst = currentGPSTuple.GpsTime;
retTuple.Latitude = currentGPSTuple.Latitude;
retTuple.Longitude = currentGPSTuple.Longitude;
retTuple.Speed = currentGPSTuple.Speed;
// 10mメッシュ標高データから取得する。
// シングルトンクラスのGetInstance()はインスタンスそのものを返すのでnewいらない。
AltitudeCalculator altitudeCalculator = AltitudeCalculator.GetInstance();
Tuple <int, double> meshIdAltitudeTuple = altitudeCalculator.CalcAltitude(currentGPSTuple.Latitude, currentGPSTuple.Longitude);
retTuple.TerrainAltitude = meshIdAltitudeTuple.Item2;
double previusAltitude = altitudeCalculator.CalcAltitude(previusGPS.Latitude, previusGPS.Longitude).Item2;
double altitudeDiff = retTuple.TerrainAltitude - previusAltitude;
// distance
double distanceDiff = DistanceCalculator.CalcDistance(previusGPS.Latitude,
previusGPS.Longitude,
currentGPSTuple.Latitude,
currentGPSTuple.Longitude);
// calculate resistancePower
double airResistancePower = 0;
double rollingResistancePower = 0;
double climbingResistancePower = 0;
double accResitancePower = 0;
if (currentGPSTuple.Speed > 1 && distanceDiff > 0)
{
airResistancePower = AirResistanceCalculator.CalcPower(Rho,
car.CdValue,
car.FrontalProjectedArea,
(currentGPSTuple.Speed + WindSpeed) / 3.6,
currentGPSTuple.Speed / 3.6);
rollingResistancePower = RollingResistanceCalculator.CalcPower(Myu,
car.Weight,
Math.Atan(altitudeDiff / distanceDiff), // TODO: 前のタプルとの標高差と距離が角度求めるには必要。
currentGPSTuple.Speed / 3.6);
climbingResistancePower = ClimbingResistanceCalculator.CalcPower(car.Weight,
Math.Atan(altitudeDiff / distanceDiff), // TODO: rollingResistancePowerと同様
currentGPSTuple.Speed / 3.6);
accResitancePower = AccResistanceCalculator.CalcPower(previusGPS.Speed / 3.6,
previusGPS.GpsTime,
currentGPSTuple.Speed / 3.6,
currentGPSTuple.GpsTime,
car.Weight);
}
double drivingResistancePower = airResistancePower
+ rollingResistancePower
+ climbingResistancePower
+ accResitancePower;
// torque
double torque = 0;
if (drivingResistancePower > 0 && currentGPSTuple.Speed > 0)
{
torque = drivingResistancePower * 1000 * 3600 / (currentGPSTuple.Speed / 3.6) * car.TireRadius / car.ReductionRatio;
}
// Efficiency
int efficiency = EfficiencyCalculator.GetInstance().GetEfficiency(car, currentGPSTuple.Speed / 3.6, torque);
// 各種損失はSensorLogInserterReの計算メソッドを用いて算出する
double convertLoss = ConvertLossCaluculator.CalcEnergy(drivingResistancePower,
car,
currentGPSTuple.Speed / 3.6,
efficiency);
double regeneEnergy = RegeneEnergyCalculator.CalcEnergy(drivingResistancePower,
currentGPSTuple.Speed / 3.6,
car,
efficiency);
double regeneLoss = RegeneLossCalculator.CalcEnergy(drivingResistancePower,
regeneEnergy,
car,
currentGPSTuple.Speed / 3.6,
efficiency);
double lostEnergy = LostEnergyCalculator.CalcEnergy(convertLoss,
regeneLoss,
airResistancePower,
rollingResistancePower);
// link and semantic_link
// 近傍リンクをSQLで指定、すべてのリンクに対して、GPS点とリンクの距離を求める
// 最近傍リンクまでの距離が閾値を超えない場合には、それをGPS点が通過するリンクとする
return(retTuple);
}
