public static NQuaternion GetDeltaQuaternionWithDirectionVectors(NQuaternion from, NQuaternion to) { var a = NVector3.Transform(NVector3.UnitZ, NMatrix.CreateFromQuaternion(from)); var b = NVector3.Transform(NVector3.UnitZ, NMatrix.CreateFromQuaternion(to)); var dot = NVector3.Dot(a, b); if (dot < -0.999999) { var cross = NVector3.Cross(a, b); if (cross.Length() < 0.000001) { cross = NVector3.Cross(NVector3.UnitY, a); } cross = NVector3.Normalize(cross); return(NQuaternion.CreateFromAxisAngle(cross, Pi)); } else if (dot > 0.999999) { return(new NQuaternion(0, 0, 0, 1)); } else { var xyz = NVector3.Cross(a, b); var w = (float)(Math.Sqrt(a.Length() * a.Length() + b.Length() * b.Length()) + dot); return(new NQuaternion(xyz.X, xyz.Y, xyz.Z, w)); } }
public static FLVERBoneTransform FromMatrix4x4(Matrix4x4 m, bool applyMemes) { var result = new FLVERBoneTransform(); m.Decompose(out Vector3D s, out Quaternion rq, out Vector3D t); result.Translation = t.ToNumerics(); if (applyMemes) { result.Translation.X = -result.Translation.X; } result.Scale = s.ToNumerics(); NMatrix mat = NMatrix.Identity; if (applyMemes) { NQuaternion quat = SapMath.MirrorQuat(rq.ToNumerics()); mat = NMatrix.CreateFromQuaternion(quat); } else { mat = NMatrix.CreateFromQuaternion(new NQuaternion(rq.X, rq.Y, rq.Z, rq.W)); } result.Rotation = SapMath.MatrixToEulerXZY(mat); return(result); }
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!"); } }
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); }