// <summary>
// Use this for initialization
// </summary>
void Start()
{
startTime = Time.realtimeSinceStartup;
input = GetComponent <GetInput> ();
//GameObject o = GameObject.Find("Kart " + kartNum);
Debug.Log("here");
//Debug.Log("rrr"+o.transform);
rb = GetComponent <Rigidbody> ();
Debug.Log("rrr" + rb);
boost = 1f;
fr = GameObject.Find("wcfr " + kartNum).GetComponent <WheelCollider>();
fl = GameObject.Find("wcfl " + kartNum).GetComponent <WheelCollider>();
br = GameObject.Find("wcbr " + kartNum).GetComponent <WheelCollider>();
bl = GameObject.Find("wcbl " + kartNum).GetComponent <WheelCollider>();
fr.enabled = true;
fl.enabled = true;
br.enabled = true;
bl.enabled = true;
if (isAI)
{
ItemsAI.updateItems();
PathPlanningDataStructures.initializePathPlanning();
kartp = GetComponentInChildren <PathPlanningKart> ();
kartp.PathPlanInitialSegment();
MAX_SPEED *= .8f;
}
}
// <summary>
// Draws the current waypoints for an AI car
// </summary>
public void OnDrawGizmosSelected()
{
if (isAI)
{
PathPlanningKart k = GetComponent <PathPlanningKart> ();
List <Vector3> currentWayPoints = k.currentWayPoints;
if (currentWayPoints == null)
{
return;
}
for (int i = 0; i < currentWayPoints.Count; i++)
{
Vector3 point = currentWayPoints [i];
Gizmos.color = new Color(0.0f, 0.0f, 1.0f, 0.3f);
Gizmos.DrawCube(point, new Vector3(3.0f, 3.0f, 3.0f));
int x = i + 1;
if (x < currentWayPoints.Count)
{
Gizmos.color = Color.magenta;
Gizmos.DrawLine(point, currentWayPoints [x]);
}
}
}
}
private void cleanUpPath(PathPlanningKart kart)
{
freeUpNodes(kart);
kart.currentWayPoints = kart.nextWayPoints;
kart.usesWaypoints = new bool[kart.currentWayPoints.Count];
for (int i = 0; i < kart.usesWaypoints.Length; i++)
{
kart.usesWaypoints[i] = true;
}
kart.nextWayPoints = null;
kart.current_waypoint = 0;
}
private void freeUpNodes(PathPlanningKart kart)
{
lock (PathPlanningDataStructures.globalLock) {
for (int i = 0; i < kart.currentWayPoints.Count; i++)
{
if (kart.usesWaypoints[i])
{
PathPlanningDataStructures.nodeToCount[kart.currentWayPoints[i]] -= 1;
}
}
}
}
//movement variable descriptions
//force from motor
// maxTourque (scaling for forward input)
// Effeciency (assume 1)
// Gear Ratio (1 - 150 some int)
// Angular speed (rpms)
// Wheel Radius
// uphill vs downhill (1 for uphill -1 for downhill)
// m =mass
// g = 9.8
// Gradient Angle (Gradient alpha) (angle of slope - 0)
//
//rolling resistance
// rolling resistance coeffecient (fr)
// wieight
// cos(alpha) slope
//
//drag force
// density of air (p=1.21 kg/m^3)
// drag coeffecient (cd=1.2)
// Area (cross sectional = .004 m^2)
// Velocity vector (y) squared magnitude
// Lw = car geoemetry = 0.15
//mass m - ydot squared = velocity squared in forward direction
//tan(phi) phi is max turning angle (30 degrees) * turning input to get current phi
// Use this for initialization
void Start()
{
input = GetComponent <GetInput>();
position.Set(50, 50, 0);
motorOutForce = (mTor * effec * gearRatio * angularSpeed / 60) / wheelRad;
//resistForce = 0;
rollingResistForce = rollResist * mass * g * Mathf.Cos(gradAngle);
//rollingResistForce = 0;
airResistForce = .5f * airDensity * dragCoef * areaCross;
centripetalForce = mass / lW;
if (isAI)
{
ItemsAI.updateItems();
PathPlanningKart k = GetComponent <PathPlanningKart> ();
k.PathPlanInitialSegment();
MAX_SPEED = MAX_SPEED * .95f;
}
}
//uncomment for original movement code
void FixedUpdate()
{
if (isAI)
{
PathPlanningKart k = GetComponent <PathPlanningKart>();
k.PathPlanNextSegment();
Tuple <float, float> t = carController.speedAndTurn(this.gameObject);
turnInput = (float)t.Second;
forwardInput = (float)t.First;
ItemsAI.updateItems();
k.UseItem();
}
else if (Input.GetKeyUp(KeyCode.P))
{
Time.timeScale = paused ? 1 : 0;
paused = !paused;
}
else
{
turnInput = input.getTurnInput();
forwardInput = input.getForwardInput();
}
//how quickly do we want to turn
currentDeltaTurn = DELTA_TURN * turnInput;
//takes care of ideling to go back straight && also turning
//Debug.Log ("(turnValue + currentDeltaTurn): "+(turnValue + currentDeltaTurn)+" MAXTU: "+(MAX_TURN)+" speed: "+speed);
if (Mathf.Abs(turnInput) < .1 && turnValue > 0)
{
turnValue = 0;
// Debug.Log("Idle Pos");
if (turnValue < DELTA_TURN)
{
turnValue = 0;
}
else
{
turnValue -= DELTA_TURN;
}
}
else if (Mathf.Abs(turnInput) < .1 && turnValue < 0)
{
turnValue = 0;
// Debug.Log("Idle Neg");
if (turnValue > -DELTA_TURN)
{
turnValue = 0;
}
else
{
turnValue += DELTA_TURN;
}
}
else if ((turnValue + currentDeltaTurn) < (MAX_TURN) && (turnValue + currentDeltaTurn) > (-MAX_TURN))
{
// Debug.Log("Turn");
turnValue += currentDeltaTurn;
}
//Debug.Log("DeltaTurn: " + currentDeltaTurn);
//Debug.Log("turnValue: " + turnValue);
//Debug.Log("Vert: "+Input.GetAxis(vertical));
//which way do we want to go
//takes care of ACCELeration
speed += ACCEL * forwardInput;
if (speed < -MAX_SPEED)
{
speed = -MAX_SPEED;
}
if (speed > MAX_SPEED)
{
speed = MAX_SPEED;
}
if (forwardInput == 1 && speed < MAX_SPEED)
{
speed += ACCEL;
}
else if (forwardInput == -1 && speed > -MAX_SPEED)
{
speed -= ACCEL;
}
else if (forwardInput == 0 && speed < 0)
{
if (speed > -IDLE_ACCEL)
{
speed = 0;
}
else
{
speed += IDLE_ACCEL;
}
}
else if (forwardInput == 0 && speed > 0)
{
if (speed < IDLE_ACCEL)
{
speed = 0;
}
else
{
speed -= IDLE_ACCEL;
}
}
else if (Mathf.Abs(speed) > MAX_SPEED)
{
speed = Mathf.Sign(speed) * MAX_SPEED;
}
//applies boost, if any
speed = speed * boost;
//calculates the direction displacement vector
rotationVector.Set(0, turnValue * speed * Time.deltaTime, 0);
direction = Quaternion.Euler(rotationVector) * direction;
//Debug.Log ("Direction: "+ direction);
//Debug.Log ("Speed: " +speed);
transform.position += direction * (speed * Time.deltaTime);
if (!goingBackwards)
{
transform.rotation = Quaternion.LookRotation(direction);
}
else
{
transform.rotation = Quaternion.LookRotation(-1 * direction);
}
//newEquations ();
//equations();
//updateLogVectors ();
if (writePositionsToFile)
{
WriteToRepo();
}
//uncomment for original movement code
}
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));
}
// <summary>
// Use this for initialization
// </summary>
void Start()
{
startTime = Time.realtimeSinceStartup;
input = GetComponent<GetInput> ();
//GameObject o = GameObject.Find("Kart " + kartNum);
Debug.Log("here");
//Debug.Log("rrr"+o.transform);
rb = GetComponent<Rigidbody> ();
Debug.Log("rrr" + rb);
boost = 1f;
fr = GameObject.Find("wcfr "+kartNum).GetComponent<WheelCollider>();
fl = GameObject.Find("wcfl " + kartNum).GetComponent<WheelCollider>();
br = GameObject.Find("wcbr " + kartNum).GetComponent<WheelCollider>();
bl = GameObject.Find("wcbl " + kartNum).GetComponent<WheelCollider>();
fr.enabled = true;
fl.enabled = true;
br.enabled = true;
bl.enabled = true;
if (isAI) {
ItemsAI.updateItems ();
PathPlanningDataStructures.initializePathPlanning ();
kartp = GetComponentInChildren<PathPlanningKart> ();
kartp.PathPlanInitialSegment ();
MAX_SPEED *= .8f;
}
}
