//Has to be lateupdate because we overwrite the transforms
void LateUpdate()
{
if (!animation.isPlaying)
{
return;
}
_ikSolver = new IKArm();
int arm; //arm index
int keyInd;
float s = TimingControl(Tp);
//map s into the whole spline
float t = (s * (_animInfo.NextGoal - _animInfo.PrevGoal) + _animInfo.PrevGoal) / _animInfo.AnimLength;
// Debug.Log(s + " " + Tp + " " + t + " " + Time.deltaTime);
if (_animInfo.NextGoal == _animInfo.PrevGoal)
{
t = 1f;
}
if (t < 0)
{
keyInd = _animInfo.FindKeyNumberAtNormalizedTime(-t); //find an imaginary key before the start of keyframes
}
else if (t > 1)
{
keyInd = _animInfo.FindKeyNumberAtNormalizedTime(2 - t); //find an imaginary key beyond the keyframes 1 - ( t - 1)
}
else
{
keyInd = _animInfo.FindKeyNumberAtNormalizedTime(t); //including via keys
}
// if (this.gameObject == GameObject.Find("AgentPrefab"))
// Debug.Log(s + " " + t);
//if (this.gameObject == GameObject.Find("AgentPrefab"))
// Debug.Log(keyInd);
float lt; //local time between keyframes including via keys
if (keyInd + 1 < _animInfo.Keys.Length)
{
if (t < 0)
{
lt = (float)(-t * _animInfo.AnimLength - _animInfo.Keys[keyInd].Time) / (_animInfo.Keys[keyInd + 1].Time - _animInfo.Keys[keyInd].Time);
}
else if (t > 1)
{
lt = (float)((2 - t) * _animInfo.AnimLength - _animInfo.Keys[keyInd].Time) / (_animInfo.Keys[keyInd + 1].Time - _animInfo.Keys[keyInd].Time);
}
else
{
lt = (float)(t * _animInfo.AnimLength - _animInfo.Keys[keyInd].Time) / (_animInfo.Keys[keyInd + 1].Time - _animInfo.Keys[keyInd].Time);
}
}
else
{
lt = 0f;
}
// if (this.gameObject == GameObject.Find("AgentPrefab"))
// Debug.Log("t: " + t + " lt: " + lt + " keyInd: " + keyInd + " frameInd: " + frameInd + " Curr: " + _animInfo.Curr + " PrevGoal: " + _animInfo.PrevGoal + " NextGoal: " + _animInfo.NextGoal + " prevGoalInd " + _animInfo.PrevGoalKeyInd + " Tp: " + Tp);
_animInfo.InterpolateWholeBody(keyInd, lt); //linear interpolation
_animInfo.ProjectWholeBodyBeyondKeyInd(keyInd, lt, t); //linear interpolation
//update both arms
for (arm = 0; arm < 2; arm++)
{
if (_interpolatorType == InterpolatorType.EndEffector) //position
{
Vector3 target = _animInfo.ComputeInterpolatedTarget(lt, keyInd, arm);
if (t < 0) // project target to a position before position at keyInd
{
Vector3 pivot = _animInfo.ComputeInterpolatedTarget(0f, 0, arm); //TCB interpolation for position
target = 2 * pivot - target;
}
else if (t > 1) // project target to a position beyond keyInd
{
Vector3 pivot = _animInfo.ComputeInterpolatedTarget(0, _animInfo.Keys.Length - 1, arm); //TCB interpolation for position
target = 2 * pivot - target;
// Debug.Log(Time.time + " "+ t);
}
if (arm == 0)
{
_targetL = target;
}
else
{
_targetR = target;
}
#if DEBUGMODE
//To see tension
if (arm == 0)
{
_targetLPrev.Add(_targetL);
}
else
{
_targetRPrev.Add(_targetR);
}
#endif
if (arm == 0)
{
((IKArm)_ikSolver).Solve(_arms[arm].Bones, -SwivelAngle, target);
}
else
{
((IKArm)_ikSolver).Solve(_arms[arm].Bones, SwivelAngle, target);
}
}
else if (_interpolatorType == InterpolatorType.ElbowAngle)
{
// _arms[arm].Elbow.transform.localRotation = Quaternion.Slerp(Keys[keyInd].BodyRot[i], Keys[keyInd + 1].BodyRot[i], lt);
_arms[arm].Elbow.transform.localRotation = _animInfo.ComputeInterpolatedElbowAngle(lt, keyInd, arm);
//Vector3 targetElbowAngle = _animInfo.ComputeInterpolatedElbowAngle(lt, keyInd, arm);
//_arms[arm].Elbow.transform.localEulerAngles = targetElbowAngle;
}
/*
* else if (_interpolatorType == InterpolatorType.ElbowPos) {
* _arms[arm].Elbow.position = _animInfo.ComputeInterpolatedElbowPos(lt, keyInd,arm); //TCB interpolation for position
*
* }
*/
Flourish(arm, Tp, t);
//Flourish(arm, Mathf.Pow(tp, texp)); //Funda
/*
*
* if (this.gameObject == GameObject.Find("AgentPrefab")) {
*
* if (Math.Abs(lt - 0) < 0.001) { //at keyframes
* _animInfo.Keys[keyInd].EePosUpdated[arm] = target;
* _animInfo.Keys[keyInd].TimeUpdated[arm] = Time.time;
*
* }
* if (t > 1f) {
* _animInfo.ComputeEeVelUpdated(arm);
* }
*
*
*
* }
*/
}
if (this.gameObject != GameObject.Find("AgentControlPrefab"))
{
if (Tp == 0)
{
_velArr.Clear();
_tppArr.Clear();
}
//Current velocity curve
GameObject velCurveCurr = GameObject.Find("VelCurveCurr");
if (velCurveCurr == null)
{
return;
}
velCurveCurr.GetComponent <LineRenderer>().SetVertexCount(_velArr.Count);
for (int i = 0; i < _velArr.Count; i++)
{
velCurveCurr.GetComponent <LineRenderer>().SetPosition(i, new Vector3(_tppArr[i], _velArr[i], 0));
}
//General velocity curve as in EMOTE
GameObject velCurveGen = GameObject.Find("VelCurveGeneral");
velCurveGen.GetComponent <LineRenderer>().SetVertexCount(5);
velCurveGen.GetComponent <LineRenderer>().SetPosition(0, new Vector3(0, 0, 0));
velCurveGen.GetComponent <LineRenderer>().SetPosition(1, new Vector3(T0, -V0, 0));
velCurveGen.GetComponent <LineRenderer>().SetPosition(2, new Vector3(Ti, _vi, 0));
velCurveGen.GetComponent <LineRenderer>().SetPosition(3, new Vector3(T1, -V1, 0));
velCurveGen.GetComponent <LineRenderer>().SetPosition(4, new Vector3(1, 0, 0));
/*velCurveGen.GetComponent<LineRenderer>().SetPosition(1, new Vector3( T0 / _animInfo.AnimSpeed, -V0, 0));
* velCurveGen.GetComponent<LineRenderer>().SetPosition(2, new Vector3( Ti / _animInfo.AnimSpeed, _vi, 0));
* velCurveGen.GetComponent<LineRenderer>().SetPosition(3, new Vector3( T1 / _animInfo.AnimSpeed, -V1, 0));
* velCurveGen.GetComponent<LineRenderer>().SetPosition(4, new Vector3( 1 / _animInfo.AnimSpeed, 0, 0));
*/
}
}