// Update is called once per frame
void Update()
{
if (behaviorNum != 0)
{
// update my position and rotation
this.transform.position += linearVelocity * Time.deltaTime;
Vector3 v = new Vector3(0, angularVelocity, 0);
this.transform.eulerAngles += v * Time.deltaTime;
}
// update linear and angular velocities
SteeringOutput steering = new SteeringOutput();
// Seek: target is "alsoMyTargetForPursueTho"
if (behaviorNum == 1)
{
text.text = "Seek";
Seek mySeek = new Seek();
mySeek.target = alsoMyTargetForPursueTho;
mySeek.character = this;
steering = mySeek.getSteering();
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
// Flee
else if (behaviorNum == 2)
{
Flee myFlee = new Flee();
text.text = "Flee";
myFlee.character = this;
myFlee.target = myTarget;
steering = myFlee.getSteering();
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
// Arrive
else if (behaviorNum == 3)
{
Arrive myArrive = new Arrive();
text.text = "Arrive";
myArrive.character = this;
myArrive.target = myTarget;
steering = myArrive.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
else
{
linearVelocity = Vector3.zero;
}
}
// Align
else if (behaviorNum == 4)
{
Align myAlign = new Align();
text.text = "Align";
myAlign.character = this;
myAlign.target = myTarget;
steering = myAlign.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Face
else if (behaviorNum == 5)
{
Face myFace = new Face();
text.text = "Face";
myFace.character = this;
myFace.target = myTarget;
steering = myFace.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Look where you're going
else if (behaviorNum == 6)
{
LWYG myLook = new LWYG();
text.text = "LWYG";
myLook.character = this;
steering = myLook.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Path following
else if (behaviorNum == 7)
{
text.text = "Path Follow";
myPathFollow.character = this;
myPathFollow.path = pathToFollow;
steering = myPathFollow.getSteering();
if (steering != null)
{
if (linearVelocity.magnitude > 2)
{
linearVelocity = linearVelocity.normalized * 2;
}
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Pursue
else if (behaviorNum == 8)
{
Pursue myPursue = new Pursue();
text.text = "Pursue";
myPursue.character = this;
myPursue.target = alsoMyTargetForPursueTho;
steering = myPursue.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Seperate
else if (behaviorNum == 9)
{
text.text = "Seperate";
mySeperate.character = this;
mySeperate.targets = targetsForSeperate;
steering = mySeperate.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Collision Avoidance
else if (behaviorNum == 10)
{
text.text = "Collision Avoidance";
CollisionAvoidance myCollision = new CollisionAvoidance();
myCollision.character = this;
myCollision.targets = targetsForSeperate;
steering = myCollision.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Obstacle Avoidance
else if (behaviorNum == 11)
{
text.text = "Obstacle Avoidance";
ObstacleAvoid myObstacleAvoid = new ObstacleAvoid();
myObstacleAvoid.character = this;
myObstacleAvoid.target = myTarget;
steering = myObstacleAvoid.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Flocking
else if (behaviorNum == 12)
{
text.text = "Flocking";
if (!setUp)
{
// BehaviorAndWeight1 : Seperate
BehaviorAndWeight behavior1 = new BehaviorAndWeight();
Seperate mySeperate = new Seperate();
mySeperate.character = this;
GameObject[] birds = GameObject.FindGameObjectsWithTag("Bird");
Kinematic[] kBirds = new Kinematic[birds.Length - 1];
int j = 0;
for (int i = 0; i < birds.Length - 1; i++)
{
if (birds[i] == this)
{
continue;
}
kBirds[j++] = birds[i].GetComponent <Kinematic>();
}
mySeperate.targets = kBirds;
behavior1.behavior = mySeperate;
behavior1.weight = 10f;
// BehaviorAndWeight2 : Arrive
BehaviorAndWeight behavior2 = new BehaviorAndWeight();
Arrive myArrive = new Arrive();
myArrive.character = this;
myArrive.target = myTarget;
behavior2.behavior = myArrive;
behavior2.weight = 10f;
// BehaviorAndWeight3 : Align
BehaviorAndWeight behavior3 = new BehaviorAndWeight();
Align myAlign = new Align();
myAlign.character = this;
myAlign.target = myTarget;
behavior3.behavior = myAlign;
behavior3.weight = 3f;
// BehaviorAndWeight4 : ObstacleAvoidance
BehaviorAndWeight behavior4 = new BehaviorAndWeight();
ObstacleAvoid myObstacle = new ObstacleAvoid();
myObstacle.character = this;
myObstacle.target = myTargetObstacle; // Does this make sense?
behavior4.behavior = myObstacle;
behavior4.weight = 1f;
// Lower priority steering behaviors: Arrive, Align, & Seperate
BlendedSteeringDemo myBlended = new BlendedSteeringDemo();
myBlended.behaviors = new BehaviorAndWeight[3];
myBlended.behaviors[0] = new BehaviorAndWeight();
myBlended.behaviors[0] = behavior1;
myBlended.behaviors[1] = new BehaviorAndWeight();
myBlended.behaviors[1] = behavior2;
myBlended.behaviors[2] = new BehaviorAndWeight();
myBlended.behaviors[2] = behavior3;
// Higher priority steering behavior: Obstacle avoidance
BlendedSteeringDemo myBlendedAvoid = new BlendedSteeringDemo();
myBlendedAvoid.behaviors = new BehaviorAndWeight[1];
myBlendedAvoid.behaviors[0] = new BehaviorAndWeight();
myBlendedAvoid.behaviors[0] = behavior4;
// Initialize myPriority's array of groups with two groups
myPriority.groups = new BlendedSteeringDemo[2];
myPriority.groups[0] = new BlendedSteeringDemo();
myPriority.groups[0] = myBlendedAvoid;
myPriority.groups[1] = new BlendedSteeringDemo();
myPriority.groups[1] = myBlended;
setUp = true;
}
steering = myPriority.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
}
// Update is called once per frame
void Update()
{
if (behaviorNum != 0)
{
// update my position and rotation
this.transform.position += linearVelocity * Time.deltaTime;
Vector3 v = new Vector3(0, angularVelocity, 0);
this.transform.eulerAngles += v * Time.deltaTime;
}
// update linear and angular velocities
SteeringOutput steering = new SteeringOutput();
// Seek: target is "alsoMyTargetForPursueTho"
if (behaviorNum == 1)
{
Seek mySeek = new Seek();
text.text = "Seek";
mySeek.target = alsoMyTargetForPursueTho;
mySeek.character = this;
steering = mySeek.getSteering();
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
// Flee
else if (behaviorNum == 2)
{
Flee myFlee = new Flee();
text.text = "Flee";
myFlee.character = this;
myFlee.target = myTarget;
steering = myFlee.getSteering();
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
// Arrive
else if (behaviorNum == 3)
{
Arrive myArrive = new Arrive();
text.text = "Arrive";
myArrive.character = this;
myArrive.target = myTarget;
steering = myArrive.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
else
{
linearVelocity = Vector3.zero;
}
}
// Align
else if (behaviorNum == 4)
{
Align myAlign = new Align();
text.text = "Align";
myAlign.character = this;
myAlign.target = myTarget;
steering = myAlign.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Face
else if (behaviorNum == 5)
{
Face myFace = new Face();
text.text = "Face";
myFace.character = this;
myFace.target = myTarget;
steering = myFace.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Look where you're going
else if (behaviorNum == 6)
{
LWYG myLook = new LWYG();
text.text = "LWYG";
myLook.character = this;
steering = myLook.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Path following
else if (behaviorNum == 7)
{
text.text = "Path Follow";
myPathFollow.character = this;
myPathFollow.path = pathToFollow;
steering = myPathFollow.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Pursue
else if (behaviorNum == 8)
{
Pursue myPursue = new Pursue();
text.text = "Pursue";
myPursue.character = this;
myPursue.target = alsoMyTargetForPursueTho;
steering = myPursue.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Seperate
else if (behaviorNum == 9)
{
text.text = "Seperate";
mySeperate.character = this;
mySeperate.targets = targetsForSeperate;
steering = mySeperate.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Collision Avoidance
else if (behaviorNum == 10)
{
text.text = "Collision Avoidance";
CollisionAvoidance myCollision = new CollisionAvoidance();
myCollision.character = this;
myCollision.targets = targetsForSeperate;
steering = myCollision.getSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
// Obstacle Avoidance
else if (behaviorNum == 11)
{
text.text = "Obstacle Avoidance";
ObstacleAvoid myObstacleAvoid = new ObstacleAvoid();
myObstacleAvoid.character = this;
myObstacleAvoid.target = myTarget;
steering = myObstacleAvoid.GetSteering();
if (steering != null)
{
linearVelocity += steering.linear * Time.deltaTime;
angularVelocity += steering.angular * Time.deltaTime;
}
}
}
public Tuple <float, float> speedAndTurn(GameObject car)
{
PathPlanningKart kart = car.GetComponent <PathPlanningKart>();
//Set current position and check if car needs to move backwards
if (kart.iterationOffset == 0)
{
kart.currentPosition = car.transform.position;
}
if (kart.isStuck && kart.isStuckOffset < 60)
{
kart.isStuckOffset += 1;
return(new Tuple <float, float>(-1.0f, 0));
}
else
{
kart.isStuck = false;
kart.isStuckOffset = 0;
}
kart.iterationOffset = (kart.iterationOffset + 1) % 100;
if (kart.iterationOffset == 99)
{
if (Vector3.Distance(kart.currentPosition, car.transform.position) <= 1.5f)
{
kart.isStuck = true;
return(new Tuple <float, float>(-1.0f, 0));
}
}
//Adjust steer accordingly if obstacles are present
float speed = 0;
float steer = 0;
//Obtain current movement direction of the car
//Determine current angle of car steer
Vector3 forward = car.transform.forward;
float carAngle = Mathf.Atan(forward.z / forward.x);
if (kart.dynamicReplan && kart.dynamicWayPoints != null && kart.dynamicWayPoints.Count > 0)
{
kart.nextWayPoints = kart.dynamicWayPoints;
kart.dynamicReplan = false;
cleanUpPath(kart);
}
if (kart.nextWayPoints != null && kart.nextWayPoints.Count == 0)
{
kart.nextWayPoints = null;
}
if (kart.current_waypoint >= kart.currentWayPoints.Count && kart.nextWayPoints != null)
{
cleanUpPath(kart);
}
Vector3 currentWayPoint = kart.currentWayPoints[kart.current_waypoint];
Vector3 desiredDirection = new Vector3(0, 0, 0);
desiredDirection.y = currentWayPoint.y;
desiredDirection.x = currentWayPoint.x - car.transform.position.x;
desiredDirection.z = currentWayPoint.z - car.transform.position.z;
float desiredAngle = Mathf.Atan(desiredDirection.z / desiredDirection.x);
//Conversion from radians to a steer between -1 and 1
if (carAngle - desiredAngle > Mathf.PI / 2)
{
desiredAngle += Mathf.PI;
}
if (carAngle - desiredAngle < -Mathf.PI / 2)
{
carAngle += Mathf.PI;
}
steer = (carAngle - desiredAngle) / (Mathf.PI / 2);
if (steer > 1)
{
steer = Mathf.Min(steer, 1f);
}
if (steer < -1)
{
steer = Mathf.Max(steer, -1f);
}
//If within small distance away to the current waypoint, move onto the next waypoint
if (desiredDirection.magnitude < 5)
{
lock (PathPlanningDataStructures.globalLock) {
if (kart.usesWaypoints [kart.current_waypoint])
{
PathPlanningDataStructures.nodeToCount[kart.currentWayPoints[kart.current_waypoint]] -= 1;
kart.usesWaypoints [kart.current_waypoint] = false;
}
}
kart.current_waypoint = kart.current_waypoint + 1;
justSwitchedWaypoint = true;
if (kart.current_waypoint >= kart.currentWayPoints.Count && kart.nextWayPoints != null)
{
cleanUpPath(kart);
}
}
else
{
justSwitchedWaypoint = false;
}
if (kart.current_waypoint + 1 < kart.currentWayPoints.Count)
{
if (Vector3.Distance(kart.transform.position, kart.currentWayPoints [kart.current_waypoint]) > 15)
{
kart.dynamicReplan = true;
}
}
else
{
if (Vector3.Distance(kart.transform.position, kart.currentWayPoints [kart.current_waypoint - 1]) > 15)
{
kart.dynamicReplan = true;
}
}
Vector3 inversePoint = kart.transform.InverseTransformPoint(kart.currentWayPoints[kart.currentWayPoints.Count - 1]);
if (inversePoint.z < 0)
{
Debug.Log("Going Backwards");
if (inversePoint.x > 0)
{
steer = 1;
}
else
{
steer = -1;
}
}
ObstacleAvoid obstacleAvoid = car.GetComponent <ObstacleAvoid> ();
if (obstacleAvoid.leftObs && !obstacleAvoid.rightObs)
{
steer = Mathf.Min(.5f / obstacleAvoid.LeftDis, 0.5f);
}
if (obstacleAvoid.rightObs && !obstacleAvoid.leftObs)
{
steer = -1 * Mathf.Min(.5f / obstacleAvoid.RightDis, 0.5f);
}
if (obstacleAvoid.centerObs && !obstacleAvoid.rightObs && !obstacleAvoid.leftObs)
{
steer = Mathf.Min(.5f / obstacleAvoid.CenterDis, 0.5f);
}
speed = Mathf.Sqrt(1.15f - (steer * steer));
return(new Tuple <float, float> (speed, steer));
}
public Tuple <float, float> speedAndTurn(GameObject car)
{
ObstacleAvoid obstacleAvoid = car.GetComponent <ObstacleAvoid> ();
float speed = 0;
float steer = 0;
if (obstacleAvoid.leftObs && !obstacleAvoid.rightObs)
{
steer = Mathf.Max(1.2f / obstacleAvoid.LeftDis, 0.35f);
}
if (obstacleAvoid.rightObs && !obstacleAvoid.leftObs)
{
steer = -1 * Mathf.Max(1.2f / obstacleAvoid.RightDis, 0.35f);
}
if (obstacleAvoid.centerObs && !obstacleAvoid.rightObs && !obstacleAvoid.leftObs)
{
steer = Mathf.Max(1.2f / obstacleAvoid.CenterDis, 0.35f);
}
if (obstacleAvoid.centerObs && (obstacleAvoid.leftObs || obstacleAvoid.rightObs))
{
steer = steer * 1.42f;
}
Kart kart = car.GetComponent <Kart> ();
Vector3 travelDirection = car.transform.InverseTransformPoint(new Vector3(kart.wayPoints[kart.current_point].x,
car.transform.position.y,
kart.wayPoints[kart.current_point].z));
// For skipping if the waypoint is behind the car
Vector3 relPosition = car.transform.InverseTransformPoint(kart.wayPoints [kart.current_point]);
if (relPosition.z <= 0)
{
int see_ahead = kart.current_point + 1;
if (see_ahead >= kart.wayPoints.Count)
{
see_ahead = 0;
}
Vector3 seeDirection = car.transform.InverseTransformPoint(new Vector3(kart.wayPoints[see_ahead].x,
car.transform.position.y,
kart.wayPoints[see_ahead].z));
if (seeDirection.z > 0)
{
kart.current_point = see_ahead;
return(speedAndTurn(car));
}
}
steer = travelDirection.x / travelDirection.magnitude + steer;
if (steer > 1)
{
//input_steer = 1;
steer = Mathf.Min(steer, 1.25f);
}
if (steer < -1)
{
//input_steer = -1;
steer = Mathf.Max(steer, -1.25f);
}
if (travelDirection.magnitude < 12)
{
kart.current_point = kart.current_point + 1;
if (kart.current_point >= kart.wayPoints.Count)
{
kart.current_point = 0;
}
}
speed = Mathf.Sqrt(1 - (steer * steer));
return(new Tuple <float, float> (speed, steer));
}
