public void Initialize(DateTime startTime, NavMeshVector startPosition)
{
this.navMesh = WanderingSegments.ConvertAll(x =>
new Segment3d(new Point3d(x.A.X, 0, x.A.Z), new Point3d(x.B.X, 0, x.B.Z)));
StartPositionTime = startTime;
WanderingCenter = startPosition;
StartPosition = startPosition;
GoalPosition = GetGoalPoint(CurrentPoint);
}
public void CalculateNextPoint(DateTime currentTime)
{
DateTime nextPoint = StartPositionTime + TimeSpan.FromMilliseconds(ToNextPoint + Waiting);
while (currentTime > nextPoint)
{
StartPositionTime = nextPoint;
CurrentPoint++;
StartPosition = GoalPosition;
GoalPosition = GetGoalPoint(CurrentPoint);
nextPoint = StartPositionTime + TimeSpan.FromMilliseconds(ToNextPoint + Waiting);
}
}
private NavMeshVector GetGoalPoint(int point)
{
string xRnd = Name + "X" + point;
string zRnd = Name + "Z" + point;
var newGoal = new NavMeshVector()
{
X = DeterministicRandom.Get(xRnd, WanderingCenter.X - Radius, WanderingCenter.X + Radius),
Y = WanderingCenter.Y,
Z = DeterministicRandom.Get(zRnd, WanderingCenter.Z - Radius, WanderingCenter.Z + Radius)
};
Segment3d path = new Segment3d(new Point3d(StartPosition.X, 0, StartPosition.Z), new Point3d(newGoal.X, 0, newGoal.Z));
List <Point3d> possibleHits = new List <Point3d>();
foreach (var s in navMesh)
{
var result = path.IntersectionWith(s);
if (result == null)
{
continue;
}
Point3d pResult = result as Point3d;
Segment3d sResult = result as Segment3d;
if (pResult != null)
{
var hit = pResult + ((path.P1 - pResult).ToVector.Normalized * 0.001f).ToPoint;
possibleHits.Add(hit);
}
else if (sResult != null)
{
var hit1 = sResult.P1 + ((path.P1 - sResult.P1).ToVector.Normalized * 0.001f).ToPoint;
possibleHits.Add(hit1);
var hit2 = sResult.P2 + ((path.P1 - sResult.P2).ToVector.Normalized * 0.001f).ToPoint;
possibleHits.Add(hit2);
}
}
if (possibleHits.Count == 0)
{
return(newGoal);
}
float lowestDistance = float.MaxValue;
Point3d newGoalHit = new Point3d(newGoal.X, WanderingCenter.Y, newGoal.Z);
foreach (var hit in possibleHits)
{
var distance = hit.DistanceTo(path.P1);
if (distance < lowestDistance)
{
newGoalHit = hit;
lowestDistance = (float)distance;
}
}
return(new NavMeshVector()
{
X = (float)newGoalHit.X,
Y = WanderingCenter.Y,
Z = (float)newGoalHit.Z
});
}
private Vector3d ToVector3d(NavMeshVector vec)
{
return(new Vector3d(vec.X, vec.Y, vec.Z));
}