public void RotationMatrix_ToQuaternion_VsSystemNumerics()
{
RotationMatrix m;
Quaternion q;
SysMatrix44 m44, m44bis;
SysQuat sq;
Vector vecX, vecY;
int dir;
int runs = 500;
for (var i = 0; i < runs; i++)
{
if (i < 0.5 * runs)
{
vecX = Vector.RandomFromDoubles(-100, 100);
vecY = Vector.RandomFromDoubles(-100, 100);
}
else
{
vecX = Vector.RandomFromInts(-1, 1);
vecY = Vector.RandomFromInts(-1, 1);
}
dir = Vector.CompareDirections(vecX, vecY);
m = new RotationMatrix(vecX, vecY);
q = m.ToQuaternion();
Trace.WriteLine("");
Trace.WriteLine(vecX + " " + vecY + " dir:" + dir);
Trace.WriteLine(m);
Trace.WriteLine(q);
// Use the matrix's orthogonal values to create a M44 matrix with just rotation values:
m44 = new SysMatrix44((float)m.m00, (float)m.m01, (float)m.m02, 0,
(float)m.m10, (float)m.m11, (float)m.m12, 0,
(float)m.m20, (float)m.m21, (float)m.m22, 0,
0, 0, 0, 1);
m44 = SysMatrix44.Transpose(m44); // Numerics.Matrix4x4 uses a transposed convention, meaning the translation vector is horizontal in m41-42-43 instead of vertical in m14-24-34
sq = SysQuat.CreateFromRotationMatrix(m44);
m44bis = SysMatrix44.CreateFromQuaternion(sq);
Trace.WriteLine(m44);
Trace.WriteLine(sq);
Trace.WriteLine(m44bis);
Assert.IsTrue(q.IsEquivalent(new Quaternion(sq.W, sq.X, sq.Y, sq.Z)), "Quaternions are not equivalent!");
}
}
virtual public Transform Step(double time, LocationOptions options)
{
Clamp(options);
var up = new Vector3(normal.x, normal.y, -normal.z);
Vector3 east = new Vector3(local_orientation.v11, local_orientation.v21, local_orientation.v31);;
Vector3 north;
if (options.RotationOptions.HasFlag(RotationOptions.AlignToSurface))
{
east = Vector3.Normalize(east - (Vector3.Dot(east, up) * up));
north = Vector3.Cross(east, up);
}
else
{
north = new Vector3(-local_orientation.v12, -local_orientation.v22, -local_orientation.v32);
}
var m = new Matrix4x4(east.X, east.Y, east.Z, 0,
up.X, up.Y, up.Z, 0,
north.X, north.Y, north.Z, 0,
0, 0, 0, 1);
var qm = Quaternion.CreateFromRotationMatrix(m);
var r = qm * Rotation;
return(new Transform
{
Pos = { X = (float)position.x, Y = (float)position.y, Z = (float)position.z },
Rot = r
});
}
public static Scene ExportToScene(HavokAnimationData anim)
{
var scene = new Scene();
scene.RootNode = new Node("scene_root");
scene.RootNode.Metadata["FrameRate"] = new Metadata.Entry(MetaDataType.Int32, 14);
scene.RootNode.Metadata["TimeSpanStart"] = new Metadata.Entry(MetaDataType.UInt64, (ulong)0);
scene.RootNode.Metadata["TimeSpanStop"] = new Metadata.Entry(MetaDataType.UInt64, (ulong)0);
scene.RootNode.Metadata["CustomFrameRate"] = new Metadata.Entry(MetaDataType.Float, 1f / anim.FrameDuration);
scene.RootNode.Metadata["FrontAxisSign"] = new Metadata.Entry(MetaDataType.Int32, -1);
scene.RootNode.Metadata["OriginalUnitScaleFactor"] = new Metadata.Entry(MetaDataType.Int32, 100);
scene.RootNode.Metadata["UnitScaleFactor"] = new Metadata.Entry(MetaDataType.Int32, 100);
var a = new Assimp.Animation();
a.DurationInTicks = anim.Duration * 30;
a.TicksPerSecond = 30;
a.Name = anim.Name;
List <Node> animTrackNodes = new List <Node>();
for (int i = 0; i < anim.TransformTrackIndexToHkxBoneMap.Length; i++)
{
int boneIndex = anim.TransformTrackIndexToHkxBoneMap[i];
if (boneIndex < 0 || boneIndex > anim.hkaSkeleton.Bones.Capacity)
{
continue;
}
string trackName = anim.hkaSkeleton.Bones[boneIndex].Name.GetString();
var animTrack = new NodeAnimationChannel();
animTrack.NodeName = trackName;
for (int f = 0; f < anim.FrameCount; f++)
{
var t = anim.GetTransformOnFrame(i, f, enableLooping: false);
animTrack.PositionKeys.Add(new VectorKey(1.0 * f * anim.FrameDuration, new Vector3D(t.Translation.X * -100, t.Translation.Y * 100, t.Translation.Z * 100)));
animTrack.ScalingKeys.Add(new VectorKey(1.0 * f * anim.FrameDuration, new Vector3D(t.Scale.X, t.Scale.Y, t.Scale.Z)));
var q = t.Rotation;
//q.X *= -1;
//q.Y *= -1;
//q.Z *= -1;
//q.W *= -1;
q = SapMath.MirrorQuat(q);
//q.X *= -1;
//q.Y *= -1;
//q.Z *= -1;
//q.W *= -1;
animTrack.RotationKeys.Add(new QuaternionKey(1.0 * f * anim.FrameDuration, new Quaternion(q.W, q.X, q.Y, q.Z)));
}
a.NodeAnimationChannels.Add(animTrack);
var fakeNode = new Node(trackName);
animTrackNodes.Add(fakeNode);
}
List <Node> topLevelTrackNodes = new List <Node>();
for (int i = 0; i < animTrackNodes.Count; i++)
{
var hkxBoneIndex = anim.TransformTrackIndexToHkxBoneMap[i];
var parentBoneIndex = anim.hkaSkeleton.ParentIndices[hkxBoneIndex].data;
if (parentBoneIndex >= 0)
{
var parentTrackIndex = anim.HkxBoneIndexToTransformTrackMap[parentBoneIndex];
if (parentTrackIndex >= 0)
{
animTrackNodes[parentTrackIndex].Children.Add(animTrackNodes[i]);
}
}
else
{
topLevelTrackNodes.Add(animTrackNodes[i]);
}
}
var actualRootNode = new Node("root");
if (anim.RootMotion != null)
{
var animTrack = new NodeAnimationChannel();
animTrack.NodeName = actualRootNode.Name;
for (int f = 0; f < anim.FrameCount; f++)
{
var rootMotionOnFrame = anim.RootMotion.GetSampleClamped(f * anim.FrameDuration);
animTrack.PositionKeys.Add(new VectorKey(1.0 * f * anim.FrameDuration, new Vector3D(rootMotionOnFrame.X * -100, rootMotionOnFrame.Y * 100, rootMotionOnFrame.Z * 100)));
var q = NQuaternion.CreateFromRotationMatrix(NMatrix.CreateRotationY(rootMotionOnFrame.W));
animTrack.RotationKeys.Add(new QuaternionKey(1.0 * f * anim.FrameDuration, new Quaternion(q.W, q.X, q.Y, q.Z)));
}
a.NodeAnimationChannels.Add(animTrack);
}
foreach (var t in topLevelTrackNodes)
{
actualRootNode.Children.Add(t);
}
scene.RootNode.Children.Add(actualRootNode);
scene.Animations.Add(a);
return(scene);
}
public void Quaternion_ToRotationMatrix_ToQuaternion()
{
Quaternion q1, q2;
RotationMatrix m;
SysQuat sq1, sq2;
SysMatrix44 sm;
double w, x, y, z;
//// Temp dumps used for data viz... :)
//List<string> quatsIn = new List<string>();
//quatsIn.Add("W,X,Y,Z,FLIPPED,METHOD");
//List<string> quatsOut = new List<string>();
//quatsOut.Add("W,X,Y,Z,FLIPPED,METHOD");
// Test random quaternions
for (var i = 0; i < 2000; i++)
{
w = Random(-1, 1); // force vector-normalization
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
q1 = new Quaternion(w, x, y, z); // gets automatically normalized
m = q1.ToRotationMatrix();
q2 = m.ToQuaternion();
// Compare with System quaternions
sq1 = new SysQuat((float)q1.X, (float)q1.Y, (float)q1.Z, (float)q1.W); // use same q1 normalization
sm = SysMatrix44.CreateFromQuaternion(sq1);
sq2 = SysQuat.CreateFromRotationMatrix(sm);
Trace.WriteLine("");
Trace.WriteLine(w + " " + x + " " + y + " " + z);
Trace.WriteLine(q1);
Trace.WriteLine(m);
Trace.WriteLine(q2);
Trace.WriteLine(sq1);
Trace.WriteLine(sm);
Trace.WriteLine(sq2);
// TEMP: create a dump CSV file with these quats
// quatsIn.Add(string.Format("{0},{1},{2},{3},{4},{5}", q1.W, q1.X, q1.Y, q1.Z, q1 == q2 ? 0 : 1, method));
// quatsOut.Add(string.Format("{0},{1},{2},{3},{4},{5}", q2.W, q2.X, q2.Y, q2.Z, q1 == q2 ? 0 : 1, method));
//System.IO.File.WriteAllLines(@"quaternionsIn.csv", quatsIn, System.Text.Encoding.UTF8);
//System.IO.File.WriteAllLines(@"quaternionsOut.csv", quatsOut, System.Text.Encoding.UTF8);
// NOTE: second quaternion might be the opposite sign, i.e. q1 = -q2.
// Note that all quaternions multiplied by a real number (-1 in this case) represent the same spatial rotation
Assert.IsTrue(q1.IsEquivalent(q2), "Booo! :(");
}
// Test all permutations of unitary components quaternions (including zero)
for (w = -2; w <= 2; w += 0.5) // test vector + non-vector normalization
{
for (x = -1; x <= 1; x += 0.5)
{
for (y = -1; y <= 1; y += 0.5)
{
for (z = -1; z <= 1; z += 0.5)
{
q1 = new Quaternion(w, x, y, z); // gets automatically normalized
m = q1.ToRotationMatrix();
q2 = m.ToQuaternion();
// Compare with System quaternions
sq1 = new SysQuat((float)q1.X, (float)q1.Y, (float)q1.Z, (float)q1.W); // use same q1 normalization
sm = SysMatrix44.CreateFromQuaternion(sq1);
sq2 = SysQuat.CreateFromRotationMatrix(sm);
Trace.WriteLine("");
Trace.WriteLine(w + " " + x + " " + y + " " + z);
Trace.WriteLine(q1);
Trace.WriteLine(m);
Trace.WriteLine(q2);
Trace.WriteLine(sq1);
Trace.WriteLine(sm);
Trace.WriteLine(sq2);
Assert.IsTrue(q1.IsEquivalent(q2), "Booo! :(");
}
}
}
}
}
public static ImportedAnimation ImportFromAssimpScene(Scene scene,
AnimationImportSettings settings)
{
ImportedAnimation result = new ImportedAnimation();
var sceneMatrix = NMatrix.Identity;
if (!settings.FlipQuaternionHandedness)
{
sceneMatrix *= NMatrix.CreateScale(-1, 1, 1);
}
if (settings.ExistingHavokAnimationTemplate == null)
{
throw new NotImplementedException("Reading skeleton/binding from assimp scene not supported yet. Please import using existing havok animation as template.");
}
else
{
result.hkaSkeleton = settings.ExistingHavokAnimationTemplate.hkaSkeleton;
result.HkxBoneIndexToTransformTrackMap = settings.ExistingHavokAnimationTemplate.HkxBoneIndexToTransformTrackMap;
result.TransformTrackIndexToHkxBoneMap = settings.ExistingHavokAnimationTemplate.TransformTrackIndexToHkxBoneMap;
}
if (settings.ConvertFromZUp)
{
sceneMatrix *= NMatrix.CreateRotationZ((float)(Math.PI));
sceneMatrix *= NMatrix.CreateRotationX((float)(-Math.PI / 2.0));
}
var sceneMatrix_ForRootMotion = NMatrix.CreateScale(NVector3.One * settings.SceneScale) * sceneMatrix;
if (settings.UseRootMotionScaleOverride)
{
sceneMatrix_ForRootMotion = NMatrix.CreateScale(NVector3.One * settings.RootMotionScaleOverride) * sceneMatrix;
}
sceneMatrix = NMatrix.CreateScale(NVector3.One * settings.SceneScale) * sceneMatrix;
foreach (var anim in scene.Animations)
{
if (anim.HasNodeAnimations)
{
// Setup framerate.
double tickScaler = (settings.ResampleToFramerate / anim.TicksPerSecond);
result.Duration = anim.DurationInTicks != 0 ? // Don't divide by 0
(float)(anim.DurationInTicks / anim.TicksPerSecond) : 0;
result.FrameDuration = (float)(1 / settings.ResampleToFramerate);
//result.Duration += result.FrameDuration;
int frameCount = (int)Math.Round(result.Duration / result.FrameDuration);
double resampleTickMult = settings.ResampleToFramerate / anim.TicksPerSecond;
Dictionary <string, int> transformTrackIndexMapping
= new Dictionary <string, int>();
List <string> transformTrackNames = new List <string>();
// Populate transform track names.
foreach (var nodeChannel in anim.NodeAnimationChannels)
{
if (nodeChannel.NodeName == settings.RootMotionNodeName && settings.ExcludeRootMotionNodeFromTransformTracks)
{
continue;
}
transformTrackNames.Add(nodeChannel.NodeName);
}
result.TransformTrackToBoneIndices.Clear();
if (settings.ExistingBoneDefaults != null)
{
var boneNamesInExistingSkel = settings.ExistingBoneDefaults.Keys.ToList();
transformTrackNames = boneNamesInExistingSkel;
foreach (var tt in transformTrackNames)
{
result.TransformTrackToBoneIndices.Add(tt, boneNamesInExistingSkel.IndexOf(tt));
}
}
else
{
int i = 0;
foreach (var t in transformTrackNames)
{
result.TransformTrackToBoneIndices.Add(t, i++);
}
}
// Populate transform track names.
foreach (var nodeChannel in anim.NodeAnimationChannels)
{
//if (nodeChannel.NodeName == settings.RootMotionNodeName && settings.ExcludeRootMotionNodeFromTransformTracks)
// continue;
transformTrackIndexMapping.Add(nodeChannel.NodeName, transformTrackNames.IndexOf(nodeChannel.NodeName));
}
result.TransformTrackNames = transformTrackNames;
result.Frames = new List <ImportedAnimation.Frame>();
for (int i = 0; i <= frameCount; i++)
{
var f = new ImportedAnimation.Frame();
for (int j = 0; j < transformTrackNames.Count; j++)
{
if (settings.ExistingBoneDefaults != null && settings.ExistingBoneDefaults.ContainsKey(transformTrackNames[j]) && settings.InitalizeUnanimatedTracksToTPose)
{
f.BoneTransforms.Add(settings.ExistingBoneDefaults[transformTrackNames[j]]);
}
else
{
f.BoneTransforms.Add(NewBlendableTransform.Identity);
}
}
result.Frames.Add(f);
}
var rootMotionRotationFrames = new NQuaternion[frameCount + 1];
//DEBUGGING
var DEBUG_ALL_NODE_NAMES_SORTED = anim.NodeAnimationChannels.Select(n => n.NodeName).OrderBy(n => n).ToList();
for (int i = 0; i < anim.NodeAnimationChannelCount; i++)
{
var nodeChannel = anim.NodeAnimationChannels[i];
int lastKeyIndex = -1;
bool hasPosition = nodeChannel.HasPositionKeys;
bool hasRotation = nodeChannel.HasRotationKeys;
bool hasScale = nodeChannel.HasScalingKeys;
if (nodeChannel.NodeName.Contains("$AssimpFbx$_Translation"))
{
hasPosition = true;
hasRotation = false;
hasScale = false;
}
else if (nodeChannel.NodeName.Contains("$AssimpFbx$_Rotation"))
{
hasPosition = false;
hasRotation = true;
hasScale = false;
}
else if (nodeChannel.NodeName.Contains("$AssimpFbx$_Scaling"))
{
hasPosition = false;
hasRotation = false;
hasScale = true;
}
bool isRootMotionNode = nodeChannel.NodeName == settings.RootMotionNodeName || (nodeChannel.NodeName.StartsWith(settings.RootMotionNodeName) && nodeChannel.NodeName.Contains("_$AssimpFbx$_"));
if (isRootMotionNode)
{
if (hasPosition)
{
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.PositionKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
result.Frames[frame].RootMotionTranslation =
NVector3.Transform(keyPos.Value.ToNumerics(), sceneMatrix_ForRootMotion);
//if (settings.FlipQuaternionHandedness)
//{
// result.Frames[frame].RootMotionTranslation.X *= -1;
//}
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].RootMotionTranslation;
var blendTo = result.Frames[frame].RootMotionTranslation;
result.Frames[f].RootMotionTranslation = NVector3.Lerp(blendFrom, blendTo, lerpS);
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
result.Frames[f].RootMotionTranslation = result.Frames[lastKeyIndex].RootMotionTranslation;
}
}
if (hasRotation && settings.EnableRotationalRootMotion)
{
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.RotationKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var curFrameRotation = keyPos.Value.ToNumerics();
curFrameRotation.Y *= -1;
curFrameRotation.Z *= -1;
if (settings.FlipQuaternionHandedness)
{
curFrameRotation = SapMath.MirrorQuat(curFrameRotation);
}
if (frame >= 0 && frame < frameCount)
{
rootMotionRotationFrames[frame] = curFrameRotation;
}
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = rootMotionRotationFrames[lastKeyIndex];
var blendTo = curFrameRotation;
var blended = NQuaternion.Slerp(blendFrom, blendTo, lerpS);
//blended = NQuaternion.Normalize(blended);
rootMotionRotationFrames[f] = blended;
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
rootMotionRotationFrames[f] = rootMotionRotationFrames[lastKeyIndex];
}
}
}
if (isRootMotionNode)
{
hasPosition = false;
hasRotation = !settings.EnableRotationalRootMotion;
}
if (!(isRootMotionNode && !settings.ExcludeRootMotionNodeFromTransformTracks))
{
string nodeName = nodeChannel.NodeName;
int transformIndex = transformTrackIndexMapping[nodeName];
int memeIndex = nodeName.IndexOf("_$AssimpFbx$_");
if (memeIndex >= 0)
{
nodeName = nodeName.Substring(0, memeIndex);
}
if (transformIndex >= 0 && transformIndex < transformTrackNames.Count)
{
// TRANSLATION
if (hasPosition)
{
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.PositionKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var curFrameTransform = result.Frames[frame].BoneTransforms[transformIndex];
curFrameTransform.Translation = NVector3.Transform(keyPos.Value.ToNumerics(), sceneMatrix);
result.Frames[frame].BoneTransforms[transformIndex] = curFrameTransform;
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Translation;
var blendTo = curFrameTransform.Translation;
var blended = NVector3.Lerp(blendFrom, blendTo, lerpS);
var copyOfStruct = result.Frames[f].BoneTransforms[transformIndex];
copyOfStruct.Translation = blended;
result.Frames[f].BoneTransforms[transformIndex] = copyOfStruct;
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
var x = result.Frames[f].BoneTransforms[transformIndex];
x.Translation = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Translation;
result.Frames[f].BoneTransforms[transformIndex] = x;
}
}
// SCALE
if (hasScale)
{
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.ScalingKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var curFrameTransform = result.Frames[frame].BoneTransforms[transformIndex];
curFrameTransform.Scale = keyPos.Value.ToNumerics();
result.Frames[frame].BoneTransforms[transformIndex] = curFrameTransform;
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Scale;
var blendTo = curFrameTransform.Scale;
var blended = NVector3.Lerp(blendFrom, blendTo, lerpS);
var copyOfStruct = result.Frames[f].BoneTransforms[transformIndex];
copyOfStruct.Scale = blended;
result.Frames[f].BoneTransforms[transformIndex] = copyOfStruct;
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
var x = result.Frames[f].BoneTransforms[transformIndex];
x.Scale = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Scale;
result.Frames[f].BoneTransforms[transformIndex] = x;
}
}
// ROTATION
if (hasRotation)
{
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.RotationKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var curFrameTransform = result.Frames[frame].BoneTransforms[transformIndex];
curFrameTransform.Rotation = keyPos.Value.ToNumerics();
curFrameTransform.Rotation.Y *= -1;
curFrameTransform.Rotation.Z *= -1;
if (settings.FlipQuaternionHandedness)
{
curFrameTransform.Rotation = SapMath.MirrorQuat(curFrameTransform.Rotation);
}
result.Frames[frame].BoneTransforms[transformIndex] = curFrameTransform;
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Rotation;
var blendTo = curFrameTransform.Rotation;
var blended = NQuaternion.Slerp(blendFrom, blendTo, lerpS);
//blended = NQuaternion.Normalize(blended);
var copyOfStruct = result.Frames[f].BoneTransforms[transformIndex];
copyOfStruct.Rotation = blended;
result.Frames[f].BoneTransforms[transformIndex] = copyOfStruct;
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
var x = result.Frames[f].BoneTransforms[transformIndex];
x.Rotation = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Rotation;
result.Frames[f].BoneTransforms[transformIndex] = x;
}
}
}
else
{
Console.WriteLine("unmapped transform track.");
}
}
}
if (settings.BonesToFlipBackwardsAboutYAxis.Count > 0)
{
var trackIndicesToFlip = result.TransformTrackNames
.Select((x, i) => i)
.Where(x => settings.BonesToFlipBackwardsAboutYAxis.Contains(result.TransformTrackNames[x]))
.ToList();
foreach (var f in result.Frames)
{
foreach (var i in trackIndicesToFlip)
{
var t = f.BoneTransforms[i];
t.Rotation = NQuaternion.CreateFromRotationMatrix(NMatrix.CreateFromQuaternion(f.BoneTransforms[i].Rotation) * NMatrix.CreateRotationY(SapMath.Pi));
t.Translation = NVector3.Transform(t.Translation, NMatrix.CreateRotationY(SapMath.Pi));
f.BoneTransforms[i] = t;
}
}
}
result.FrameCount = frameCount;
result.Name = anim.Name ?? settings.ExistingHavokAnimationTemplate?.Name ?? "SAP Custom Animation";
float rootMotionRot = 0;
for (int f = 0; f < result.Frames.Count; f++)
{
if (f > 0 && settings.EnableRotationalRootMotion)
{
var curMat = NMatrix.CreateFromQuaternion(rootMotionRotationFrames[f]);
var oldMat = NMatrix.CreateFromQuaternion(rootMotionRotationFrames[f - 1]);
if (NMatrix.Invert(oldMat, out NMatrix inverseOldMat))
{
var deltaMat = curMat * inverseOldMat;
var deltaVec = NVector3.Transform(NVector3.UnitX, deltaMat);
float deltaAngle = (float)Math.Atan2(deltaVec.Z, deltaVec.X);
rootMotionRot += deltaAngle;
}
}
result.Frames[f].RootMotionRotation = rootMotionRot;
}
break;
}
}
result.RootMotion = new RootMotionData(
new NVector4(0, 1, 0, 0),
new NVector4(0, 0, 1, 0),
result.Duration, result.Frames.Select(f => f.RootMotion).ToArray());
// Copy first frame for loop?
//for (int i = 0; i < result.TransformTrackNames.Count; i++)
//{
// result.Frames[result.Frames.Count - 1].BoneTransforms[i] = result.Frames[0].BoneTransforms[i];
//}
var rootMotionStart = result.Frames[0].RootMotion;
for (int i = 0; i < result.Frames.Count; i++)
{
result.Frames[i].RootMotionTranslation.X -= rootMotionStart.X;
result.Frames[i].RootMotionTranslation.Y -= rootMotionStart.Y;
result.Frames[i].RootMotionTranslation.Z -= rootMotionStart.Z;
result.Frames[i].RootMotionRotation -= rootMotionStart.W;
var xyz = NVector3.Transform(result.Frames[i].RootMotion.XYZ(), NMatrix.CreateRotationY(-rootMotionStart.W));
result.Frames[i].RootMotionTranslation.X = xyz.X;
result.Frames[i].RootMotionTranslation.Y = xyz.Y;
result.Frames[i].RootMotionTranslation.Z = xyz.Z;
//for (int t = 0; t < result.Frames[i].BoneTransforms.Count; t++)
//{
// if (i > 0 && NQuaternion.Dot(result.Frames[i - 1].BoneTransforms[t].Rotation, result.Frames[i].BoneTransforms[t].Rotation) < 0.995)
// {
// var tf = result.Frames[i].BoneTransforms[t];
// tf.Rotation = NQuaternion.Conjugate(result.Frames[i].BoneTransforms[t].Rotation);
// result.Frames[i].BoneTransforms[t] = tf;
// }
//}
}
// HOTFIX FOR BAD FBX
if (result.Frames.Count >= 3)
{
result.Frames[result.Frames.Count - 1].RootMotionRotation = result.Frames[result.Frames.Count - 2].RootMotionRotation +
(result.Frames[result.Frames.Count - 2].RootMotionRotation - result.Frames[result.Frames.Count - 3].RootMotionRotation);
}
//var endFrame = new ImportedAnimation.Frame();
//foreach (var t in result.Frames[0].BoneTransforms)
//{
// endFrame.BoneTransforms.Add(t);
//}
//endFrame.RootMotionTranslation = result.Frames[result.Frames.Count - 1].RootMotionTranslation +
// (result.Frames[result.Frames.Count - 1].RootMotionTranslation - result.Frames[result.Frames.Count - 2].RootMotionTranslation);
//endFrame.RootMotionRotation = result.Frames[result.Frames.Count - 1].RootMotionRotation +
// (result.Frames[result.Frames.Count - 1].RootMotionRotation - result.Frames[result.Frames.Count - 2].RootMotionRotation);
////endFrame.RootMotionRotation = result.Frames[result.Frames.Count - 1].RootMotionRotation;
//result.Frames.Add(endFrame);
return(result);
}
