private void GetCurves(AnimationSet objectAnim, List <AnimationSet> animation)
{
//Rotation curves for each animated element in hierarchy
animation.ForEach(anim =>
{
if (null != anim)
{
animationList.Add(anim);
controllers.Add(anim.transform.GetComponent <RigGoalController>());
curves.Add(anim.GetCurve(AnimatableProperty.RotationX));
curves.Add(anim.GetCurve(AnimatableProperty.RotationY));
curves.Add(anim.GetCurve(AnimatableProperty.RotationZ));
}
});
animationList.Add(objectAnim);
controllers.Add(objectAnim.transform.GetComponent <RigGoalController>());
//Object rotation curves
curves.Add(objectAnim.GetCurve(AnimatableProperty.RotationX));
curves.Add(objectAnim.GetCurve(AnimatableProperty.RotationY));
curves.Add(objectAnim.GetCurve(AnimatableProperty.RotationZ));
//Position curves for root
curves.Add(animationList[0].GetCurve(AnimatableProperty.PositionX));
curves.Add(animationList[0].GetCurve(AnimatableProperty.PositionY));
curves.Add(animationList[0].GetCurve(AnimatableProperty.PositionZ));
}
public void AddFilteredKeyframeTangent(GameObject gobjet, AnimatableProperty property, AnimationKey key, int start, int end, bool updateCurves = true)
{
AnimationSet animationSet = GetObjectAnimation(gobjet);
Curve curve = animationSet.GetCurve(property);
// Filter rotation
if (property == AnimatableProperty.RotationX || property == AnimatableProperty.RotationY || property == AnimatableProperty.RotationZ)
{
AnimationKey previousKey = curve.GetPreviousKey(key.frame);
if (null != previousKey)
{
float delta = Mathf.DeltaAngle(previousKey.value, key.value);
key.value = previousKey.value + delta;
}
else
{
float delta = Mathf.DeltaAngle(0, key.value);
key.value = delta;
}
}
curve.AddTangentKey(key, start, end);
if (updateCurves)
{
onChangeCurve.Invoke(gobjet, property);
}
}
private void AddSegmentKeyframes(int frame, AnimationSet animation)
{
if (!animation.GetCurve(AnimatableProperty.PositionX).Evaluate(frame, out float posx))
{
posx = animation.transform.localPosition.x;
}
if (!animation.GetCurve(AnimatableProperty.PositionY).Evaluate(frame, out float posy))
{
posy = animation.transform.localPosition.y;
}
if (!animation.GetCurve(AnimatableProperty.PositionZ).Evaluate(frame, out float posz))
{
posz = animation.transform.localPosition.z;
}
if (!animation.GetCurve(AnimatableProperty.RotationX).Evaluate(frame, out float rotx))
{
rotx = animation.transform.localEulerAngles.x;
}
if (!animation.GetCurve(AnimatableProperty.RotationY).Evaluate(frame, out float roty))
{
roty = animation.transform.localEulerAngles.y;
}
if (!animation.GetCurve(AnimatableProperty.RotationZ).Evaluate(frame, out float rotz))
{
rotz = animation.transform.localEulerAngles.z;
}
if (!animation.GetCurve(AnimatableProperty.ScaleX).Evaluate(frame, out float scax))
{
scax = animation.transform.localScale.x;
}
if (!animation.GetCurve(AnimatableProperty.ScaleY).Evaluate(frame, out float scay))
{
scay = animation.transform.localScale.y;
}
if (!animation.GetCurve(AnimatableProperty.ScaleZ).Evaluate(frame, out float scaz))
{
scaz = animation.transform.localScale.z;
}
AddFilteredKeyframeTangent(animation.transform.gameObject,
new AnimationKey(frame, posx),
new AnimationKey(frame, posy),
new AnimationKey(frame, posz),
new AnimationKey(frame, rotx),
new AnimationKey(frame, roty),
new AnimationKey(frame, rotz),
new AnimationKey(frame, scax),
new AnimationKey(frame, scay),
new AnimationKey(frame, scaz));
}
private void RecursiveAnimation(List <Curve> curves, Transform target)
{
AnimationSet anim = GlobalState.Animation.GetObjectAnimation(target.gameObject);
if (null != anim)
{
curves.Add(anim.GetCurve(AnimatableProperty.PositionX));
}
foreach (Transform child in target)
{
RecursiveAnimation(curves, child);
}
}
private Matrix4x4 GetBoneMatrix(AnimationSet anim, int frame)
{
if (null == anim)
{
return(Matrix4x4.identity);
}
Vector3 position = Vector3.zero;
Curve posx = anim.GetCurve(AnimatableProperty.PositionX);
Curve posy = anim.GetCurve(AnimatableProperty.PositionY);
Curve posz = anim.GetCurve(AnimatableProperty.PositionZ);
if (null != posx && null != posy && null != posz)
{
if (posx.Evaluate(frame, out float px) && posy.Evaluate(frame, out float py) && posz.Evaluate(frame, out float pz))
{
position = new Vector3(px, py, pz);
}
}
Quaternion rotation = Quaternion.identity;
Curve rotx = anim.GetCurve(AnimatableProperty.RotationX);
Curve roty = anim.GetCurve(AnimatableProperty.RotationY);
Curve rotz = anim.GetCurve(AnimatableProperty.RotationZ);
if (null != posx && null != roty && null != rotz)
{
if (rotx.Evaluate(frame, out float rx) && roty.Evaluate(frame, out float ry) && rotz.Evaluate(frame, out float rz))
{
rotation = Quaternion.Euler(rx, ry, rz);
}
}
Vector3 scale = Vector3.one;
Curve scalex = anim.GetCurve(AnimatableProperty.ScaleX);
Curve scaley = anim.GetCurve(AnimatableProperty.ScaleY);
Curve scalez = anim.GetCurve(AnimatableProperty.ScaleZ);
if (null != scalex && null != scaley && null != scalez)
{
if (scalex.Evaluate(frame, out float sx) && scaley.Evaluate(frame, out float sy) && scalez.Evaluate(frame, out float sz))
{
scale = new Vector3(sx, sy, sz);
}
}
return(Matrix4x4.TRS(position, rotation, scale));
}
public void MoveKeyframe(GameObject gobject, AnimatableProperty property, int frame, int newFrame)
{
AnimationSet animationSet = GetObjectAnimation(gobject);
if (null == animationSet)
{
return;
}
animationSet.GetCurve(property).MoveKey(frame, newFrame);
if (!IsAnimating())
{
EvaluateAnimations();
}
}
public CommandAddKeyframeTangent(GameObject obj, AnimatableProperty property, int frame, int startFrame, int endFrame, List <AnimationKey> keysChanged)
{
gObject = obj;
this.property = property;
oldKeys = new List <AnimationKey>();
newKeys = keysChanged;
AnimationSet animationSet = GlobalState.Animation.GetObjectAnimation(gObject);
if (null == animationSet)
{
return;
}
Curve curve = animationSet.GetCurve(property);
curve.GetTangentKeys(startFrame, endFrame, ref oldKeys);
}
public void RemoveKeyframe(GameObject gobject, AnimatableProperty property, int frame)
{
AnimationSet animationSet = GetObjectAnimation(gobject);
if (null == animationSet)
{
return;
}
Curve curve = animationSet.GetCurve(property);
curve.RemoveKey(frame);
RemoveEmptyAnimationSet(gobject);
if (!IsAnimating())
{
EvaluateAnimations();
}
onChangeCurve.Invoke(gobject, property);
}
public CommandRemoveKeyframe(GameObject obj, AnimatableProperty property, int frame)
{
gObject = obj;
this.property = property;
AnimationSet animationSet = GlobalState.Animation.GetObjectAnimation(obj);
if (null == animationSet)
{
return;
}
Curve curve = animationSet.GetCurve(property);
if (null == curve)
{
return;
}
curve.TryFindKey(frame, out oldAnimationKey);
}
public void HoverLine(GameObject curveObject, Vector3 point)
{
LineRenderer line = curveObject.GetComponent <LineRenderer>();
GameObject gobject = CurveManager.GetObjectFromCurve(curveObject);
AnimationSet animationSet = GlobalState.Animation.GetObjectAnimation(gobject);
int frame = GetFrameFromPoint(line, point, animationSet);
DrawCurveGhost(gobject, frame);
if (CurveMode == CurveEditMode.Zone || CurveMode == CurveEditMode.Segment)
{
DrawZone(line, frame - zoneSize, frame + zoneSize);
}
if (CurveMode == CurveEditMode.Tangents)
{
Curve curve = animationSet.GetCurve(AnimatableProperty.PositionX);
int prev = curve.GetPreviousKeyFrame(frame);
int next = curve.GetNextKeyFrame(frame);
DrawZone(line, prev, next);
}
}
public bool Apply()
{
double[] new_theta = new double[paramCount];
for (int i = 0; i < paramCount; i++)
{
new_theta[i] = delta_theta[i] + theta[i];
}
for (int l = 0; l < animationCount; l++)
{
AnimationSet currentAnim = animationList[l];
for (int i = 0; i < 3; i++)
{
AnimatableProperty property = (AnimatableProperty)i + 3;
Curve curve = currentAnim.GetCurve(property);
for (int k = 0; k < keyCount; k++)
{
curve.GetKeyIndex(requiredKeyframe[k], out int index);
Vector2 inTangent = new Vector2((float)new_theta[12 * keyCount * l + 4 * (i * keyCount + k) + 0], (float)new_theta[12 * keyCount * l + 4 * (i * keyCount + k) + 1]);
Vector2 outTangent = new Vector2((float)new_theta[12 * keyCount * l + 4 * (i * keyCount + k) + 2], (float)new_theta[12 * keyCount * l + 4 * (i * keyCount + k) + 3]);
ModifyTangents(curve, index, inTangent, outTangent);
}
}
}
for (int i = 3; i < 6; i++)
{
Curve curve = animationList[0].GetCurve((AnimatableProperty)i - 3);
for (int k = 0; k < keyCount; k++)
{
curve.GetKeyIndex(requiredKeyframe[k], out int index);
Vector2 inTangent = new Vector2((float)new_theta[12 * keyCount * animationCount + 4 * ((i - 3) * keyCount + k) + 0], (float)new_theta[12 * keyCount * animationCount + 4 * ((i - 3) * keyCount + k) + 1]);
Vector2 outTangent = new Vector2((float)new_theta[12 * keyCount * animationCount + 4 * ((i - 3) * keyCount + k) + 2], (float)new_theta[12 * keyCount * animationCount + 4 * ((i - 3) * keyCount + k) + 3]);
ModifyTangents(curve, index, inTangent, outTangent);
}
}
return(true);
}
private static KeyProperty GetProperty(AnimationSet set, AnimatableProperty property, int frame)
{
Curve curve = set.GetCurve(property);
if (curve.GetKeyIndex(frame, out int index))
{
KeyProperty key = new KeyProperty()
{
value = curve.keys[index].value,
inTanX = curve.keys[index].inTangent.x,
inTanY = curve.keys[index].inTangent.y,
outTanX = curve.keys[index].outTangent.x,
outTanY = curve.keys[index].outTangent.y
};
return(key);
}
else
{
KeyProperty key = new KeyProperty();
return(key);
}
}
public CommandAddKeyframe(GameObject obj, AnimatableProperty property, int frame, float value, Interpolation interpolation)
{
gObject = obj;
this.property = property;
newAnimationKey = new AnimationKey(frame, value, interpolation);
AnimationSet animationSet = GlobalState.Animation.GetObjectAnimation(obj);
if (null == animationSet)
{
return;
}
Curve curve = animationSet.GetCurve(property);
if (null == curve)
{
return;
}
curve.TryFindKey(frame, out oldAnimationKey);
}
public void RemoveKeyframe(GameObject gobject, AnimatableProperty property, int frame, bool updateCurves = true, bool lockTangents = false)
{
AnimationSet animationSet = GetObjectAnimation(gobject);
if (null == animationSet)
{
return;
}
Curve curve = animationSet.GetCurve(property);
curve.RemoveKey(frame, lockTangents);
RemoveEmptyAnimationSet(gobject);
if (!IsAnimating())
{
EvaluateAnimations();
}
if (updateCurves)
{
onChangeCurve.Invoke(gobject, property);
}
}
private int GetFrameFromPoint(LineRenderer line, Vector3 pointPosition, AnimationSet anim)
{
Vector3 localPointPosition = line.transform.InverseTransformPoint(pointPosition);
Vector3[] positions = new Vector3[line.positionCount];
line.GetPositions(positions);
int closestPoint = 0;
float closestDistance = Vector3.Distance(positions[0], localPointPosition);
for (int i = 1; i < line.positionCount; i++)
{
float dist = Vector3.Distance(positions[i], localPointPosition);
if (dist < closestDistance)
{
closestDistance = dist;
closestPoint = i;
}
}
int firstAnimFrame = anim.GetCurve(AnimatableProperty.PositionX).keys[0].frame + GlobalState.Animation.StartFrame - 1;
return(closestPoint + firstAnimFrame);
}
public CommandAddKeyframeZone(GameObject obj, AnimatableProperty property, int frame, float value, int startFrame, int endFrame, Interpolation interpolation)
{
gObject = obj;
this.property = property;
oldKeys = new List <AnimationKey>();
newKeys = new List <AnimationKey>();
AnimationSet animationSet = GlobalState.Animation.GetObjectAnimation(gObject);
if (null == animationSet)
{
return;
}
Curve curve = animationSet.GetCurve(property);
if (null == curve)
{
return;
}
AnimationKey newKey = new AnimationKey(frame, value, interpolation);
curve.GetZoneKeyChanges(newKey, startFrame, endFrame, oldKeys, newKeys);
}
void AddCurve(GameObject gObject)
{
AnimationSet animationSet = GlobalState.Animation.GetObjectAnimation(gObject);
if (null == animationSet)
{
return;
}
Curve positionX = animationSet.GetCurve(AnimatableProperty.PositionX);
Curve positionY = animationSet.GetCurve(AnimatableProperty.PositionY);
Curve positionZ = animationSet.GetCurve(AnimatableProperty.PositionZ);
if (null == positionX || null == positionY || null == positionZ)
{
return;
}
if (positionX.keys.Count == 0)
{
return;
}
if (positionX.keys.Count != positionY.keys.Count || positionX.keys.Count != positionZ.keys.Count)
{
return;
}
int frameStart = Mathf.Clamp(positionX.keys[0].frame, GlobalState.Animation.StartFrame, GlobalState.Animation.EndFrame);
int frameEnd = Mathf.Clamp(positionX.keys[positionX.keys.Count - 1].frame, GlobalState.Animation.StartFrame, GlobalState.Animation.EndFrame);
Transform curves3DTransform = GlobalState.Instance.world.Find("Curves3D");
Matrix4x4 matrix = curves3DTransform.worldToLocalMatrix * gObject.transform.parent.localToWorldMatrix;
List <Vector3> positions = new List <Vector3>();
Vector3 previousPosition = Vector3.positiveInfinity;
for (int i = frameStart; i <= frameEnd; i++)
{
positionX.Evaluate(i, out float x);
positionY.Evaluate(i, out float y);
positionZ.Evaluate(i, out float z);
Vector3 position = new Vector3(x, y, z);
if (previousPosition != position)
{
position = matrix.MultiplyPoint(position);
positions.Add(position);
previousPosition = position;
}
}
int count = positions.Count;
GameObject curve = Instantiate(curvePrefab, curvesParent);
LineRenderer line = curve.GetComponent <LineRenderer>();
line.positionCount = count;
for (int index = 0; index < count; index++)
{
line.SetPosition(index, positions[index]);
}
line.startWidth = lineWidth / GlobalState.WorldScale;
line.endWidth = line.startWidth;
curves.Add(gObject, curve);
}
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);
}
/// <summary>
/// Initialize Curve Manipulation for objects
/// </summary>
public CurveManipulation(GameObject target, int frame, Transform mouthpiece, AnimationTool.CurveEditMode manipMode, int zoneSize, double tanCont)
{
isHuman = false;
manipulationMode = manipMode;
AnimationSet previousSet = GlobalState.Animation.GetObjectAnimation(target);
initialMouthMatrix = mouthpiece.worldToLocalMatrix;
Target = target;
Frame = frame;
continuity = tanCont;
if (!previousSet.GetCurve(AnimatableProperty.PositionX).Evaluate(frame, out float posx))
{
posx = target.transform.localPosition.x;
}
if (!previousSet.GetCurve(AnimatableProperty.PositionY).Evaluate(frame, out float posy))
{
posy = target.transform.localPosition.y;
}
if (!previousSet.GetCurve(AnimatableProperty.PositionZ).Evaluate(frame, out float posz))
{
posz = target.transform.localPosition.z;
}
if (!previousSet.GetCurve(AnimatableProperty.RotationX).Evaluate(frame, out float rotx))
{
rotx = target.transform.localEulerAngles.x;
}
if (!previousSet.GetCurve(AnimatableProperty.RotationY).Evaluate(frame, out float roty))
{
roty = target.transform.localEulerAngles.y;
}
if (!previousSet.GetCurve(AnimatableProperty.RotationZ).Evaluate(frame, out float rotz))
{
rotz = target.transform.localEulerAngles.z;
}
if (!previousSet.GetCurve(AnimatableProperty.ScaleX).Evaluate(frame, out float scax))
{
scax = target.transform.localScale.x;
}
if (!previousSet.GetCurve(AnimatableProperty.ScaleY).Evaluate(frame, out float scay))
{
scay = target.transform.localScale.y;
}
if (!previousSet.GetCurve(AnimatableProperty.ScaleZ).Evaluate(frame, out float scaz))
{
scaz = target.transform.localScale.z;
}
Vector3 initialPosition = new Vector3(posx, posy, posz);
Quaternion initialRotation = Quaternion.Euler(rotx, roty, rotz);
Vector3 initialScale = new Vector3(scax, scay, scaz);
objectData = new ObjectData()
{
Animation = new AnimationSet(previousSet),
InitialParentMatrix = target.transform.parent.localToWorldMatrix,
InitialParentMatrixWorldToLocal = target.transform.parent.worldToLocalMatrix,
InitialTRS = Matrix4x4.TRS(initialPosition, initialRotation, initialScale),
ScaleIndice = 1f
};
if (manipulationMode == AnimationTool.CurveEditMode.Zone)
{
startFrame = frame - zoneSize;
endFrame = frame + zoneSize;
}
if (manipulationMode == AnimationTool.CurveEditMode.Segment)
{
startFrame = frame - zoneSize;
endFrame = frame + zoneSize;
AddSegmentKeyframes(frame, previousSet);
}
if (manipMode == AnimationTool.CurveEditMode.Tangents)
{
startFrame = objectData.Animation.GetCurve(AnimatableProperty.PositionX).GetPreviousKeyFrame(frame);
endFrame = objectData.Animation.GetCurve(AnimatableProperty.PositionX).GetNextKeyFrame(frame);
}
}
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);
}
private void AddGoalCurve(RigGoalController goalController, RigController skinController)
{
if (!goalController.ShowCurve)
{
return;
}
AnimationSet goalAnimation = GlobalState.Animation.GetObjectAnimation(goalController.gameObject);
if (null == goalAnimation)
{
return;
}
Curve rotationX = goalAnimation.GetCurve(AnimatableProperty.RotationX);
if (rotationX.keys.Count == 0)
{
return;
}
int frameStart = Mathf.Clamp(rotationX.keys[0].frame, GlobalState.Animation.StartFrame, GlobalState.Animation.EndFrame);
int frameEnd = Mathf.Clamp(rotationX.keys[rotationX.keys.Count - 1].frame, GlobalState.Animation.StartFrame, GlobalState.Animation.EndFrame);
List <Vector3> positions = new List <Vector3>();
GameObject curve3D = curves.TryGetValue(goalController.gameObject, out GameObject current) ? current : Instantiate(curvePrefab, curvesParent);
Vector3 forwardOffset = (skinController.transform.forward * skinController.transform.localScale.x) * currentCurveOffset;
goalController.CheckAnimations();
for (int i = frameStart; i <= frameEnd; i++)
{
Vector3 position = curve3D.transform.InverseTransformDirection(goalController.FramePosition(i) - (forwardOffset * i));
positions.Add(position);
}
LineRenderer line = curve3D.GetComponent <LineRenderer>();
line.positionCount = positions.Count;
line.SetPositions(positions.ToArray());
line.startWidth = lineWidth / GlobalState.WorldScale;
line.endWidth = line.startWidth;
curve3D.transform.position = forwardOffset * GlobalState.Animation.CurrentFrame;
MeshCollider collider = curve3D.GetComponent <MeshCollider>();
Mesh lineMesh = new Mesh();
line.BakeMesh(lineMesh);
collider.sharedMesh = lineMesh;
curves[goalController.gameObject] = curve3D;
if (goalCurves.ContainsKey(skinController))
{
goalCurves[skinController].Add(curve3D);
}
else
{
goalCurves[skinController] = new List <GameObject>();
goalCurves[skinController].Add(curve3D);
}
}
void AddCurve(GameObject gObject)
{
AnimationSet animationSet = GlobalState.Animation.GetObjectAnimation(gObject);
if (null == animationSet)
{
return;
}
Curve positionX = animationSet.GetCurve(AnimatableProperty.PositionX);
Curve positionY = animationSet.GetCurve(AnimatableProperty.PositionY);
Curve positionZ = animationSet.GetCurve(AnimatableProperty.PositionZ);
if (null == positionX || null == positionY || null == positionZ)
{
return;
}
if (positionX.keys.Count == 0)
{
return;
}
if (positionX.keys.Count != positionY.keys.Count || positionX.keys.Count != positionZ.keys.Count)
{
return;
}
int frameStart = Mathf.Clamp(positionX.keys[0].frame, GlobalState.Animation.StartFrame, GlobalState.Animation.EndFrame);
int frameEnd = Mathf.Clamp(positionX.keys[positionX.keys.Count - 1].frame, GlobalState.Animation.StartFrame, GlobalState.Animation.EndFrame);
Matrix4x4 matrix = curvesParent.worldToLocalMatrix * gObject.transform.parent.localToWorldMatrix;
List <Vector3> positions = new List <Vector3>();
for (int i = frameStart; i <= frameEnd; i++)
{
positionX.Evaluate(i, out float x);
positionY.Evaluate(i, out float y);
positionZ.Evaluate(i, out float z);
Vector3 position = new Vector3(x, y, z);
position = matrix.MultiplyPoint(position);
positions.Add(position);
}
int count = positions.Count;
GameObject curve3D = curves.TryGetValue(gObject, out GameObject current) ? current : Instantiate(curvePrefab, curvesParent);
LineRenderer line = curve3D.GetComponent <LineRenderer>();
line.positionCount = count;
for (int index = 0; index < count; index++)
{
line.SetPosition(index, positions[index]);
}
line.startWidth = lineWidth / GlobalState.WorldScale;
line.endWidth = line.startWidth;
MeshCollider collider = curve3D.GetComponent <MeshCollider>();
Mesh lineMesh = new Mesh();
line.BakeMesh(lineMesh);
collider.sharedMesh = lineMesh;
curves[gObject] = curve3D;
}
