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);
}
double[,] ds_dtheta(int frame, int n, int p, int K, List <int> requiredKeyframes)
{
double[,] Js1 = ds_dc(frame, n);
double[,] Js2 = dc_dtheta(frame, n, p, K, requiredKeyframes);
return(Maths.Multiply(Js1, Js2));
}
private IEnumerator ImportHierarchy(Assimp.Node node, Transform parent, GameObject go)
{
if (parent != null && parent != go.transform)
{
go.transform.parent = parent;
}
GlobalState.Instance.messageBox.ShowMessage("Importing Hierarchy : " + importCount);
// Do not use Assimp Decompose function, it does not work properly
// use unity decomposition instead
Matrix4x4 mat = new Matrix4x4(
new Vector4(node.Transform.A1, node.Transform.B1, node.Transform.C1, node.Transform.D1),
new Vector4(node.Transform.A2, node.Transform.B2, node.Transform.C2, node.Transform.D2),
new Vector4(node.Transform.A3, node.Transform.B3, node.Transform.C3, node.Transform.D3),
new Vector4(node.Transform.A4, node.Transform.B4, node.Transform.C4, node.Transform.D4)
);
Vector3 position, scale;
Quaternion rotation;
Maths.DecomposeMatrix(mat, out position, out rotation, out scale);
node.Transform.Decompose(out Assimp.Vector3D scale1, out Assimp.Quaternion rot, out Assimp.Vector3D trans);
AssignMeshes(node, go);
if (node.Parent != null)
{
go.transform.localPosition = position;
go.transform.localRotation = rotation;
go.transform.localScale = scale;
go.name = isHuman ? node.Name : Utils.CreateUniqueName(node.Name);
if (isHuman)
{
if (bones.ContainsKey(node.Name))
{
bones[node.Name] = go.transform;
}
if (node.Name.Contains("Hips"))
{
rootBone = go.transform;
}
}
}
if (scene.HasAnimations)
{
ImportAnimation(node, go, Quaternion.identity);
}
importCount++;
foreach (Assimp.Node assimpChild in node.Children)
{
GameObject child = new GameObject();
if (blocking)
{
ImportHierarchy(assimpChild, go.transform, child).MoveNext();
}
else
{
yield return(StartCoroutine(ImportHierarchy(assimpChild, go.transform, child)));
}
}
}
public void SetObjectMatrix(GameObject gobject, Matrix4x4 matrix)
{
Maths.DecomposeMatrix(matrix, out Vector3 position, out Quaternion rotation, out Vector3 scale);
SetObjectTransform(gobject, position, rotation, scale);
}
private IEnumerator ImportHierarchy(Assimp.Node node, Transform parent, GameObject go, Matrix4x4 cumulMatrix, Quaternion preRotation)
{
if (parent != null && parent != go.transform)
{
go.transform.parent = parent;
}
GlobalState.Instance.messageBox.ShowMessage("Importing Hierarchy : " + importCount);
// Do not use Assimp Decompose function, it does not work properly
// use unity decomposition instead
Matrix4x4 nodeMatrix = new Matrix4x4(
new Vector4(node.Transform.A1, node.Transform.B1, node.Transform.C1, node.Transform.D1),
new Vector4(node.Transform.A2, node.Transform.B2, node.Transform.C2, node.Transform.D2),
new Vector4(node.Transform.A3, node.Transform.B3, node.Transform.C3, node.Transform.D3),
new Vector4(node.Transform.A4, node.Transform.B4, node.Transform.C4, node.Transform.D4)
);
Maths.DecomposeMatrix(nodeMatrix, out Vector3 nodePosition, out Quaternion nodeRotation, out Vector3 nodeScale);
Maths.DecomposeMatrix(cumulMatrix, out Vector3 cumulPosition, out Quaternion cumulRotation, out Vector3 cumulScale);
if (node.Name.Contains("$AssimpFbx$") && node.HasChildren)
{
if (node.Name.Contains("Translation"))
{
nodePosition = cumulRotation * nodePosition;
}
else
{
nodePosition += cumulPosition;
}
if (node.Name.Contains("PreRotation"))
{
preRotation = nodeRotation;
nodeRotation = cumulRotation * nodeRotation;
}
else
{
nodeRotation = cumulRotation;
}
cumulMatrix = Matrix4x4.TRS(nodePosition, nodeRotation, nodeScale);
if (blocking)
{
ImportHierarchy(node.Children[0], parent, go, cumulMatrix, preRotation).MoveNext();
}
else
{
yield return(StartCoroutine(ImportHierarchy(node.Children[0], parent, go, cumulMatrix, preRotation)));
}
}
else
{
nodePosition = (cumulRotation * nodePosition) + cumulPosition;
node.Transform.Decompose(out Assimp.Vector3D scale1, out Assimp.Quaternion rot, out Assimp.Vector3D trans);
AssignMeshes(node, go);
if (node.Parent != null)
{
go.transform.localPosition = nodePosition;
go.transform.localRotation = nodeRotation;
go.transform.localScale = nodeScale;
go.name = isHuman ? node.Name : Utils.CreateUniqueName(node.Name);
if (isHuman)
{
if (bones.ContainsKey(node.Name))
{
bones[node.Name] = go.transform;
}
if (node.Name.Contains("Hips"))
{
rootBone = go.transform;
}
}
}
if (scene.HasAnimations)
{
ImportAnimation(node, go, cumulRotation);
}
nodeMatrix = Matrix4x4.TRS(Vector3.zero, cumulRotation * nodeRotation, nodeScale);
importCount++;
foreach (Assimp.Node assimpChild in node.Children)
{
GameObject child = new GameObject();
if (blocking)
{
ImportHierarchy(assimpChild, go.transform, child, nodeMatrix, Quaternion.identity).MoveNext();
}
else
{
yield return(StartCoroutine(ImportHierarchy(assimpChild, go.transform, child, nodeMatrix, Quaternion.identity)));
}
}
}
}
