// Update is called once per frame
void FixedUpdate()
{
if (Level.AllowMotion) {
/** calc closest poiunt on path */
if (cPP_i+1 < pc.getPathPointCount()-1) {
current_distance = Vector3.Distance(pc.getPathPoint(cPP_i), this.transform.position);
next_distance = Vector3.Distance(pc.getPathPoint(cPP_i+1), this.transform.position);
if (current_distance > next_distance) {
cPP_i++;
if (!pc.getPathObject(cPP_i).tag.Equals(prev_tag)) {
Debug.Log(pc.getPathObject(cPP_i).tag);
}
prev_tag = pc.getPathObject(cPP_i).tag;
}
}
/** push ship forward along the path */
if ((cPP_i + 1 < pc.getPathPointCount() - 1) && (logic.countdownOver)) {
Vector3 forward = (pc.getPathPoint(cPP_i + 1) - pc.getPathPoint(cPP_i)).normalized * 4000;
this.GetComponent<Rigidbody>().AddForce(forward);
}
/** rotate ship towards current path_point+40 */
if ((cPP_i + 40 < pc.getPathPointCount() - 1) && (logic.countdownOver)) {
// pid controller to adjust the torgue of the ship
// ref: http://webber.physik.uni-freiburg.de/~hon/vorlss02/Literatur/Ingenieurswiss/pid/pid+matlab/PID%20systems%20tutorial.htm
VectorPid angularVelocityController = new VectorPid(1.7766f, 0, 0.2553191f);
VectorPid headingController = new VectorPid(1.244681f, 0, 0.06382979f);
Vector3 target = pc.getPathPoint(cPP_i + 40);
var angularVelocityError = this.GetComponent<Rigidbody>().angularVelocity * -1;
Debug.DrawRay(this.transform.position, this.GetComponent<Rigidbody>().angularVelocity * 10, Color.black);
var angularVelocityCorrection = angularVelocityController.Update(angularVelocityError, Time.deltaTime);
Debug.DrawRay(this.transform.position, angularVelocityCorrection, Color.green);
this.GetComponent<Rigidbody>().AddTorque(angularVelocityCorrection);
var desiredHeading = target - this.transform.position;
Debug.DrawRay(this.transform.position, desiredHeading, Color.yellow);
var currentHeading = this.transform.forward;
Debug.DrawRay(this.transform.position, currentHeading * 15, Color.blue);
var headingError = Vector3.Cross(currentHeading, desiredHeading);
var headingCorrection = headingController.Update(headingError, Time.deltaTime);
this.GetComponent<Rigidbody>().AddTorque(headingCorrection);
}
/** drag ship to its connected point*/
// apply force
float perc_dist = current_distance / radius;
// is the ship in the inner 60% around the actual portalpoint?
if (perc_dist > 0.6f) {
// calc the impact 0 - 100% of the force that should be applied
float impact_factor = (perc_dist - 0.6f) / 0.4f; // 0 - 0.4
Vector3 push_f = ((pc.getPathPoint(cPP_i) - this.transform.position).normalized * 22500) * impact_factor;
this.GetComponent<Rigidbody>().AddForce(push_f);
}
/** limit velocity of ship */
float current_vel = fastEuclidDist(transform.position, prev_pos);
if (current_vel > 3.6f) { // too fast > reduce maxspeed
spbar.setSpeed(100);
this.GetComponent<Rigidbody>().velocity = this.GetComponent<Rigidbody>().velocity * 0.99f;
} else {
if (current_vel > 3.5f) {
spbar.setSpeed(100);
} else {
//Debug.Log("current spedd" + (current_vel / 3.6f));
int speed = Mathf.RoundToInt((current_vel / 3.6f) * 100);
spbar.setSpeed(speed);
}
}
prev_pos = transform.position;
}
}
