public void Multiply()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3, 9, -2), new QuaternionF(3, -2, 1, 9).Normalized, new Vector3F(7, -4, 2));
var result1 = SrtTransform.Multiply(a, b).ToMatrix44F();
var result2 = a * b;
Assert.IsTrue(Matrix44F.AreNumericallyEqual(result1, result2));
}
public void MultiplyVector4()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var v = new Vector4F(7, 9, -12, -2);
var result1 = a * v;
var result2 = a.ToMatrix44F() * v;
Assert.IsTrue(Vector4F.AreNumericallyEqual(result1, result2));
result1 = SrtTransform.Multiply(a, v);
result2 = a.ToMatrix44F() * v;
Assert.IsTrue(Vector4F.AreNumericallyEqual(result1, result2));
}
/// <summary>
/// Updates the absolute bone pose for the specified bone.
/// </summary>
/// <param name="boneIndex">The index of the bone.</param>
private void UpdateBonePoseAbsolute(int boneIndex)
{
if (_isBonePoseAbsoluteDirty[boneIndex])
{
lock (_syncRoot)
{
if (_isBonePoseAbsoluteDirty[boneIndex])
{
// Make sure relative bone pose is up-to-date.
UpdateBonePoseRelative(boneIndex);
int parentIndex = _skeletonPose.Skeleton.GetParent(boneIndex);
if (parentIndex < 0)
{
// No parent.
_bonePoseAbsolute[boneIndex] = _bonePoseRelative[boneIndex];
}
else
{
// Make sure parent is up-to-date. (Recursively update ancestors.)
UpdateBonePoseAbsolute(parentIndex);
//_bonePoseAbsolute[boneIndex] = _bonePoseAbsolute[parentIndex] * _bonePoseRelative[boneIndex];
SrtTransform.Multiply(ref _bonePoseAbsolute[parentIndex], ref _bonePoseRelative[boneIndex],
out _bonePoseAbsolute[boneIndex]);
}
#if !NETFX_CORE && !NET45
Thread.MemoryBarrier();
#else
Interlocked.MemoryBarrier();
#endif
_isBonePoseAbsoluteDirty[boneIndex] = false;
}
}
}
}
/// <summary>
/// Updates the relative bone pose for the specified bone.
/// </summary>
/// <param name="boneIndex">The index of the bone.</param>
private void UpdateBonePoseRelative(int boneIndex)
{
if (_isBonePoseRelativeDirty[boneIndex])
{
lock (_syncRoot)
{
if (_isBonePoseRelativeDirty[boneIndex])
{
// _bonePoseRelative[boneIndex] = _skeletonPose.Skeleton.BindPosesRelative[boneIndex] * _skeletonPose.BoneTransforms[boneIndex];
SrtTransform.Multiply(ref _skeletonPose.Skeleton.BindPosesRelative[boneIndex],
ref _skeletonPose.BoneTransforms[boneIndex], out _bonePoseRelative[boneIndex]);
#if !NETFX_CORE && !NET45
Thread.MemoryBarrier();
#else
Interlocked.MemoryBarrier();
#endif
_isBonePoseRelativeDirty[boneIndex] = false;
}
}
}
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <summary>
/// Initializes the skeleton.
/// </summary>
/// <param name="boneParents">The bone parents.</param>
/// <param name="boneNames">The bone names.</param>
/// <param name="bindPosesRelative">The bind poses.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="boneParents"/>, <paramref name="boneNames"/> or
/// <paramref name="bindPosesRelative"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// Either the given lists are empty, have different length, or the
/// <paramref name="boneParents"/> are invalid (parent bones must come be before their child
/// bones).
/// </exception>
internal void Initialize(IList <int> boneParents, IList <string> boneNames, IList <SrtTransform> bindPosesRelative)
{
if (boneParents == null)
{
throw new ArgumentNullException("boneParents");
}
if (boneNames == null)
{
throw new ArgumentNullException("boneNames");
}
if (bindPosesRelative == null)
{
throw new ArgumentNullException("bindPosesRelative");
}
var numberOfBones = boneParents.Count;
if (numberOfBones == 0)
{
throw new ArgumentException("boneParents list must not be empty.");
}
if (boneNames.Count == 0)
{
throw new ArgumentException("boneNames list must not be empty.");
}
if (bindPosesRelative.Count == 0)
{
throw new ArgumentException("bindPosesRelative list must not be empty.");
}
if (numberOfBones != boneNames.Count || numberOfBones != bindPosesRelative.Count)
{
throw new ArgumentException("The lists must have the same number of elements.");
}
// Check if bone parents come before children. This ordering also forbids cycles.
for (int index = 0; index < numberOfBones; index++)
{
var parentIndex = boneParents[index];
if (parentIndex >= index)
{
throw new ArgumentException("Invalid boneParents list. Parent bones must have a lower index than child bones.");
}
}
// Build list of bone nodes.
Bones = new Bone[numberOfBones];
var children = new List <int>();
for (int index = 0; index < numberOfBones; index++)
{
Bone bone = new Bone();
// Set parent index.
int parentIndex = boneParents[index];
if (parentIndex < -1)
{
parentIndex = -1;
}
bone.Parent = parentIndex;
// Create array of child indices.
children.Clear();
for (int childIndex = index + 1; childIndex < numberOfBones; childIndex++)
{
if (boneParents[childIndex] == index)
{
children.Add(childIndex);
}
}
if (children.Count > 0)
{
bone.Children = children.ToArray();
}
Bones[index] = bone;
}
// Initialize bone name/index dictionary.
if (BoneNames == null)
{
BoneNames = new Dictionary <string, int>(numberOfBones);
}
else
{
BoneNames.Clear();
}
for (int index = 0; index < numberOfBones; index++)
{
var name = boneNames[index];
if (name != null)
{
BoneNames[name] = index;
}
}
// Copy relative bind poses.
if (BindPosesRelative == null)
{
BindPosesRelative = new SrtTransform[numberOfBones];
}
for (int index = 0; index < numberOfBones; index++)
{
BindPosesRelative[index] = bindPosesRelative[index];
}
// Initialize absolute bind poses (inverse).
if (BindPosesAbsoluteInverse == null)
{
BindPosesAbsoluteInverse = new SrtTransform[numberOfBones];
}
// First store the non-inverted BindTransforms in model space.
BindPosesAbsoluteInverse[0] = BindPosesRelative[0];
for (int index = 1; index < numberOfBones; index++)
{
var parentIndex = Bones[index].Parent;
//BindPosesAbsoluteInverse[index] = BindPosesAbsoluteInverse[parentIndex] * BindPosesRelative[index];
SrtTransform.Multiply(ref BindPosesAbsoluteInverse[parentIndex], ref BindPosesRelative[index], out BindPosesAbsoluteInverse[index]);
}
// Invert matrices.
for (int index = 0; index < numberOfBones; index++)
{
BindPosesAbsoluteInverse[index].Invert();
}
}
/// <summary>
/// Updates all bone transformations.
/// </summary>
public void Update()
{
// This method updates does not check the individual dirty flags. If something is
// dirty all transforms are updated. Checking the dirty flags makes the average
// case (whole skeleton is animated) a lot slower.
if (_isDirty)
{
lock (_syncRoot)
{
if (_isDirty)
{
var skeleton = _skeletonPose.Skeleton;
int numberOfBones = skeleton.NumberOfBones;
// ----- Update dirty relative bone poses.
var boneTransforms = _skeletonPose.BoneTransforms;
var bindPosesRelative = skeleton.BindPosesRelative;
for (int i = 0; i < numberOfBones; i++)
{
//_bonePoseRelative[i] = bindPosesRelative[i] * boneTransforms[i];
SrtTransform.Multiply(ref bindPosesRelative[i], ref boneTransforms[i], out _bonePoseRelative[i]);
}
// ----- Update dirty absolute bone poses.
_bonePoseAbsolute[0] = _bonePoseRelative[0];
for (int i = 1; i < numberOfBones; i++)
{
int parentIndex = skeleton.GetParent(i);
//_bonePoseAbsolute[i] = _bonePoseAbsolute[parentIndex] * _bonePoseRelative[i];
SrtTransform.Multiply(ref _bonePoseAbsolute[parentIndex], ref _bonePoseRelative[i],
out _bonePoseAbsolute[i]);
}
// ----- Update skinning matrices (either the Matrix44F or the XNA variant).
if (_skinningMatrices != null)
{
for (int i = 0; i < numberOfBones; i++)
{
//_skinningMatrices[i] = _bonePoseAbsolute[i] * skeleton.BindPosesAbsoluteInverse[i];
SrtTransform.Multiply(ref _bonePoseAbsolute[i], ref skeleton.BindPosesAbsoluteInverse[i],
out _skinningMatrices[i]);
}
}
#if XNA || MONOGAME
if (_skinningMatricesXna != null)
{
if (_skinningMatrices != null)
{
for (int i = 0; i < numberOfBones; i++)
{
_skinningMatricesXna[i] = (Matrix)_skinningMatrices[i];
}
}
else
{
for (int i = 0; i < numberOfBones; i++)
{
//_skinningMatricesXna[i] = _bonePoseAbsolute[i] * skeleton.BindPosesAbsoluteInverse[i];
SrtTransform.Multiply(ref _bonePoseAbsolute[i], ref skeleton.BindPosesAbsoluteInverse[i],
out _skinningMatricesXna[i]);
}
}
}
#endif
#if !NETFX_CORE && !NET45
Thread.MemoryBarrier();
#else
Interlocked.MemoryBarrier();
#endif
_isBonePoseRelativeDirty.SetAll(false);
_isBonePoseAbsoluteDirty.SetAll(false);
_isSkinningMatrixDirty.SetAll(false);
_isDirty = false;
}
}
}
}
