protected virtual void Update () {
deltaTime = Mathf.Min (Time.smoothDeltaTime*2, Time.deltaTime);
if (rp != null) {
//System.Diagnostics.Stopwatch w = new System.Diagnostics.Stopwatch();
//w.Start();
RichPathPart pt = rp.GetCurrentPart();
RichFunnel fn = pt as RichFunnel;
if (fn != null) {
//Clear buffers for reuse
Vector3 position = UpdateTarget ( fn );
//tr.position = ps;
//Only get walls every 5th frame to save on performance
if (Time.frameCount % 5 == 0) {
wallBuffer.Clear();
fn.FindWalls (wallBuffer, wallDist);
}
/*for (int i=0;i<wallBuffer.Count;i+=2) {
Debug.DrawLine (wallBuffer[i],wallBuffer[i+1],Color.magenta);
}*/
//Pick next waypoint if current is reached
int tgIndex = 0;
/*if (buffer.Count > 1) {
if ((buffer[tgIndex]-tr.position).sqrMagnitude < pickNextWaypointDist*pickNextWaypointDist) {
tgIndex++;
}
}*/
//Target point
Vector3 tg = buffer[tgIndex];
Vector3 dir = tg-position;
dir.y = 0;
bool passedTarget = Vector3.Dot (dir,currentTargetDirection) < 0;
//Check if passed target in another way
if (passedTarget && buffer.Count-tgIndex > 1) {
tgIndex++;
tg = buffer[tgIndex];
}
if (tg != lastTargetPoint) {
currentTargetDirection = (tg - position);
currentTargetDirection.y = 0;
currentTargetDirection.Normalize();
lastTargetPoint = tg;
//Debug.DrawRay (tr.position, Vector3.down*2,Color.blue,0.2f);
}
//Direction to target
dir = (tg-position);
dir.y = 0;
float magn = dir.magnitude;
//Write out for other scripts to read
distanceToWaypoint = magn;
//Normalize
dir = magn == 0 ? Vector3.zero : dir/magn;
Vector3 normdir = dir;
Vector3 force = Vector3.zero;
if (wallForce > 0 && wallDist > 0) {
float wLeft = 0;
float wRight = 0;
for (int i=0;i<wallBuffer.Count;i+=2) {
Vector3 closest = AstarMath.NearestPointStrict (wallBuffer[i],wallBuffer[i+1],tr.position);
float dist = (closest-position).sqrMagnitude;
if (dist > wallDist*wallDist) continue;
Vector3 tang = (wallBuffer[i+1]-wallBuffer[i]).normalized;
//Using the fact that all walls are laid out clockwise (seeing from inside)
//Then left and right (ish) can be figured out like this
float dot = Vector3.Dot (dir, tang) * (1 - System.Math.Max (0,(2*(dist / (wallDist*wallDist))-1)));
if (dot > 0) wRight = System.Math.Max (wRight, dot);
else wLeft = System.Math.Max (wLeft, -dot);
}
Vector3 norm = Vector3.Cross (Vector3.up, dir);
force = norm*(wRight-wLeft);
//Debug.DrawRay (tr.position, force, Color.cyan);
}
//Is the endpoint of the path (part) the current target point
bool endPointIsTarget = lastCorner && buffer.Count-tgIndex == 1;
if (endPointIsTarget) {
//Use 2nd or 3rd degree motion equation to figure out acceleration to reach target in "exact" [slowdownTime] seconds
//Clamp to avoid divide by zero
if (slowdownTime < 0.001f) {
slowdownTime = 0.001f;
}
Vector3 diff = tg - position;
diff.y = 0;
if (preciseSlowdown) {
//{ t = slowdownTime
//{ diff = vt + at^2/2 + qt^3/6
//{ 0 = at + qt^2/2
//{ solve for a
dir = (6*diff - 4*slowdownTime*velocity)/(slowdownTime*slowdownTime);
} else {
dir = 2*( diff - slowdownTime*velocity)/(slowdownTime*slowdownTime);
}
dir = Vector3.ClampMagnitude (dir, acceleration);
force *= System.Math.Min (magn/0.5f,1);
if (magn < endReachedDistance) {
//END REACHED
NextPart ();
}
} else {
dir *= acceleration;
}
//Debug.DrawRay (tr.position+Vector3.up, dir*3, Color.blue);
velocity += (dir + force*wallForce)*deltaTime;
if (slowWhenNotFacingTarget) {
float dot = (Vector3.Dot (normdir, tr.forward)+0.5f)*(1.0f/1.5f);
//velocity = Vector3.ClampMagnitude (velocity, maxSpeed * Mathf.Max (dot, 0.2f) );
float xzmagn = Mathf.Sqrt (velocity.x*velocity.x + velocity.z*velocity.z);
float prevy = velocity.y;
velocity.y = 0;
float mg = Mathf.Min (xzmagn, maxSpeed * Mathf.Max (dot, 0.2f) );
velocity = Vector3.Lerp ( tr.forward * mg, velocity.normalized * mg, Mathf.Clamp ( endPointIsTarget ? (magn*2) : 0, 0.5f, 1.0f) );
velocity.y = prevy;
} else {
// Clamp magnitude on the XZ axes
float xzmagn = Mathf.Sqrt (velocity.x*velocity.x + velocity.z*velocity.z);
xzmagn = maxSpeed/xzmagn;
if ( xzmagn < 1 ) {
velocity.x *= xzmagn;
velocity.z *= xzmagn;
//Vector3.ClampMagnitude (velocity, maxSpeed);
}
}
//Debug.DrawLine (tr.position, tg, lastCorner ? Color.red : Color.green);
if (endPointIsTarget) {
Vector3 trotdir = Vector3.Lerp(velocity,currentTargetDirection, System.Math.Max (1 - magn*2,0));
RotateTowards(trotdir);
} else {
RotateTowards(velocity);
}
//Applied after rotation to enable proper checks on if velocity is zero
velocity += deltaTime * gravity;
if (rvoController != null && rvoController.enabled) {
//Use RVOController
tr.position = position;
rvoController.Move (velocity);
} else
if (controller != null && controller.enabled) {
//Use CharacterController
tr.position = position;
controller.Move (velocity * deltaTime);
} else {
//Use Transform
float lasty = position.y;
position += velocity*deltaTime;
position = RaycastPosition (position, lasty);
tr.position = position;
}
}
else {
if (rvoController != null && rvoController.enabled) {
//Use RVOController
rvoController.Move (Vector3.zero);
}
}
if (pt is RichSpecial) {
RichSpecial rs = pt as RichSpecial;
if (!traversingSpecialPath) {
StartCoroutine(TraverseSpecial(rs));
}
}
//w.Stop();
//Debug.Log ((w.Elapsed.TotalMilliseconds*1000));
} else {
if (rvoController != null && rvoController.enabled) {
//Use RVOController
rvoController.Move (Vector3.zero);
} else
if (controller != null && controller.enabled) {
} else {
tr.position = RaycastPosition (tr.position, tr.position.y);
}
}
}
