public bool Setup()
{
Curve rotXCurve = objectAnimation.GetCurve(AnimatableProperty.RotationX);
rotXCurve.GetKeyIndex(firstFrame, out int firstIndex);
rotXCurve.GetKeyIndex(lastFrame, out int lastIndex);
if (currentFrame < firstFrame)
{
return(false);
}
if (currentFrame > lastFrame)
{
return(false);
}
requiredKeyframe = new List <int>()
{
firstFrame, lastFrame
};
keyCount = requiredKeyframe.Count;
int curveCount = curves.Count;
//number of curve * (in tangent x , in tangent y, out tangent x, out tangent y) * (k- , k+)
paramCount = curveCount * 4 * 2;
ds_thetaJob(paramCount, keyCount);
theta = new double[paramCount];
Stiffnes_D = new double[paramCount, paramCount];
Continuity_T = new double[paramCount, paramCount];
curvesMinMax = new Vector2[curveCount, 2];
lowerBound = new double[paramCount];
upperBound = new double[paramCount];
scale = new double[paramCount];
delta_theta_0 = new double[paramCount];
//Hierarchy rotation curves
for (int animIndex = 0; animIndex < animationCount; animIndex++)
{
for (int curve = 0; curve < 3; curve++)
{
int curveIndex = animIndex * 3 + curve;
AnimationKey previous1Key = curves[curveIndex].keys[firstIndex];
AnimationKey previous2Key = firstIndex > 0 ? curves[curveIndex].keys[firstIndex - 1] : new AnimationKey(previous1Key.frame, previous1Key.value, inTangent: Vector2.zero, outTangent: Vector2.zero);
AnimationKey next1Key = curves[curveIndex].keys[lastIndex];
AnimationKey next2key = lastIndex < curves[curveIndex].keys.Count - 1 ? curves[curveIndex].keys[lastIndex + 1] : new AnimationKey(next1Key.frame, next1Key.value, inTangent: Vector2.zero, outTangent: Vector2.zero);
GetTangents(curveIndex * 8, previous1Key, next1Key);
curvesMinMax[curveIndex, 0] = GetMinMax(previous2Key, previous1Key);
curvesMinMax[curveIndex, 1] = GetMinMax(next1Key, next2key);
float Min = controllers[animIndex].LowerAngleBound[curve];
float Max = controllers[animIndex].UpperAngleBound[curve];
FillLowerBounds(curveIndex, previous1Key, next1Key, Min, Max);
FillUpperBounds(curveIndex, previous1Key, next1Key, previous2Key, next2key, Min, Max);
GetContinuity(curveIndex * 8, Min, Max);
//k- in x, k- in y, k- out x, k- out y, k+ in x, k+ in y, k+ out x, k+ out y
for (int tan = 0; tan < 8; tan++)
{
int tanIndice = curveIndex * 8 + tan;
Stiffnes_D[tanIndice, tanIndice] = animIndex == animationCount - 1 ? 0 : controllers[animIndex].stiffness;
scale[tanIndice] = 1d;
delta_theta_0[tanIndice] = 0;
}
}
}
//Root position curves
int aIndex = animationCount;
for (int curve = 0; curve < 3; curve++)
{
int curveIndex = aIndex * 3 + curve;
AnimationKey previous1Key = curves[curveIndex].keys[firstIndex];
AnimationKey previous2Key = firstIndex > 0 ? curves[curveIndex].keys[firstIndex - 1] : new AnimationKey(previous1Key.frame, previous1Key.value, inTangent: Vector2.zero, outTangent: Vector2.zero);
AnimationKey next1Key = curves[curveIndex].keys[lastIndex];
AnimationKey next2Key = lastIndex < curves[curveIndex].keys.Count - 1 ? curves[curveIndex].keys[lastIndex + 1] : new AnimationKey(next1Key.frame, next1Key.value, inTangent: Vector2.zero, outTangent: Vector2.zero);
GetTangents(curveIndex * 8, previous1Key, next1Key);
curvesMinMax[curveIndex, 0] = GetMinMax(previous2Key, previous1Key);
curvesMinMax[curveIndex, 1] = GetMinMax(next1Key, next2Key);
GetContinuity(curveIndex * 8);
for (int tan = 0; tan < 8; tan++)
{
int tanIndice = curveIndex * 8 + tan;
Stiffnes_D[tanIndice, tanIndice] = controllers[0].stiffness;
lowerBound[tanIndice] = -10;
upperBound[tanIndice] = 10;
scale[tanIndice] = 1d;
delta_theta_0[tanIndice] = 0;
}
}
currentState = GetCurrentState(currentFrame);
targetState = new State()
{
position = targetPosition,
rotation = targetRotation,
time = currentFrame
};
Delta_s_prime = new double[7, 1];
for (int i = 0; i < 3; i++)
{
Delta_s_prime[i, 0] = targetState.position[i] - currentState.position[i];
}
if ((currentState.rotation * Quaternion.Inverse(targetState.rotation)).w < 0)
{
targetState.rotation = new Quaternion(-targetState.rotation.x, -targetState.rotation.y, -targetState.rotation.z, -targetState.rotation.w);
}
for (int i = 0; i < 4; i++)
{
Delta_s_prime[i + 3, 0] = targetState.rotation[i] - currentState.rotation[i];
}
Theta = Maths.ColumnArrayToArray(theta);
return(true);
}