// Creates a list of moves.
/* Returns the shortest set of moves between two kinematic car states. */
public Tuple <List <Move>, DynamicCarState> MovesTo(DynamicCarState other)
{
List <Move> moves = new List <Move> ();
// create the move
//float dr = Tangents.RotationAngle (orientation, other.position - this.position);
float time = timeStep;
// Sign of the acceleration determined by the difference in velocities
// magnitude of acceleration is either maxAcc or the value needed to accelerate to reach the target speed.
float acc = (speed < other.speed ? 1 : -1) * Mathf.Min(maxAcc, Mathf.Abs(this.speed - other.speed) / time);
////Debug.Log ("acc:" + acc);
// The angle from current orientation to the destination is used to determine how much and in which direction the car should steer.
float rotAngle = Tangents.RotationAngle(orientation, other.position - this.position);
// The sign of the steering * maximum or sufficient steering angle.
float phi = Mathf.Sign(rotAngle) * Mathf.Min(Mathf.Abs(rotAngle), maxPhi);
// turning radius determined and used to calculate the angular velocity
float r = L / Mathf.Tan(Mathf.Abs(phi) * toRad);
// Find the new state:
DynamicCarState newState;
float endSpeed = speed + acc * time;
float angle = ((speed + endSpeed) / (2 * r)) * time * toDeg;
Vector3 carToCenter = r * (Quaternion.Euler(0, 90 * Mathf.Sign(phi), 0) * orientation).normalized;
Vector3 centerToCar = -carToCenter;
Vector3 center = position + carToCenter;
Vector3 newPosition = Quaternion.Euler(0, angle * Mathf.Sign(phi), 0) * centerToCar + center;
Vector3 newOrientation = Quaternion.Euler(0, angle * Mathf.Sign(phi), 0) * orientation;
newState = new DynamicCarState(newPosition.x,
newPosition.z,
newOrientation,
speed + acc * time);
////Debug.Log ("pos: " + newState.position);
//Debug.Log ("Moving from: "+position+", or: "+orientation+"speed: "+speed+
// "\nTowards: "+other.position+", or: "+other.orientation+"speed: "+other.speed+
// "\nphi: " + phi + ", r: "+r+", angle: "+angle+", acc: "+acc+
// "\nnew state: "+newState.position + ", or: "+newState.orientation+", speed"+newState.speed);
// //Debug.Log (speed + acc * time);
Move move = new DynamicCarMove(orientation, speed, acc, phi, r, newState, time);
moves.Add(move);
return(new Tuple <List <Move>, DynamicCarState> (moves, newState));
}
// Initializes and runs rrt
override protected void LocalStart()
{
// Setting state options
DynamicCarState.maxAcc = maxAcc;
DynamicCarState.maxPhi = maxPhi;
DynamicCarState.L = L;
DynamicCarState.r = L / Mathf.Tan(maxPhi * Mathf.PI / 180);
DynamicCarState.lo = lowLimitRRT;
DynamicCarState.hi = highLimitRRT;
DynamicCarState.timeStep = time;
// Set scale and rotate to right
transform.localScale = new Vector3(1, 1, L);
transform.rotation = Quaternion.Euler(0, 90, 0);
// Create states, rotation is right
startState = new DynamicCarState(startPos.x, startPos.y, Vector3.right, 0);
goalState = new DynamicCarState(goalPos.x, goalPos.y, Vector3.right, 0);
// Run rrt
rrt = new RRT <DynamicCarState>(
startState,
goalState,
DynamicCarState.GenerateRandom,
polys,
iterations,
neighborhood
);
// Remove the last move in the list and replace it with breaking
DynamicCarMove last =
rrt.moves[rrt.moves.Count - 1] as DynamicCarMove;
Vector3 lastVel = last.velocity;
float lastTime = last.speed / maxAcc;
Move lastMove = new DynamicPointMove(lastVel * last.speed,
-lastVel.normalized * maxAcc, lastTime);
moves = new Stack <Move>(
Enumerable.Concat(
new Move[] { lastMove },
Enumerable.Reverse(rrt.moves).Skip(1)
)
);
// Set moves and other data needed for base
cost = rrt.cost + lastTime - last.t;
rrtTime = rrt.runTime;
Debug.Log("Time: " + cost + " RRT: " + rrtTime);
// This part generates points for realistic path
GameObject tmp = new GameObject();
Transform tr = tmp.transform;
tr.position = transform.position;
tr.rotation = transform.rotation;
List <Move> tmpMoves = new List <Move>();
foreach (Move m in rrt.moves)
{
tmpMoves.Add(m.Copy());
}
foreach (Move m in tmpMoves)
{
while (m.t > 0)
{
m.MoveMe(tr, 0.1f);
poss.Add(tr.position);
}
}
Destroy(tmp);
}
