public bool Compute()
{
double targetW = 20;
double continuityW = tangentContinuity * 10;
double stiffnessW = 50;
double[,] Js = ThetaFromJob(paramCount);
Q_opt = Maths.Add(Maths.Add(Maths.Multiply(2d * targetW, Maths.Multiply(Maths.Transpose(Js), Js)), Maths.Add(Maths.Multiply(2d * stiffnessW, Stiffnes_D),
Maths.Multiply(2d * continuityW, Continuity_T))), Maths.Multiply((double)Mathf.Pow(10, -6), Maths.Identity(paramCount)));
double[,] B_opt = Maths.Add(Maths.Multiply(-2d * targetW, Maths.Multiply(Maths.Transpose(Js), Delta_s_prime)), Maths.Multiply(2d * continuityW, Maths.Multiply(Continuity_T, Theta)));
b_opt = Maths.ArrayToColumnArray(B_opt);
alglib.minqpstate state_opt;
alglib.minqpreport rep;
alglib.minqpcreate(paramCount, out state_opt);
alglib.minqpsetquadraticterm(state_opt, Q_opt);
alglib.minqpsetlinearterm(state_opt, b_opt);
alglib.minqpsetstartingpoint(state_opt, delta_theta_0);
alglib.minqpsetbc(state_opt, lowerBound, upperBound);
alglib.minqpsetscale(state_opt, scale);
alglib.minqpsetalgobleic(state_opt, 0.0, 0.0, 0.0, 0);
alglib.minqpoptimize(state_opt);
alglib.minqpresults(state_opt, out delta_theta, out rep);
return(true);
}
public bool Setup()
{
// rotation curves * hierarchy size + root position curves
p = 3 * hierarchySize + 3;
theta = GetAllValues(p);
currentState = new State()
{
position = MeshController.transform.InverseTransformPoint(oTransform.position),
rotation = oTransform.rotation
};
desiredState = new State()
{
position = MeshController.transform.InverseTransformPoint(targetPosition),
rotation = targetRotation
};
double[,] Js = ds_dtheta(p);
double[,] DT_D = new double[p, p];
//Root rotation
for (int i = 0; i < 3; i++)
{
DT_D[i, i] = 1d;
}
//nonRoot rotation
for (int i = 3; i < (hierarchySize - 1) * 3; i++)
{
if (fullHierarchy[i / 3].TryGetComponent <RigGoalController>(out RigGoalController controller))
{
DT_D[i, i] = controller.stiffness;
}
else
{
DT_D[i, i] = 1d;
}
}
//root position
for (int i = 0; i < 3; i++)
{
int j = 3 * hierarchySize + i;
if (fullHierarchy[0].TryGetComponent <RigGoalController>(out RigGoalController controller))
{
DT_D[i, i] = controller.stiffness;
}
else
{
DT_D[j, j] = 1d;
}
}
double[,] Delta_s_prime = new double[7, 1];
for (int i = 0; i < 3; i++)
{
Delta_s_prime[i, 0] = desiredState.position[i] - currentState.position[i];
}
if ((currentState.rotation * Quaternion.Inverse(desiredState.rotation)).w < 0)
{
desiredState.rotation = new Quaternion(-desiredState.rotation.x, -desiredState.rotation.y, -desiredState.rotation.z, -desiredState.rotation.w);
}
for (int i = 0; i < 4; i++)
{
Delta_s_prime[i + 3, 0] = desiredState.rotation[i] - currentState.rotation[i];
}
double wm = 20;
double wd = 50;
Q_opt = Maths.Add(Maths.Add(Maths.Multiply(2d * wm, Maths.Multiply(Maths.Transpose(Js), Js)), Maths.Multiply(2d * wd, DT_D)), Maths.Multiply((double)Mathf.Pow(10, -6), Maths.Identity(p)));
double[,] B_opt = Maths.Multiply(-2d * wm, Maths.Multiply(Maths.Transpose(Js), Delta_s_prime));
b_opt = Maths.ArrayToColumnArray(B_opt);
lowerBound = InitializeUBound(p);
upperBound = InitializeVBound(p);
delta_theta_0 = new double[p];
s = new double[p];
for (int i = 0; i < p; i++)
{
s[i] = 1d;
delta_theta_0[i] = 0d;
}
return(true);
}
public bool TrySolver()
{
ObjectAnimation.curves[AnimatableProperty.PositionX].GetKeyIndex(startFrame, out int firstIndex);
int firstFrame = ObjectAnimation.curves[AnimatableProperty.PositionX].keys[firstIndex].frame;
ObjectAnimation.curves[AnimatableProperty.PositionX].GetKeyIndex(endFrame, out int lastIndex);
int lastFrame = ObjectAnimation.curves[AnimatableProperty.PositionX].keys[lastIndex].frame;
if (currentFrame < firstFrame)
{
return(false);
}
if (currentFrame > lastFrame)
{
return(false);
}
RequiredKeyframeIndices = FindRequiredTangents(firstFrame, lastFrame, ObjectAnimation.GetCurve(AnimatableProperty.PositionX));
int K = RequiredKeyframeIndices.Count;
int totalKeyframes = ObjectAnimation.GetCurve(AnimatableProperty.PositionX).keys.Count;
int n = 6;
int p = 24 * K;
double[] theta = GetAllTangents(p, K, RequiredKeyframeIndices);
double[,] Theta = ColumnArrayToArray(theta);
State currentState = GetCurrentState(currentFrame);
State desiredState = new State()
{
position = positionTarget,
euler_orientation = rotationTarget.eulerAngles,
time = currentFrame
};
double[,] Js = ds_dtheta(currentFrame, n, p, K, RequiredKeyframeIndices);
double[,] DT_D = new double[p, p];
for (int i = 0; i < p; i++)
{
DT_D[i, i] = 0d * 0d;
}
double[,] Delta_s_prime = new double[6, 1];
for (int i = 0; i <= 2; i++)
{
Delta_s_prime[i, 0] = desiredState.position[i] - currentState.position[i];
}
for (int i = 3; i <= 5; i++)
{
Delta_s_prime[i, 0] = -Mathf.DeltaAngle(desiredState.euler_orientation[i - 3], currentState.euler_orientation[i - 3]);
}
double[,] TT_T = new double[p, p];
for (int j = 0; j < p; j++)
{
TT_T[j, j] = 1d;
if (j % 4 == 0 || j % 4 == 1)
{
TT_T[j + 2, j] = -1d;
}
else
{
TT_T[j - 2, j] = -1d;
}
}
double wm = 100d;
double wb = tangentEnergy;
double wd = 1d;
double[,] Q_opt = Maths.Add(Maths.Add(Maths.Multiply(2d * wm, Maths.Multiply(Maths.Transpose(Js), Js)), Maths.Add(Maths.Multiply(2d * wd, DT_D), Maths.Multiply(2d * wb, TT_T))), Maths.Multiply((double)Mathf.Pow(10, -6), Maths.Identity(p)));
double[,] B_opt = Maths.Add(Maths.Multiply(-2d * wm, Maths.Multiply(Maths.Transpose(Js), Delta_s_prime)), Maths.Multiply(2d * wb, Maths.Multiply(TT_T, Theta)));
double[] b_opt = Maths.ArrayToColumnArray(B_opt);
double[] delta_theta_0 = new double[p];
double[] delta_theta;
double[] s = new double[p];
for (int i = 0; i < p; i++)
{
s[i] = 1d;
delta_theta_0[i] = 0d;
}
alglib.minqpstate state_opt;
alglib.minqpreport rep;
alglib.minqpcreate(p, out state_opt);
alglib.minqpsetquadraticterm(state_opt, Q_opt);
alglib.minqpsetlinearterm(state_opt, b_opt);
alglib.minqpsetstartingpoint(state_opt, delta_theta_0);
alglib.minqpsetscale(state_opt, s);
alglib.minqpsetalgobleic(state_opt, 0.0, 0.0, 0.0, 0);
alglib.minqpoptimize(state_opt);
alglib.minqpresults(state_opt, out delta_theta, out rep);
double[] new_theta = new double[p];
for (int i = 0; i < p; i++)
{
new_theta[i] = delta_theta[i] + theta[i];
}
for (int i = 0; i < p; i++)
{
if (System.Double.IsNaN(delta_theta[i]))
{
return(false);
}
}
for (int i = 0; i < 6; i++)
{
AnimatableProperty property = (AnimatableProperty)i;
Curve curve = ObjectAnimation.curves[property];
for (int k = 0; k < K; k++)
{
Vector2 inTangent = new Vector2((float)new_theta[4 * (i * K + k) + 0], (float)new_theta[4 * (i * K + k) + 1]);
Vector2 outTangent = new Vector2((float)new_theta[4 * (i * K + k) + 2], (float)new_theta[4 * (i * K + k) + 3]);
ModifyTangents(curve, RequiredKeyframeIndices[k], inTangent, outTangent);
}
}
return(true);
}
