//float toRad = Mathf.PI/180; // Computes radius of the moving circle and center offset public DynamicCarMove(Vector3 velocity, float speed, float acceleration, float phi, float r, DynamicCarState newState, float t) : base(t) { this.velocity = velocity.normalized; this.speed = speed; this.acceleration = acceleration; this.r = r; this.turn = (int)Mathf.Sign(phi); this.omega = turn * (speed / r) * toDeg; this.newState = newState; //Debug.Log("created"); // Setting centerOff //if (turn != 0) { // Vector3 normal = Quaternion.Euler(0, Mathf.Sign(turn)*90, 0) * velocity; // this.centerOff = normal.normalized * r; //} }
// 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); }