/// <summary>
/// Method used to smooth a received path, using a string pulling technique
/// it returns a new path, where the path positions are selected in order to provide a smoother path
/// </summary>
/// <param name="data"></param>
/// <param name="globalPath"></param>
/// <returns></returns>
public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
{
var smoothedPath = new GlobalPath
{
IsPartial = globalPath.IsPartial
};
var endPoint = globalPath.PathPositions[globalPath.PathPositions.Count - 1];
var nextPoint = globalPath.PathPositions[globalPath.PathPositions.Count - 1];
for(int i = globalPath.PathNodes.Count - 1; i >= 0; i--) {
var node = globalPath.PathNodes[i];
var edge = node as RAIN.Navigation.NavMesh.NavMeshEdge;
if (edge != null)
{
var pointInEdge = Utils.MathHelper.ClosestPointInLineSegment2ToLineSegment1(data.position, endPoint, edge.PointOne, edge.PointTwo, nextPoint);
smoothedPath.PathNodes.Add(node);
smoothedPath.PathPositions.Add(pointInEdge);
nextPoint = pointInEdge;
}
}
smoothedPath.PathNodes.Reverse();
smoothedPath.PathPositions.Reverse();
return smoothedPath;
}
public bool WillViolate(GlobalPath path, out Vector3 suggestedPosition)
{
foreach (LocalPath local in path.LocalPaths)
{
if (local.StartPosition == local.EndPosition) continue;
var direction = local.EndPosition - local.StartPosition;
RaycastHit collision;
if (!Physics.Raycast(local.StartPosition, direction, out collision, direction.magnitude)) continue; // can't be optimized to sqrMagnitude with no side effects
suggestedPosition = this.Character.KinematicData.position +
(collision.normal +
(local.EndPosition - this.Character.KinematicData.position)).normalized *
this.AvoidMargin;
Debug.DrawRay(collision.point, collision.normal, Color.cyan);
Debug.DrawLine(this.Character.KinematicData.position, suggestedPosition, Color.magenta);
return true;
}
suggestedPosition = Vector3.zero;
return false;
}
public override bool Search(out GlobalPath solution, bool returnPartialSolution = false)
{
NodeRecord node;
var nodeCount = 0;
while (this.Open.CountOpen() != 0)
{
node = this.Open.GetBestAndRemove();
nodeCount++;
this.TotalProcessedNodes++;
var count = this.Open.CountOpen();
if (count > this.MaxOpenNodes) this.MaxOpenNodes = count;
if (nodeCount < this.NodesPerSearch)
{
if (GoalNode.Equals(node.node)) { solution = this.CalculateSolution(node, false); InProgress = false; this.TotalProcessingTime = UnityEngine.Time.deltaTime; return true; }
this.Closed.AddToClosed(node);
var outConnections = node.node.OutEdgeCount;
for (int i = 0; i < outConnections; i++)
{
this.ProcessChildNode(node, node.node.EdgeOut(i));
}
}
else if (returnPartialSolution) { solution = this.CalculateSolution(node, true); InProgress = true; this.TotalProcessingTime = UnityEngine.Time.deltaTime; return true; }
}
InProgress = false;
solution = null;
return true;
}
//private NavMeshPathGraph navMesh;
public DecomposerPersonPathCreation()
{
//this.navMesh = NavigationManager.Instance.NavMeshGraphs[0];
personPath = new GlobalPath();
LocalPath localPath = new LineSegmentPath();
personPath.LocalPaths.Add(localPath);
}
public Goal Suggest(KinematicData character, GlobalPath path, Goal goal)
{
Vector3 characterPoint = NavigationManager.Instance.NavMeshGraphs [0].ClosestPointOnGraph(character.position, aMaxYOffset);
NavigationGraphNode characterNodeInGraph = NavigationManager.Instance.NavMeshGraphs [0].QuantizeToNode(characterPoint, 1.0f);
Vector3 suggestedPoint = NavigationManager.Instance.NavMeshGraphs [0].ClosestPointOnGraph(suggestedPosition, aMaxYOffset);
NavigationGraphNode nodeInGraph = NavigationManager.Instance.NavMeshGraphs [0].QuantizeToNode(suggestedPoint, 1.0f);
Debug.DrawRay(characterPoint, (suggestedPoint - characterPoint), Color.cyan);
Goal suggestedGoal = new Goal()
{
HasPosition = true,
position = suggestedPoint,
IsNew = true
};
GlobalPath suggestedPath = new GlobalPath();
suggestedPath.PathPositions.Add(characterPoint);
suggestedPath.PathPositions.Add(suggestedPoint);
suggestedPath.PathNodes.Add(characterNodeInGraph);
suggestedPath.PathNodes.Add(nodeInGraph);
suggestedPath.CalculateLocalPathsFromPathPositions(characterPoint);
suggestedGoal.Path = suggestedPath;
forgive = true;
return suggestedGoal;
}
public SteeringAlgorithm(Targeter tag)
{
this.targeter = tag;
maxConstraintSteps = 10.0f; //alterar dps consoante
validPath = false;
goal = new Goal();
smoothpath = new GlobalPath();
this.goal.updateChannels(targeter.getGoal());
}
/// <summary>
/// Method used to smooth a received path, using a string pulling technique
/// it returns a new path, where the path positions are selected in order to provide a smoother path
/// </summary>
/// <param name="data"></param>
/// <param name="globalPath"></param>
/// <returns></returns>
public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
{
var smoothedPath = new GlobalPath
{
IsPartial = globalPath.IsPartial
};
Vector3 initialPosition = data.position;
int closestIndex = globalPath.PathNodes.Count - 1;
if (globalPath.PathNodes.Count > 0)
{
float minMagnitude = float.MaxValue;
for (int i = globalPath.PathNodes.Count - 1; i >= previousIndex; i--)
{
if ((globalPath.PathNodes[i].Position - data.position).sqrMagnitude < minMagnitude
&& i >= previousIndex)
{
minMagnitude = (globalPath.PathNodes[i].Position - data.position).sqrMagnitude;
closestIndex = i;
}
}
previousIndex = closestIndex;
NavMeshEdge edge;
Vector3 previousPoint = globalPath.PathPositions[globalPath.PathPositions.Count - 1];
for (int i = globalPath.PathNodes.Count - 1; i > previousIndex; i--)
{
edge = globalPath.PathNodes[i] as NavMeshEdge;
if (edge == null)
{
globalPath.PathNodes.RemoveAt(i);
globalPath.PathPositions.RemoveAt(i);
continue;
}
Vector3 newPoint = MathHelper.ClosestPointInLineSegment2ToLineSegment1(initialPosition,
previousPoint, edge.PointTwo, edge.PointOne, initialPosition);
var distance = (newPoint - previousPoint).sqrMagnitude;
if (distance < 1.44f) // 1.44 = 1.2^2
{
globalPath.PathNodes.RemoveAt(i);
globalPath.PathPositions.RemoveAt(i);
continue;
}
previousPoint = newPoint;
smoothedPath.PathPositions.Add(newPoint);
}
smoothedPath.PathPositions.Reverse();
}
return smoothedPath;
}
public bool WillViolate(GlobalPath path, out Vector3 suggestedPosition)
{
var targetsToAvoid = false;
var shortestTime = float.MaxValue;
var closestMinSeparation = 0f;
var closestDistance = 0f;
var closestDeltaPos = Vector3.zero;
var closestDeltaVel = Vector3.zero;
DynamicCharacter closestTarget = null;
foreach (var target in this.Pedestrians)
{
var deltaPos = target.KinematicData.position - this.Character.KinematicData.position;
var deltaVel = target.KinematicData.velocity - this.Character.KinematicData.velocity;
var deltaSpeed = deltaVel.magnitude; // can't be optimized to sqrMagnitude with no side effects
if (deltaSpeed < 0.005f)
continue;
var timeToClosest = -Vector3.Dot(deltaPos, deltaVel) / (deltaSpeed * deltaSpeed);
if (timeToClosest > MaxTimeLookAhead)
continue;
var distance = deltaPos.magnitude; // can't be optimized to sqrMagnitude with no side effects
var minSeparation = distance - deltaSpeed * timeToClosest;
if (minSeparation > 2 * CollisionRadius)
continue;
if (timeToClosest > 0 && timeToClosest < shortestTime)
{
targetsToAvoid = true;
shortestTime = timeToClosest;
closestTarget = target;
closestMinSeparation = minSeparation;
closestDistance = distance;
closestDeltaPos = deltaPos;
closestDeltaVel = deltaVel;
}
}
if (!targetsToAvoid)
{
suggestedPosition = Vector3.zero;
return false;
}
Vector3 avoidanceDirection;
if (closestMinSeparation <= 0 || closestDistance < 2 * CollisionRadius)
avoidanceDirection = this.Character.KinematicData.position - closestTarget.KinematicData.position;
else
avoidanceDirection = (closestDeltaPos + closestDeltaVel * shortestTime) * -1;
suggestedPosition = this.Character.KinematicData.position + avoidanceDirection.normalized * AvoidMargin;
return true;
}
public DynamicFollowPath(KinematicData character, GlobalPath path)
{
this.Target = new KinematicData();
this.Character = character;
this.globalPath = path;
this.CurrentParam = 0.0f;
this.PathOffset = 0.2f;
this.PredictTime = 0.05f;
}
public MovementOutput GetMovement(DynamicCharacter character, GlobalPath currentSmoothedSolution)
{
DynamicMovement output;
output = new DynamicFollowPath(character.KinematicData, currentSmoothedSolution)
{
MaxAcceleration = 20.0f,
PathOffset = 0.15f
};
return output.GetMovement();
}
/// <summary>
/// Smoothes the current path
/// </summary>
public GlobalPath SmoothPath(GlobalPath currentSolution)
{
GlobalPath currentSmoothedSolution = null;
currentSmoothedSolution = StringPullingPathSmoothing.SmoothPath(Character.KinematicData,
currentSolution);
currentSmoothedSolution.PathPositions.Add(Position);
currentSmoothedSolution.CalculateLocalPathsFromPathPositions(
Character.KinematicData.position);
// currentSmoothedSolution.LastParam = lastParam;
return currentSmoothedSolution;
}
public void Search(KinematicData characterData)
{
if (this.searchAlgorithm.InProgress)
{
var finished = this.searchAlgorithm.Search(out this.currentSolution, true);
if (finished && this.currentSolution != null)
{
//lets smooth out the Path
this.currentSmoothedSolution = StringPullingPathSmoothing.SmoothPath(characterData, this.currentSolution);
this.currentSmoothedSolution.CalculateLocalPathsFromPathPositions(characterData.position);
}
}
}
public MovementOutput GetMovement(KinematicData character, GlobalPath path, Goal goal)
{
if (goal.HasPosition && path != null)
{
movement.Character = character;
movement.Path = path;
movement.CurrentParam = goal.CurrentParam;
MovementOutput movementOut = movement.GetMovement();
goal.CurrentParam = movement.CurrentParam;
return movementOut;
}
return movement.EmptyMovementOutput;
}
/// <summary>
/// Calculates the path if there is a new goal
/// </summary>
public void CalculatePath(out GlobalPath currentSolution)
{
currentSolution = null;
//call the pathfinding method if the user specified a new goal
if (aStarPathFinding.InProgress)
{
bool finished = aStarPathFinding.Search(out currentSolution, true);
if (finished && currentSolution != null)
{
Pipeline.CalculatePath = false;
}
}
}
/// <summary>
/// Method used to smooth a received path, using a string pulling technique
/// it returns a new path, where the path positions are selected in order to provide a smoother path
/// </summary>
/// <param name="data"></param>
/// <param name="globalPath"></param>
/// <returns></returns>
public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
{
var smoothedPath = new GlobalPath
{
IsPartial = globalPath.IsPartial
};
Vector3 currentPos = data.position;
Vector3 Pnext = globalPath.PathPositions[globalPath.PathPositions.Count - 1];
smoothedPath.PathPositions.Add(Pnext);
int index = globalPath.PathPositions.Count - 2;
if (index <= 0)
{
return globalPath;
}
bool finish = false;
while (true)
{
NavigationGraphNode node = globalPath.PathNodes[index];
LocalPath L = new LineSegmentPath(currentPos, Pnext);
Vector3 closestPoint = GetShortestDistancePositionInEdge(node, L);
if ((closestPoint - smoothedPath.PathPositions[smoothedPath.PathPositions.Count - 1]).sqrMagnitude >= (MinWidth * MinWidth))
{
smoothedPath.PathPositions.Add(closestPoint);
Pnext = closestPoint;
}
index--;
if (index <= 0 || finish)
{
//smoothedPath.PathPositions.Reverse();
//return smoothedPath;
break;
}
if ((closestPoint - currentPos).sqrMagnitude < MinDistanceToCharacter * MinDistanceToCharacter)
{
finish = true;
}
}
smoothedPath.PathPositions.Reverse();
return smoothedPath;
}
public GlobalPath GetPath(GlobalPath smoothedPath)
{
GlobalPath realPath = new GlobalPath();
int i;
for (i = 0; i < smoothedPath.LocalPaths.Count; ++i)
{
if ((smoothedPath.LocalPaths[i].EndPosition - this.Character.KinematicData.position).magnitude >
// can't be optimized to sqrMagnitude with no side effects
LookAheadDistance)
{
break;
}
}
if (i == 0)
{
Debug.Log("PATH VAZIO");
realPath.LocalPaths.Add(new LineSegmentPath(this.Character.KinematicData.position,
smoothedPath.LocalPaths[0].EndPosition));
return realPath;
}
Vector3 newGoal = smoothedPath.LocalPaths[i - 1].EndPosition;
newGoal.y = 0;
Vector3 newDirection = newGoal - this.Character.KinematicData.position;
// tests if the new position is behind an obstacle
Debug.DrawLine(this.Character.KinematicData.position, newGoal, Color.magenta);
if (!Physics.Raycast(this.Character.KinematicData.position, newDirection, newDirection.magnitude))
// can't be optimized to sqrMagnitude with no side effects
{
realPath.LocalPaths.Add(new LineSegmentPath(this.Character.KinematicData.position, newGoal));
}
else
{
//Debug.Log("Colisao");
realPath.LocalPaths.Add(new LineSegmentPath(this.Character.KinematicData.position,
smoothedPath.LocalPaths[0].EndPosition));
this.Character.KinematicData.velocity *= 0.995f;
}
return realPath;
}
public virtual bool Search(out GlobalPath solution, bool returnPartialSolution = false)
{
solution = null;
if (InitialTime == -1.0f)
InitialTime = Time.realtimeSinceStartup;
float finalTime = 0f;
for (int i = 0; Open.CountOpen() > 0; ++i)
{
NodeRecord record = Open.GetBestAndRemove();
if (i >= NodesPerSearch)
{
if (returnPartialSolution)
solution = CalculateSolution(record, returnPartialSolution);
return false;
}
// update debug information
if (Open.CountOpen() > MaxOpenNodes)
MaxOpenNodes = Open.CountOpen();
++TotalProcessedNodes;
if (record.node == GoalNode)
{
solution = CalculateSolution(record, returnPartialSolution);
CleanUp();
finalTime = Time.realtimeSinceStartup;
TotalProcessingTime = finalTime - InitialTime;
return true;
}
Closed.AddToClosed(record);
for (int e = 0; e < record.node.OutEdgeCount; ++e)
ProcessChildNode(record, record.node.EdgeOut(e));
}
CleanUp();
finalTime = Time.realtimeSinceStartup;
TotalProcessingTime = finalTime - InitialTime;
return true;
}
/// <summary>
/// Method used to smooth a received path, using a string pulling technique
/// it returns a new path, where the path positions are selected in order to provide a smoother path
/// </summary>
/// <param name="data"></param>
/// <param name="globalPath"></param>
/// <returns></returns>
public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
{
NavMeshEdge edge;
var lookAhead = 3;
Vector3 lookAheadTarget;
var smoothedPath = new GlobalPath
{
IsPartial = globalPath.IsPartial
};
//we will string pull from the begginning to the end
var endPosition = globalPath.PathPositions.Last();
var currentPosition = data.position;
for (int i = 0; i < globalPath.PathNodes.Count; i++)
{
edge = globalPath.PathNodes[i] as NavMeshEdge;
if (edge != null)
{
if ((i + lookAhead) < globalPath.PathNodes.Count)
{
lookAheadTarget = globalPath.PathNodes[i + lookAhead].LocalPosition;
}
else
{
lookAheadTarget = endPosition;
}
if (!lookAheadTarget.Equals(currentPosition))
{
var closestPointInEdge = MathHelper.ClosestPointInLineSegment2ToLineSegment1(currentPosition, lookAheadTarget, edge.PointOne, edge.PointTwo, lookAheadTarget);
smoothedPath.PathPositions.Add(closestPointInEdge);
currentPosition = closestPointInEdge;
}
}
}
smoothedPath.PathPositions.Add(endPosition);
return smoothedPath;
}
public GlobalPath GetPath(Movement.KinematicData character, Goal goal)
{
if (goal.HasPosition && goal.Path != null)
{
float actualTime = Time.realtimeSinceStartup;
if (actualTime - previousSmooth > SecondsToSmooth ||
currentSmoothedSolution == null ||
goal.IsNew ||
goal != previousGoal)
{
previousSmooth = actualTime;
currentSmoothedSolution = StringPullingPathSmoothing.SmoothPath(character, goal.Path);
currentSmoothedSolution.CalculateLocalPathsFromPathPositions(character.position);
}
previousGoal = goal;
return currentSmoothedSolution;
}
return null;
}
/// <summary>
/// Method used to smooth a received path, using a string pulling technique
/// it returns a new path, where the path positions are selected in order to provide a smoother path
/// </summary>
/// <param name="data"></param>
/// <param name="globalPath"></param>
/// <returns></returns>
public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
{
var smoothedPath = new GlobalPath
{
IsPartial = globalPath.IsPartial
};
Vector3 currentPos = globalPath.PathPositions[0];
Vector3 Pnext = globalPath.PathPositions[globalPath.PathPositions.Count - 1];
smoothedPath.PathPositions.Add(Pnext);
int index = globalPath.PathPositions.Count - 2;
if (index <= 0)
{
return globalPath;
}
while (true)
{
NavigationGraphNode node = globalPath.PathNodes[index];
LocalPath L = new LineSegmentPath(currentPos, Pnext);
Vector3 closestPoint = GetShortestDistancePositionInEdge(node, L);
if ((closestPoint - smoothedPath.PathPositions[smoothedPath.PathPositions.Count - 1]).sqrMagnitude >= (MinWidth * MinWidth))
{
smoothedPath.PathPositions.Add(closestPoint);
}
index--;
if (index <= 0)
{
smoothedPath.PathPositions.Reverse();
return smoothedPath;
}
else
{
Pnext = closestPoint;
}
}
}
public Goal Decompose(KinematicData character, Goal goal)
{
if (goal != null && goal.HasPosition)
{
if (goal.IsNew)
{
this.aStarPathFinding.InitializePathfindingSearch(character.position, goal.position);
this.currentSolution = null;
}
if (this.aStarPathFinding.InProgress)
{
this.aStarPathFinding.Search(out this.currentSolution, true);
if (currentSolution != null)
{
goal.Path = currentSolution;
goal.Path.CalculateLocalPathsFromPathPositions(character.position);
}
}
}
return goal;
}
protected MovementOutput GetOutput(GlobalPath suggestedPath, DynamicCharacter character)
{
DynamicMovement movement;
if (suggestedPath != null)
{
SeekMovement.Target = new KinematicData()
{
position = suggestedPath.LocalPaths[0].EndPosition
};
//Debug.DrawLine(character.KinematicData.position, SeekMovement.Target.position, Color.black);
movement = SeekMovement;
return movement.GetMovement();
}
else
{
movement = FollowPathMovement;
}
return FilterMovementOutput(movement.GetMovement(), Character);
}
/// <summary>
/// Method used to smooth a received path, using a string pulling technique
/// it returns a new path, where the path positions are selected in order to provide a smoother path
/// </summary>
/// <param name="data"></param>
/// <param name="globalPath"></param>
/// <returns></returns>
public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
{
if (globalPath.PathNodes.Count <= 2)
return globalPath;
var smoothedPath = new GlobalPath
{
IsPartial = globalPath.IsPartial
};
globalPath.PathNodes.Reverse();
globalPath.PathPositions.Reverse();
NavMeshEdge goalNode = globalPath.PathNodes[0] as NavMeshEdge;
Vector3 pnext = globalPath.PathPositions[0];
smoothedPath.PathNodes.Add(goalNode);
smoothedPath.PathPositions.Add(pnext);
globalPath.PathNodes.RemoveAt(0);
foreach (var node in globalPath.PathNodes)
{
NavMeshEdge edge = node as NavMeshEdge;
if (edge != null)
{
Vector3 point = MathHelper.ClosestPointInLineSegment2ToLineSegment1(data.position, pnext, edge.PointOne, edge.PointTwo, edge.PointOne);
pnext = point;
smoothedPath.PathNodes.Add(node);
smoothedPath.PathPositions.Add(point);
}
}
globalPath.PathNodes.Reverse();
globalPath.PathPositions.Reverse();
smoothedPath.PathNodes.Reverse();
smoothedPath.PathPositions.Reverse();
return smoothedPath;
}
public bool WillViolate(GlobalPath path, out Vector3 suggestedPosition)
{
suggestedPosition = Vector3.zero;
if (Character.KinematicData.velocity.sqrMagnitude < 0.01f)
return false;
var centralOrientation = Character.KinematicData.velocity;
var leftOrientation = MathHelper.ConvertOrientationToVector(MathHelper.ConvertVectorToOrientation(centralOrientation) - MathConstants.MATH_PI_2 / 4.0F);
var rightOrientation = MathHelper.ConvertOrientationToVector(MathHelper.ConvertVectorToOrientation(centralOrientation) + MathConstants.MATH_PI_2 / 4.0F);
Vector3 localGoal = path.LocalPaths[0].EndPosition;
if (checkCollision(localGoal, centralOrientation, 12f, out suggestedPosition)
|| checkCollision(localGoal, leftOrientation, 10f, out suggestedPosition)
|| checkCollision(localGoal, rightOrientation, 10f, out suggestedPosition))
{
//Debug.Log("there was a static object collision");
return true;
}
return false;
}
// Update is called once per frame
void Update()
{
// NavigationGraphNode node;
if (Input.GetKeyDown("z"))
{
this.aStarPathFinding = aStarPathFindingZero;
currentAStar = "A* Zero Heuristic";
}
else if (Input.GetKeyDown("x"))
{
this.aStarPathFinding = aStarPathFindingEuclidean;
currentAStar = "A* Euclidean Distance";
}
else if (Input.GetKeyDown("c"))
{
this.aStarPathFinding = aStarPathFindingGBZero;
currentAStar = "A* GoalBounding Zero Heuristic";
}
if (Input.GetKeyDown("v"))
{
this.aStarPathFinding = aStarPathFindingGBEuclidean;
currentAStar = "A* GoalBounding Euclidean Distance";
}
if (Input.GetMouseButtonDown(0))
{
//if there is a valid position
if (this.MouseClickPosition(out position))
{
//we're setting the end point
//this is just a small adjustment to better see the debug sphere
this.endDebugSphere.transform.position = position + Vector3.up;
this.endDebugSphere.SetActive(true);
//this.currentClickNumber = 1;
this.endPosition = position;
this.draw = true;
//initialize the search algorithm
this.aStarPathFinding.InitializePathfindingSearch(this.character.KinematicData.position, this.endPosition);
}
}
if (Input.GetKeyDown("space"))
{
//we're setting the end point
//this is just a small adjustment to better see the debug sphere
this.endDebugSphere.transform.position = position + Vector3.up;
this.endDebugSphere.SetActive(true);
//this.currentClickNumber = 1;
this.endPosition = position;
this.draw = true;
//initialize the search algorithm
this.aStarPathFinding.InitializePathfindingSearch(this.character.KinematicData.position, this.endPosition);
}
//call the pathfinding method if the user specified a new goal
if (this.aStarPathFinding.InProgress)
{
var finished = this.aStarPathFinding.Search(out this.currentSolution);
if (finished && this.currentSolution != null)
{
//lets smooth out the Path
this.startPosition = this.character.KinematicData.position;
this.currentSmoothedSolution = StringPullingPathSmoothing.SmoothPath(this.character.KinematicData, this.currentSolution);
this.currentSmoothedSolution.CalculateLocalPathsFromPathPositions(this.character.KinematicData.position);
this.character.Movement = new DynamicFollowPath(this.character.KinematicData, this.currentSmoothedSolution)
{
MaxAcceleration = 40.0f,
PathOffset = 0.1f,
MovementDebugColor = Color.blue
};
}
}
this.character.Update();
}
public override Path GetPath(GlobalPath path)
{
return path;
}
public bool WillViolate(KinematicData character, GlobalPath path)
{
//Vector3 beginPath = path.GetPosition(0);
//Vector3 endPath = path.GetPosition(1);
if (forgive)
{
forgive = false;
return forgive;
}
bool violation = false;
if (character.velocity == Vector3.zero)
{
return violation;
}
Vector3 characterPosition = character.position;
for (int index = 0; violation == false && index < obstacle.Length; index++)
{
//check if obstacle is worth considering
if ((characterPosition - obstacle[index].transform.position).sqrMagnitude < MinimumDistance * MinimumDistance)
{
Vector3 rayVector = character.velocity.normalized * MaxLookAhead;
bool hit = false;
Debug.DrawRay(character.position, rayVector, Color.red);
hit = SetTargetPosition(characterPosition, rayVector, MaxLookAhead, AvoidMargin, index);
if (!hit)
{
Vector3 leftWhisker = MathHelper.ConvertOrientationToVector(MathHelper.ConvertVectorToOrientation(rayVector) + WhiskersAngle);
Vector3 rayLeftWhisker = leftWhisker.normalized * MaxWhiskersLookAhead;
Debug.DrawRay(character.position, rayLeftWhisker, Color.red);
hit = SetTargetPosition(characterPosition, rayLeftWhisker, MaxWhiskersLookAhead, AvoidMargin, index);
Vector3 rightWhisker = MathHelper.ConvertOrientationToVector(MathHelper.ConvertVectorToOrientation(rayVector) - WhiskersAngle);
Vector3 rayRightWhisker = rightWhisker.normalized * MaxWhiskersLookAhead;
if (!hit)
{
Debug.DrawRay(character.position, rayRightWhisker, Color.red);
hit = SetTargetPosition(characterPosition, rayRightWhisker, MaxWhiskersLookAhead, AvoidMargin, index);
if (hit)
{
suggestedPosition = rayLeftWhisker + character.position;
}
}
else
{
suggestedPosition = rayRightWhisker + character.position;
}
}
if (hit)
{
violation = true;
}
}
}
return violation;
}
//private NavMeshPathGraph navMesh;
public DecomposerPersonPathCreation()
{
//this.navMesh = NavigationManager.Instance.NavMeshGraphs[0];
personPath = new GlobalPath();
}
public void UpdateChannels(Goal other)
{
this.HasPosition = other.HasPosition;
if (other.HasPosition)
{
this.position = other.position;
}
this.HasOrientation = other.HasOrientation;
if (other.HasOrientation)
{
this.orientation = other.orientation;
}
this.HasVelocity = other.HasVelocity;
if (other.HasVelocity)
{
this.velocity = other.velocity;
}
this.HasRotation = other.HasRotation;
if (other.HasRotation)
{
this.rotation = other.rotation;
}
this.IsNew = other.IsNew;
if (other.Path != null)
{
Path = other.Path;
this.CurrentParam = other.CurrentParam;
}
}
public GlobalPath GetPath(DynamicCharacter character, GlobalPath partialSolution)
{
return StringPullingPathSmoothing.SmoothPath(character.KinematicData, partialSolution);
}
