private void Update()
{
float velMag = _velocity.magnitude;
GetLegsPlanted(out int numberOfLimbsPlanted, out float percentLimbsFromMinPlanted);
#region update ik controller
var data = new IKInterpData(transform, _yawAngle, _velocity, velMag, _minMaxVelocity);
for (int i = 0; i < Limbs.Count; i++)
{
Limbs[i].ManualUpdate(data);
}
#region sort limbs by last used index
//insertion sort from largest to smallest
for (int i = 0; i < Limbs.Count; i++)
{
int currentMax = -1;
int currentMaxIndex = -1;
for (int j = i; j < Limbs.Count; j++)
{
if (Limbs[j].LastRetargetAttemptIndex > currentMax)
{
currentMax = Limbs[j].LastRetargetAttemptIndex;
currentMaxIndex = j;
}
}
var cache1 = Limbs[i];
var cache2 = Limbs[currentMaxIndex];
Limbs[i] = cache2;
Limbs[currentMaxIndex] = cache1;
}
#endregion
#region retargeting
_timeBetweenLastSucessfulLegRetarget += Time.deltaTime;
//if haven't too recently retargeted a leg...
//find new target if ik wants new target
if (_timeBetweenLastSucessfulLegRetarget > MinIntervalBetweenLegRetarget)
#region retarget if a leg desires a retarget
{
for (int i = 0; i < Limbs.Count; i++)
{
if (numberOfLimbsPlanted < MinLegsOnGround) //if we need more legs to be planted, don't retarget planted legs
{
if (Limbs[i].IKState.DesiresNewTarget && !Limbs[i].IKState.Planted)
{
if (SetNewTarget(Limbs[i]))
{
break;
}
else
{
Limbs[i].LastRetargetAttemptIndex = 0;
}
}
}
else if (Limbs[i].IKState.DesiresNewTarget)
{
if (SetNewTarget(Limbs[i]))
{
break;
}
}
}
}
#endregion
bool SetNewTarget(IKController limb)
{
bool newTargetFound = limb.SetTarget(_minMaxVelocity, Mathf.DeltaAngle(_yawAngle, _lastPhysicFramesYawAngle), _yawAngle, _velocity, velMag);
for (int j = 0; j < Limbs.Count; j++)
{
Limbs[j].LastRetargetAttemptIndex++;
}
limb.LastRetargetAttemptIndex = 0;
if (newTargetFound)
{
_timeBetweenLastSucessfulLegRetarget = 0;
}
return(newTargetFound);
}
#endregion
#endregion
}
public void ManualUpdate(IKInterpData interpData)
{
SolveIKAndDetermineStates();
InterpolateToTargetAndDetermineIfPlanted();
InterpolateIncrement();
//there are 3 basic states of operation: 1) no target data == interp to resting position 2) out of reach ik 3) and in reach leg ik
void SolveIKAndDetermineStates()
{
if (TargetData == null)
{
for (int i = 0; i < Segments.Count; i++)
{
Segments[i].ResetToResting();
}
}
else
{
_ikState.DistToTarget = Vector3.Distance(Segments[0].transform.position, TargetData.Value.Hit.point);
Vector3 goal = TargetData.Value.Hit.point;
//Debug.DrawRay(legLeft, normal, Color.red, .1f);
if (_ikState.DistToTarget < Reach)
{
for (int i = Segments.Count - 1; i >= 0; i--)
{
Segments[i].ResetToResting();
}
for (int solverCount = 0; solverCount < _maxIKItterations; solverCount++)
{
for (int i = Segments.Count - 1; i >= 0; i--)
{
//todo use abs distance
var tipDistFromTarget = Vector3.Distance(Segments[Segments.Count - 1].IKTarget.SegmentEnd.position, TargetData.Value.Hit.point);
if (tipDistFromTarget < _distanceToleranceToTarget)
{
break;
}
var currentSeg = Segments[i];
currentSeg.IKItterateInReach(Segments[Segments.Count - 1].IKTarget.SegmentEnd.position, goal);
}
}
}
else
{
for (int i = Segments.Count - 1; i >= 0; i--)
{
Segments[i].IKItterateInReach(Segments[Segments.Count - 1].IKTarget.SegmentEnd.position, goal);
}
}
#region does desire new target?
if (Mathf.Abs(TargetData.Value.InitialYaw - interpData.BodyYaw) >= _maxYawChangeBeforeDesiresNewTarget)
{
_ikState.DesiresNewTarget = true;
return;
}
var shoulderDistFromTarget = Vector3.Distance(Segments[0].transform.position, TargetData.Value.Hit.point); //todo use abs distance
if (shoulderDistFromTarget > Reach)
{
_ikState.DesiresNewTarget = true;
return;
}
if (IsPointWithinNoGoZones(Segments[Segments.Count - 1].IKTarget.SegmentEnd.position))
{
_ikState.DesiresNewTarget = true;
return;
}
#endregion
}
}
void InterpolateToTargetAndDetermineIfPlanted()
{
float interpProgress;
if (TargetData.HasValue)
{
interpProgress = Mathf.Clamp01(_interpolationProgressRawCache / _totalTargetInterpTime);
var tipDistFromTarget = Vector3.Distance(Segments[Segments.Count - 1].IKTarget.SegmentEnd.position, TargetData.Value.Hit.point);
_ikState.Planted = tipDistFromTarget < _distanceToleranceToTarget;
}
else
{
_ikState.Planted = false;
interpProgress = Mathf.Clamp01(_interpolationProgressRawCache / _totalNoTargetInterpTime);
}
for (int i = 0; i < Segments.Count; i++)
{
Segments[i].InterpolateTowardsTarget(interpProgress);
}
}
void InterpolateIncrement()
{
_interpolationProgressRawCache += Time.deltaTime;
_ikState.TimeSinceNewTarget += Time.deltaTime;
}
}
