public void Calculate(DistanceData distanceData)
{
if ((distanceData.Calc ?? "").ToLower() == "miles")
{
// convert KM to Miles
var kilometers = distanceData.HasDistanceKm ? distanceData.DistanceKm.Value : 0;
if (kilometers == 0)
{
if (!distanceData.HasDistanceM) distanceData.DistanceM = null;
return;
}
var meters = kilometers * 1000;
var inches = meters / 0.0254;
distanceData.DistanceM = inches / 63360;
}
else
{
// convert Miles to KM
double miles = distanceData.HasDistanceM ? distanceData.DistanceM.Value : 0;
if (miles == 0)
{
if (!distanceData.HasDistanceKm) distanceData.DistanceKm = null;
return;
}
var inches = miles * 63360; // 1 mile = 63360 inches
var meters = inches * 0.0254; // 1 inch = 0.0254 meters
distanceData.DistanceKm = meters / 1000;
}
}
public Distance(double distance, DistanceUnits units)
{
var calculator = new DistanceCalculator();
var distanceData = new DistanceData {
DistanceM = distance, DistanceKm = distance
};
switch (units)
{
case DistanceUnits.Miles:
distanceData.Calc = "kilometers";
break;
case DistanceUnits.Kilometers:
distanceData.Calc = "miles";
break;
default:
throw new ArgumentException("Unknown units: " + units, "units");
}
calculator.Calculate(distanceData);
DistanceInM = distanceData.DistanceM.Value;
DistanceInKm = distanceData.DistanceKm.Value;
BaseDistance = distance;
BaseUnits = units;
}
/// <summary>
/// Calculate the strongest preference paths from the pairwise preferences table.
/// </summary>
/// <param name="pairwiseData">The pairwise data.</param>
/// <param name="choicesCount">The choices count (size of the table).</param>
/// <returns>Returns a table with the strongest paths between each pairwise choice.</returns>
private static DistanceData GetStrongestPaths(DistanceData pairwiseData, int choicesCount)
{
DistanceData data = new DistanceData
{
Paths = new int[choicesCount, choicesCount]
};
int bytesInArray = data.Paths.Length * sizeof(Int32);
Buffer.BlockCopy(pairwiseData.Paths, 0, data.Paths, 0, bytesInArray);
for (int i = 0; i < choicesCount; i++)
{
for (int j = 0; j < choicesCount; j++)
{
if (i != j)
{
for (int k = 0; k < choicesCount; k++)
{
if (i != k && j != k)
{
data.Paths[j, k] = Math.Max(data.Paths[j, k], Math.Min(data.Paths[j, i], data.Paths[i, k]));
}
}
}
}
}
return(data);
}
public void CalculateNearestData()
{
var data = Coordinates.OrderBy(w => w.Distance);
var first = data.First();
var countSame = Coordinates.Count(w => w.Distance == first.Distance);
NearestData = countSame > 1 ? null : first;
}
public ActionResult CalcDistance(DistanceData distanceCalculation)
{
if (!ModelState.IsValid)
{
return(Json(distanceCalculation));
}
distanceCalc.Calculate(distanceCalculation);
return(Json(distanceCalculation));
}
private void Load()
{
dataHandler = DistanceData.ReadFromFile(levelPath);
if (dataHandler == null)
{
dataHandler = new DistanceData();
}
else
{
dataHandler.ToData(data);
}
}
protected override void CreateData()
{
DistanceData[] DistanceData = new DistanceData[Count];
for (int i = 0; i < DistanceData.Length; i++)
{
DistanceData[i] = new DistanceData();
DistanceData[i].Index = i;
DistanceData[i].Distance = 0f;
}
MyGpuBuffer.SetData(DistanceData);
}
static DistanceCalculator()
{
var calculator = new DistanceCalculator();
var distanceData = new DistanceData { DistanceM = 1, Calc = "kilometers" };
calculator.Calculate(distanceData);
MilesToKilometers = distanceData.DistanceKm.Value;
distanceData.DistanceKm = 1;
distanceData.Calc = "miles";
calculator.Calculate(distanceData);
KilometersToMiles = distanceData.DistanceM.Value;
}
public static Transform GetClosestColliderTransform(Collider[] colliders, Vector3 position)
{
List <DistanceData> dataList = new List <DistanceData>();
foreach (var c in colliders)
{
DistanceData data = new DistanceData();
data.trans = c.transform;
data.distance = Vector3.Distance(position, c.transform.position);
dataList.Add(data);
}
return(dataList.OrderBy(x => x.distance).FirstOrDefault().trans);
}
private async Task <Tuple <DistanceData, LocalisationData> > GetCurrentPosition()
{
if (this.ProviderState < ProviderState.StartedRecord)
{
return(Tuple.Create((DistanceData)null, (LocalisationData)null));
}
else
{
DistanceData distance = await AgentBroker.Instance.TryExecuteOnFirst <IDistanceAgent, DistanceData>(a => a.CurrentData).GetValueOrDefault().ConfigureAwait(false);
LocalisationData localisation = await AgentBroker.Instance.TryExecuteOnFirst <ILocalisationAgent, LocalisationData>(a => a.CurrentData).GetValueOrDefault().ConfigureAwait(false);
return(Tuple.Create(distance, localisation));
}
}
static DistanceCalculator()
{
var calculator = new DistanceCalculator();
var distanceData = new DistanceData {
DistanceM = 1, Calc = "kilometers"
};
calculator.Calculate(distanceData);
MilesToKilometers = distanceData.DistanceKm.Value;
distanceData.DistanceKm = 1;
distanceData.Calc = "miles";
calculator.Calculate(distanceData);
KilometersToMiles = distanceData.DistanceM.Value;
}
/// <summary>
/// Fills the pairwise preferences.
/// This goes through each voter's ranking options and updates an array indicating
/// which options are preferred over which other options.
/// Each higher-ranked option gains the difference in ranking in 'beating' a lower-ranked option.
/// </summary>
/// <param name="voterRankings">The voter rankings.</param>
/// <param name="listOfChoices">The list of choices.</param>
/// <returns>Returns a filled-in preferences array.</returns>
private static DistanceData GetPairwiseData(IEnumerable <VoterRankings> voterRankings, List <string> listOfChoices)
{
DistanceData data = new DistanceData
{
Paths = new int[listOfChoices.Count, listOfChoices.Count]
};
var choiceIndexes = GroupRankVotes.GetChoicesIndexes(listOfChoices);
foreach (var voter in voterRankings)
{
var rankedChoices = voter.RankedVotes.Select(v => v.Vote);
var unrankedChoices = listOfChoices.Except(rankedChoices);
foreach (var choice in voter.RankedVotes)
{
foreach (var otherChoice in voter.RankedVotes)
{
// Each ranked vote that has a higher rank (lower number) than each
// alternative has the distance between the choices added to the
// corresponding table entry.
if (choice.Vote != otherChoice.Vote && choice.Rank < otherChoice.Rank)
{
data.Paths[choiceIndexes[choice.Vote], choiceIndexes[otherChoice.Vote]] += otherChoice.Rank - choice.Rank;
}
}
// All unranked options are considered to be at distance 0 from *all* ranked options.
// There is no relative preference, nor does it place unranked options 'beneath'
// ranked options, such that higher ranked options have greater distance from them.
// Unranked options are agnostic choices.
//foreach (var nonChoice in unrankedChoices)
//{
// //data.Paths[choiceIndexes[choice.Vote], choiceIndexes[nonChoice]]++;
//}
}
// All unranked options are at distance 0 from each other, and thus have no effect
// on the distance table.
}
return(data);
}
public static void WriteToFile(string path, DistanceData data)
{
try
{
using (BinaryWriter writer = new BinaryWriter(File.Open(path, FileMode.Create)))
{
writer.Write(data.colNum);
writer.Write(data.rowNum);
foreach (var v in data.distances)
{
writer.Write(v);
}
}
}
catch (Exception e)
{
Debug.LogError(e);
}
}
public static DistanceData DataExtraction(Branch branch, float progress = 0)
{
DistanceData distanceData = new DistanceData();
distanceData.Position = Vector3.zero;
distanceData.Rotation = Quaternion.identity;
distanceData.Index = 0;
for (int i = branch.VertexDistance.Count - 2; i >= 0; i--)
{
if (progress >= branch.VertexDistance[i])
{
distanceData.Index = i;
break;
}
}
var a = distanceData.Index;
var b = distanceData.Index + 1;
var vertexA = branch.Vertices[a];
var vertexB = branch.Vertices[b];
var tangentA = branch.Tangents[a];
var tangentB = branch.Tangents[b];
var normalA = branch.Normals[a];
var normalB = branch.Normals[b];
var vertexDistanceA = branch.VertexDistance[a];
var vertexDistanceB = branch.VertexDistance[b];
var EdgeProgress = Mathf.InverseLerp(vertexDistanceA, vertexDistanceB, progress);
distanceData.Position = Vector3.Lerp(vertexA, vertexB, EdgeProgress);
Quaternion FirstNodeRot = Quaternion.LookRotation(tangentA, normalA);
Quaternion SecondNodeRot = Quaternion.LookRotation(tangentB, normalB);
distanceData.Rotation = Quaternion.Lerp(FirstNodeRot, SecondNodeRot, EdgeProgress);
return(distanceData);
}
/// <summary>
/// Gets the winning paths - The strongest of the strongest paths, for each pair option.
/// </summary>
/// <param name="strengthData">The strongest paths.</param>
/// <param name="choicesCount">The choices count (size of table).</param>
/// <returns>Returns a table with the winning choices of the strongest paths.</returns>
private static DistanceData GetWinningPaths(DistanceData strengthData, int choicesCount)
{
DistanceData winningData = new DistanceData
{
Paths = new int[choicesCount, choicesCount]
};
for (int i = 0; i < choicesCount; i++)
{
for (int j = 0; j < choicesCount; j++)
{
if (i != j)
{
winningData.Paths[i, j] = strengthData.Paths[i, j] - strengthData.Paths[j, i];
}
}
}
return(winningData);
}
public void Calculate(DistanceData distanceData)
{
if ((distanceData.Calc ?? "").ToLower() == "miles")
{
// convert KM to Miles
var kilometers = distanceData.HasDistanceKm ? distanceData.DistanceKm.Value : 0;
if (kilometers == 0)
{
if (!distanceData.HasDistanceM)
{
distanceData.DistanceM = null;
}
return;
}
var meters = kilometers * 1000;
var inches = meters / 0.0254;
distanceData.DistanceM = inches / 63360;
}
else
{
// convert Miles to KM
double miles = distanceData.HasDistanceM ? distanceData.DistanceM.Value : 0;
if (miles == 0)
{
if (!distanceData.HasDistanceKm)
{
distanceData.DistanceKm = null;
}
return;
}
var inches = miles * 63360; // 1 mile = 63360 inches
var meters = inches * 0.0254; // 1 inch = 0.0254 meters
distanceData.DistanceKm = meters / 1000;
}
}
/// <summary>
/// Implementation to generate the ranking list for the provided set
/// of votes for a specific task, based on the Schulze algorithm.
/// </summary>
/// <param name="task">The task that the votes are grouped under.</param>
/// <returns>Returns a ranking list of winning votes.</returns>
protected override RankResults RankTask(GroupedVotesByTask task)
{
if (task == null)
{
throw new ArgumentNullException(nameof(task));
}
Debug.WriteLine(">>Distance U0 Scoring<<");
List <string> listOfChoices = GroupRankVotes.GetAllChoices(task);
var voterRankings = GroupRankVotes.GroupByVoterAndRank(task);
DistanceData pairwiseData = GetPairwiseData(voterRankings, listOfChoices);
DistanceData strengthData = GetStrongestPaths(pairwiseData, listOfChoices.Count);
DistanceData winningPaths = GetWinningPaths(strengthData, listOfChoices.Count);
RankResults winningChoices = GetResultsInOrder(winningPaths, listOfChoices);
return(winningChoices);
}
/// <summary>
/// Gets the winning options in order of preference, based on the winning paths.
/// </summary>
/// <param name="winningPaths">The winning paths.</param>
/// <param name="listOfChoices">The list of choices.</param>
/// <returns>Returns a list of </returns>
private RankResults GetResultsInOrder(DistanceData winningPaths, List <string> listOfChoices)
{
int count = listOfChoices.Count;
var availableIndexes = Enumerable.Range(0, count);
var pathCounts = from index in availableIndexes
select new
{
Index = index,
Choice = listOfChoices[index],
Count = GetPositivePathCount(winningPaths.Paths, index, count),
Sum = GetPathSum(winningPaths.Paths, index, count),
};
var orderPaths = pathCounts.OrderByDescending(p => p.Sum).ThenByDescending(p => p.Count).ThenBy(p => p.Choice);
RankResults results = new RankResults();
results.AddRange(orderPaths.Select(path =>
new RankResult(listOfChoices[path.Index], $"U0: [{path.Count}/{path.Sum}]")));
return(results);
}
public static DistanceData ReadFromFile(string path)
{
try
{
using (BinaryReader reader = new BinaryReader(File.OpenRead(path)))
{
DistanceData data = new DistanceData();
data.colNum = reader.ReadInt32();
data.rowNum = reader.ReadInt32();
int length = data.colNum * data.rowNum;
data.distances = new float[length];
for (int i = 0; i < length; ++i)
{
data.distances[i] = reader.ReadSingle();
}
return(data);
}
}
catch (Exception e)
{
Debug.LogError(e);
}
return(null);
}
protected override Task <IObservable <SpeedData> > InitializeCore()
{
if (this.DistanceDataSource == null)
{
this.DistanceDataSource = new SubjectSlim <DistanceData>();
this.IsDistanceOperationalCallback = () => false;
}
else
{
// we must be able to keep sending data even if distanceDataSource completes
// so we merge it with a subject that never completes
this.DistanceDataSource = this.DistanceDataSource.Merge(new SubjectSlim <DistanceData>());
}
if (this.LocalisationDataSource == null)
{
this.LocalisationDataSource = new SubjectSlim <LocalisationData>();
this.IsLocalisationOperationalCallback = () => false;
}
return(Task.Run(
() =>
{
var speedBuffer = new ConcurrentQueue <double>();
DistanceData lastDistance = null;
SpeedData lastSpeed = null;
double?lastDistanceSpeed = null;
double?lastGpsSpeed = null;
var distanceDataSource = this.DistanceDataSource
// filter invalid data
.Where(
data =>
{
if (lastDistance == null)
{
// no previous data
// so save first data point
// but speed cannot be calculated yet
lastDistance = data;
return false;
}
var deltaDistance = data.AbsoluteDistance - lastDistance.AbsoluteDistance;
var deltaTime = data.Timestamp - lastDistance.Timestamp;
// case when distance was reset on acquisition start
if (deltaDistance < 0)
{
lastDistance = data;
}
return deltaTime.TotalMilliseconds > 0 && deltaDistance >= 0;
})
// odometric speed calculation
.Select(
data =>
{
Debug.Assert(lastDistance != null);
var deltaDistance = data.AbsoluteDistance - lastDistance.AbsoluteDistance;
var deltaTime = data.Timestamp - lastDistance.Timestamp;
speedBuffer.Enqueue(deltaDistance * (3600 / deltaTime.TotalMilliseconds));
double tmpSpeed;
if (speedBuffer.Count > this.DistanceSpeedCountForAverage)
{
speedBuffer.TryDequeue(out tmpSpeed);
}
lastDistance = data;
return new SpeedData {
CurrentSpeed = speedBuffer.Average(), CurrentDistance = lastDistance.AbsoluteDistance, SpeedSource = SpeedActiveMode.Distance
};
})
// return odometric speed immediately or wait until timeout is reached
.Buffer(this.DistanceDataTimeout, 1)
// if timeout has been reached and odometer is operational, assume that speed is equal to 0
.Select(
buffer =>
{
if (buffer.Count > 0)
{
return buffer[0];
}
else if (this.IsDistanceOperationalCallback())
{
speedBuffer.Enqueue(0);
double tmpSpeed;
if (speedBuffer.Count > this.DistanceSpeedCountForAverage)
{
speedBuffer.TryDequeue(out tmpSpeed);
}
return new SpeedData {
CurrentSpeed = speedBuffer.Average(), SpeedSource = SpeedActiveMode.Distance
};
}
else
{
return null;
}
})
// filter null values in case odometer is not operational
.Where(data => data != null)
// save last odometric speed
.Select(
data =>
{
lastDistanceSpeed = data.CurrentSpeed;
return data;
})
.Publish().RefCount();
var gpsDataSource = this.LocalisationDataSource
// filter invalid GPS data and data received while GPS is not operational
.Where(data => data.GpsStatus != GpsStatus.SignalLost && data.CorrectedData.VelocityData.SpeedKmh >= 0 && this.IsLocalisationOperationalCallback())
// save last GPS speed
.Select(
data =>
{
var gpsSpeed = new SpeedData {
CurrentSpeed = data.CorrectedData.VelocityData.SpeedKmh, SpeedSource = SpeedActiveMode.Gps
};
lastGpsSpeed = gpsSpeed.CurrentSpeed;
return gpsSpeed;
})
.Publish().RefCount();
return distanceDataSource.Merge(gpsDataSource)
// return current speed based on data sources state and hysteresis
.Select(
data =>
{
SpeedActiveMode source;
if (data.CurrentSpeed < 0)
{
source = SpeedActiveMode.None;
}
else if (data.CurrentSpeed < (this.Threshold - this.Hysteresis))
{
source = SpeedActiveMode.Distance;
}
else if (data.CurrentSpeed > (this.Threshold + this.Hysteresis))
{
source = SpeedActiveMode.Gps;
}
else
{
source = lastSpeed == null ? SpeedActiveMode.Gps : lastSpeed.SpeedSource;
}
data.DistanceSpeed = data.SpeedSource == SpeedActiveMode.Distance ? data.CurrentSpeed : this.IsDistanceOperationalCallback() ? lastDistanceSpeed : null;
data.GpsSpeed = data.SpeedSource == SpeedActiveMode.Gps ? data.CurrentSpeed : this.IsLocalisationOperationalCallback() ? lastGpsSpeed : null;
Debug.Assert(data.DistanceSpeed != null || data.GpsSpeed != null);
if (source == SpeedActiveMode.None)
{
data.CurrentSpeed = -1;
data.SpeedSource = SpeedActiveMode.None;
}
else if (source == SpeedActiveMode.Distance && data.DistanceSpeed != null)
{
data.CurrentSpeed = data.DistanceSpeed.Value;
data.SpeedSource = SpeedActiveMode.Distance;
}
else if (source == SpeedActiveMode.Gps && data.GpsSpeed != null)
{
data.CurrentSpeed = data.GpsSpeed.Value;
data.SpeedSource = SpeedActiveMode.Gps;
}
data.IsInRange = this.SpeedRanges.Any(ranges => Math.Floor(data.CurrentSpeed).Between(ranges.MinSpeed, ranges.MaxSpeed, inclusive: true));
lastSpeed = data;
return data;
})
.Publish().RefCount();
}));
}
private void Save()
{
dataHandler.FromData(data);
DistanceData.WriteToFile(levelPath, dataHandler);
}
void FindNearFloor()
{
Debug.Log("Find Near Floor Start -----");
DisListClear();
selectFloorStr = moveList[firstP];
if (selectFloorStr != null)
{
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
if (i == myXInMap && j == myYInMap)
{
continue;
}
else
{
if (myMap[i, j].floor.GetComponent <Floor>().MyColor == selectFloorStr)
{
float tarx = myMap[i, j].floor.transform.position.x;
float tary = myMap[i, j].floor.transform.position.y;
float mx = this.transform.position.x;
float my = this.transform.position.y;
float f = Vector2.Distance(new Vector2(tarx, tary), new Vector2(mx, my));
DisListAdd(f, i, j);
}
}
}
}
if (disListP == -1)
{
return;
}
int len1 = GetDisListRealLength();
tempList = new DistanceData[len1];
for (int i = 0; i < len1; i++)
{
tempList[i] = disList[i];
}
Sort(tempList);
int p = 0;
int count = 1;
while (true)
{
if (tempList.Length == 0)
{
break;
}
if (p + 1 >= tempList.Length)
{
break;
}
if ((tempList[p + 1].dis - tempList[p].dis) < 0.01f)
{
count++;
}
else
{
break;
}
p++;
}
if (count == 1)
{
targetX = myMap[tempList[0].x, tempList[0].y].floor.transform.position.x;
targetY = myMap[tempList[0].x, tempList[0].y].floor.transform.position.y;
moveEnable = true;
SetMyPosInMap(tempList[0].x, tempList[0].y);
Debug.Log("Find it ----");
return;
}
DistanceData[] nearList = new DistanceData[count];
for (int i = 0; i < count; i++)
{
nearList[i] = tempList[i];
}
DisListClear();
//DistanceData[] disToZero = new DistanceData[16];
for (int i = 0; i < nearList.Length; i++)
{
if (nearList[i].dis != 0)
{
int nx = nearList[i].x;
int ny = nearList[i].y;
Vector2 v1 = new Vector2(myMap[nx, ny].floor.transform.position.x, myMap[nx, ny].floor.transform.position.y);
Vector2 v2 = new Vector2(myMap[0, 0].floor.transform.position.x, myMap[0, 0].floor.transform.position.y);
float f = Vector2.Distance(v1, v2);
DisListAdd(f, nearList[i].x, nearList[i].y);
}
}
int len2 = GetDisListRealLength();
tempList = new DistanceData[len2];
for (int i = 0; i < len2; i++)
{
tempList[i] = disList[i];
}
Sort(tempList);
targetX = myMap[tempList[0].x, tempList[0].y].floor.transform.position.x;
targetY = myMap[tempList[0].x, tempList[0].y].floor.transform.position.y;
moveEnable = true;
SetMyPosInMap(tempList[0].x, tempList[0].y);
Debug.Log("Find it ----");
}
Debug.Log("Not Find -----");
}
