public Vector3 Evaluate(int idxA, int idxB, float t)
{
if (idxA < 0 || idxA >= _numControlPoints || idxB < 0 || idxB >= _numControlPoints)
{
throw new System.IndexOutOfRangeException();
}
s_posBuffer[0] = _controlPoints[idxA].position - _controlPoints[idxA].tangent;
s_posBuffer[1] = _controlPoints[idxA].position;
s_posBuffer[2] = _controlPoints[idxB].position;
s_posBuffer[3] = _controlPoints[idxB].position + _controlPoints[idxB].tangent;
s_timesBuffer[0] = -0.5F;
s_timesBuffer[1] = 0F;
s_timesBuffer[2] = 1F;
s_timesBuffer[3] = 1.5F;
return(CatmullRom.Interpolate(s_posBuffer, s_timesBuffer, t));
}
void Update()
{
if (!fullPoseSpline)
{
Vector3 a = A.position, b = B.position, c = C.position, d = D.position;
_spline = CatmullRom.ToCHS(a, b, c, d);
}
else
{
Pose a = A.ToPose(), b = B.ToPose(), c = C.ToPose(), d = D.ToPose();
_spline = CatmullRom.ToCHS(a.position, b.position, c.position, d.position);
_qSpline = CatmullRom.ToQuaternionCHS(a.rotation, b.rotation,
c.rotation, d.rotation);
if (poseEvaluationObj != null)
{
float incr = 1f / RESOLUTION;
var t = 0f;
_evalObjCopies[0] = poseEvaluationObj;
for (int i = 0; i <= RESOLUTION; i++)
{
var obj = _evalObjCopies[i];
if (obj == null)
{
obj = Instantiate(poseEvaluationObj);
obj.transform.parent = poseEvaluationObj.transform.parent;
_evalObjCopies[i] = obj;
}
obj.transform.position = _spline.Value.PositionAt(t);
obj.transform.rotation = _qSpline.Value.RotationAt(t);
t += incr;
}
}
}
}
