public void InitInterpolators(float tension, float continuity, float bias)
{
//Keyframe to time
ControlPoint[] controlPointsKF = new ControlPoint[Keys.Length];
for (int i = 0; i < Keys.Length; i++) {
controlPointsKF[i] = new ControlPoint();
controlPointsKF[i].Point = new Vector3(Keys[i].FrameNo, 0, 0);
controlPointsKF[i].TangentI = Vector3.zero;
controlPointsKF[i].TangentO = Vector3.zero;
controlPointsKF[i].Time = Keys[i].Time;
}
SIKeyTime = new TCBSpline(controlPointsKF, 0, 0, 0);
for(int arm = 0; arm < 2; arm++) {
//End effector
ControlPoint[] controlPointsEE = new ControlPoint[Keys.Length];
for(int i = 0; i< Keys.Length; i++) {
controlPointsEE[i] = new ControlPoint();
controlPointsEE[i].Point = Keys[i].EePos[arm];
controlPointsEE[i].TangentI = Vector3.zero;
controlPointsEE[i].TangentO = Vector3.zero;
controlPointsEE[i].Time = Keys[i].Time;
}
SIee[arm] = new TCBSpline(controlPointsEE, tension, continuity, bias);
//Elbow Rotation interpolator
ControlPoint[] controlPointsEA = new ControlPoint[Keys.Length];
for (int i = 0; i < Keys.Length; i++){
controlPointsEA[i] = new ControlPoint();
controlPointsEA[i].Rotation = Keys[i].ElbowRot[arm];
controlPointsEA[i].TangentI = Vector3.zero;
controlPointsEA[i].TangentO = Vector3.zero;
}
SIElbowAngle[arm] = new TCBSpline(controlPointsEA, tension, continuity, bias);
/*
//Elbow position
ControlPoint[] controlPointsEP = new ControlPoint[Keys.Length];
for (int i = 0; i < Keys.Length; i++) {
controlPointsEP[i] = new ControlPoint();
controlPointsEP[i].Point = Keys[i].ElbowPos[arm];
controlPointsEP[i].TangentI = Vector3.zero;
controlPointsEP[i].TangentO = Vector3.zero;
controlPointsEP[i].FrameNo = Keys[i].FrameNo;
}
//Rotation interpolator
SIElbowPos[arm] = new TCBSpline(controlPointsEP, tension, continuity, bias);
*/
}
}
public void InitInterpolators(int arm, List<ControlPoint> controlPoints, float tension, float continuity, float bias)
{
ControlPoint[] controlPointsEE = new ControlPoint[controlPoints.Count];
for (int i = 0; i < controlPoints.Count; i++)
controlPointsEE[i] = controlPoints[i];
SIee[arm] = new TCBSpline(controlPointsEE, tension, continuity, bias);
}