void Update()
{
Target.TrajectoryCorrection = Correction;
Target.TrajectoryControl = false;
Trajectory.Point pivot = Trajectory.Points[Current]; //GetClosestTrajectoryPoint(Target.transform.position);
Trajectory.Point[] future = GetFutureTrajectory(pivot);
//Target.SetTargetDirection((future[future.Length-1].GetPosition() - Target.transform.position).normalized);
//Target.SetTargetVelocity(future[future.Length-1].GetPosition() - pivot.GetPosition());
Target.SetTargetDirection(Vector3.zero);
Target.SetTargetVelocity(Vector3.zero);
Trajectory targetTrajectory = Target.GetTrajectory();
for (int i = 0; i < future.Length; i++)
{
float weight = (float)(i + 1) / (float)future.Length;
Trajectory.Point point = targetTrajectory.Points[60 + i + 1];
point.SetPosition((1f - weight) * Target.transform.position + weight * future[i].GetPosition());
//point.SetDirection(future[i].GetDirection());
point.SetDirection((future[i].GetDirection() + (future[i].GetPosition() - point.GetPosition()).normalized).normalized);
//point.SetDirection((future[i].GetDirection() + (future[future.Length-1].GetPosition() - point.GetPosition()).normalized).normalized);
point.SetVelocity(Vector3.Distance(pivot.GetPosition(), future[future.Length - 1].GetPosition()));
}
for (int i = 60; i < targetTrajectory.Points.Length; i++)
{
targetTrajectory.Points[i].Styles[0] = 0f;
targetTrajectory.Points[i].Styles[1] = 1f;
}
Current += 1;
if (Current == Trajectory.Points.Length)
{
Current = 0;
}
/*
* for(int i=0; i<Feet.Length; i++) {
* float heightThreshold = i==0 || i==1 ? 0.025f : 0.05f;
* float velocityThreshold = i==0 || i==1 ? 0.015f : 0.015f;
* Vector3 oldPosition = LastFeetPositions[i];
* Vector3 newPosition = Target.Joints[Feet[i]].position;
* float velocityWeight = Utility.Exponential01((newPosition-oldPosition).magnitude / velocityThreshold);
* float heightWeight = Utility.Exponential01(newPosition.y / heightThreshold);
* float weight = 1f - Mathf.Min(velocityWeight, heightWeight);
* Vector3 slide = newPosition - oldPosition;
* slide.y = 0f;
* Slidings.Add(weight * slide.magnitude * 60f);
* LastFeetPositions[i] = newPosition;
* }
*
* Positions.Add(Target.transform.position);
* Vector3 p = pivot.GetPosition() - Target.transform.position;
* p.y = 0f;
* OffsetErrors.Add(p.magnitude);
* AngleErrors.Add(Mathf.Abs(Vector3.SignedAngle(pivot.GetDirection(), targetTrajectory.Points[60].GetDirection(), Vector3.up)));
*/
}