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;
}
/// <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 SteeringPipeline()
{
Target = new KinematicData();
Targeters = new List<ITargeter>();
Decomposers = new List<IDecomposer>();
Constraints = new List<IConstraint>();
MaxConstraintSteps = 10;
}
public GlobalPath GetPath(KinematicData character, Goal goal)
{
if (goal.HasPosition && goal.Path != null)
{
return goal.Path;
}
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 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 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;
}
public SteeringPipeline(KinematicData character)
{
this.Target = new KinematicData();
this.Character = character;
this.DeadlockMovement = new DynamicWander
{
Character = this.Character,
MaxAcceleration = 10.0f
};
this.Targeters = new List<Targeter>();
this.Decomposers = new List<Decomposer>();
this.Constraints = new List<Constraint>();
}
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);
}
}
}
/// <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;
}
/// <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;
}
/// <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;
}
/// <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;
}
// Use this for initialization
void Start()
{
var textObj = GameObject.Find("InstructionsText");
if (textObj != null)
{
textObj.GetComponent<Text>().text =
"Instructions\n\n" +
"Red Character\n" +
"Q - Stationary\n" +
"W - Seek\n" +
"E - Flee\n" +
"R - Arrive\n" +
"T - Wander\n\n" +
"Green Character\n" +
"A - Stationary\n" +
"S - Seek\n" +
"D - Flee\n" +
"F - Arrive\n" +
"G - Wander\n";
}
var redObj = GameObject.Find("Red");
if (redObj != null) this.RedCharacter = new DynamicCharacter(redObj)
{
Drag = DRAG,
MaxSpeed = MAX_SPEED
};
var greenObj = GameObject.Find("Green");
if (greenObj != null) this.GreenCharacter = new DynamicCharacter(greenObj)
{
Drag = DRAG,
MaxSpeed = MAX_SPEED
};
this.RedMovementText = GameObject.Find("RedMovement").GetComponent<Text>();
this.GreenMovementText = GameObject.Find("GreenMovement").GetComponent<Text>();
#region movement initialization
var redKinematicData = new KinematicData(new StaticData(this.RedCharacter.GameObject.transform.position));
var greenKinematicData = new KinematicData(new StaticData(this.GreenCharacter.GameObject.transform.position));
this.RedDynamicSeek = new DynamicSeek
{
Character = redKinematicData,
Target = this.GreenCharacter.KinematicData,
MaxAcceleration = MAX_ACCELERATION
};
this.RedDynamicFlee = new DynamicFlee
{
Character = redKinematicData,
Target = this.GreenCharacter.KinematicData,
MaxAcceleration = MAX_ACCELERATION
};
this.RedDynamicWander = new DynamicWander()
{
Character = redKinematicData,
MaxAcceleration = MAX_ACCELERATION
};
this.RedDynamicArrive = new DynamicArrive()
{
Character = redKinematicData,
Target = this.GreenCharacter.KinematicData,
MaxAcceleration = MAX_ACCELERATION,
MaxSpeed = MAX_SPEED
};
this.GreenDynamicSeek = new DynamicSeek
{
Character = greenKinematicData,
Target = this.RedCharacter.KinematicData,
MaxAcceleration = MAX_ACCELERATION
};
this.GreenDynamicFlee = new DynamicFlee
{
Character = greenKinematicData,
Target = this.RedCharacter.KinematicData,
MaxAcceleration = MAX_ACCELERATION
};
this.GreenDynamicWander = new DynamicWander()
{
Character = greenKinematicData,
MaxAcceleration = MAX_ACCELERATION
};
this.GreenDynamicArrive = new DynamicArrive()
{
Character = greenKinematicData,
Target = this.RedCharacter.KinematicData,
MaxAcceleration = MAX_ACCELERATION,
MaxSpeed = MAX_SPEED,
SlowRadius = 30f,
StopRadius = 3f
};
#endregion
}
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;
}
public SteeringPipeline InitializeSteeringPipeline(DynamicCharacter orig, KinematicData dest)
{
//Pipeline
SteeringPipeline pipe = new SteeringPipeline(orig.KinematicData)
{
MaxAcceleration = 15.0f
};
//Targeter
Targeter MouseClickTargeter = new Targeter()
{
Target = dest
};
pipe.Targeters.Add(MouseClickTargeter);
//Decomposer
pathfindingDecomposer = new PathfindingDecomposer()
{
Graph = this.navMesh,
Heuristic = new EuclideanDistanceHeuristic()
};
pipe.Decomposers.Add(pathfindingDecomposer);
//Actuator - Default: Car behaviour
Actuator actuator = new CarActuator()
{
MaxAcceleration = 15.0f,
Character = orig.KinematicData
};
if (orig.GameObject.tag.Equals("Enemies"))
{
actuator = new TrollActuator()
{
MaxAcceleration = 10.0f,
Character = orig.KinematicData
};
}
pipe.Actuator = actuator;
//Constraints
foreach (DynamicCharacter troll in enemies)
{
TrollConstraint trollConstraint = new TrollConstraint()
{
Troll = troll,
margin = 1.0f
};
pipe.Constraints.Add(trollConstraint);
}
MapConstraint mapConstraint = new MapConstraint()
{
chars = character,
navMeshP = navMesh,
margin = 1.0f
};
pipe.Constraints.Add(mapConstraint);
return pipe;
}
public Goal Decompose(KinematicData character, Goal goal)
{
personPath.LocalPaths [0] = new LineSegmentPath(character.position, goal.position);
goal.Path = personPath;
return goal;
}
private void InitializeMainCharacter(GameObject[] obstacles)
{
this.Priority = new PriorityMovement
{
Character = this.RedCharacter.KinematicData
};
this.Blended = new BlendedMovement
{
Character = this.RedCharacter.KinematicData
};
foreach (var obstacle in obstacles)
{
//TODO: add your AvoidObstacle movement here
//avoidObstacleMovement = new DynamicAvoidObstacle(obstacle)
//{
// MaxAcceleration = MAX_ACCELERATION,
// AvoidMargin = AVOID_MARGIN,
// MaxLookAhead = MAX_LOOK_AHEAD,
// Character = this.RedCharacter.KinematicData,
// MovementDebugColor = Color.magenta
//};
//this.Blended.Movements.Add(new MovementWithWeight(avoidObstacleMovement,5.0f));
//this.Priority.Movements.Add(avoidObstacleMovement);
}
foreach (var otherCharacter in this.Characters)
{
if (otherCharacter != this.RedCharacter)
{
//TODO: add your AvoidCharacter movement here
//var avoidCharacter = new DynamicAvoidCharacter(otherCharacter.KinematicData)
//{
// Character = this.RedCharacter.KinematicData,
// MaxAcceleration = MAX_ACCELERATION,
// AvoidMargin = AVOID_MARGIN,
// MovementDebugColor = Color.cyan
//};
//this.Priority.Movements.Add(avoidCharacter);
}
}
var redKinematicData = new KinematicData(new StaticData(this.RedCharacter.GameObject.transform.position));
var wander = new DynamicWander
{
Character = this.RedCharacter.KinematicData,
Target = redKinematicData,
MaxAcceleration = MAX_ACCELERATION,
MovementDebugColor = Color.yellow
};
this.Priority.Movements.Add(wander);
this.Blended.Movements.Add(new MovementWithWeight(wander,obstacles.Length+this.Characters.Count));
this.RedCharacter.Movement = this.Blended;
}
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 < peopleToCollide.Length; index++)
{
Vector3 direction = peopleToCollide [index].transform.position - characterPosition;
//dentro do circulo
if (direction.sqrMagnitude <= this.MinimumDistance * this.MinimumDistance)
{
float angle = MathHelper.ConvertVectorToOrientation(direction);
float angleDifference = MathHelper.SmallestDifferenceBetweenTwoAngles(character.orientation, angle);
//dentro do cone
if (Mathf.Abs(angleDifference) <= FanAngle)
{
Vector3 rayVector = character.velocity.normalized * 10f;
if (angleDifference >= 0)
{
Vector3 rightWhisker = MathHelper.ConvertOrientationToVector(MathHelper.ConvertVectorToOrientation(rayVector) + FanAngle);
Vector3 rayRightWhisker = rightWhisker.normalized * 10f;
suggestedPosition = rayRightWhisker + character.position;
} else
{
Vector3 leftWhisker = MathHelper.ConvertOrientationToVector(MathHelper.ConvertVectorToOrientation(rayVector) + FanAngle);
Vector3 rayLeftWhisker = leftWhisker.normalized * 10f;
suggestedPosition = rayLeftWhisker + character.position;
}
violation = true;
}
}
}
return violation;
}
