public Matrix(Quaternion q)
: this()
{
double sqx = q.X*q.X;
double sqy = q.Y*q.Y;
double sqz = q.Z*q.Z;
double sqw = q.W*q.W;
double xy = q.X*q.Y;
double zw = q.Z*q.W;
double xz = q.X*q.Z;
double yw = q.Y*q.W;
double yz = q.Y*q.Z;
double xw = q.X*q.W;
M00 = (sqx - sqy - sqz + sqw);
M01 = 2.0*(xy - zw);
M02 = 2.0*(xz + yw);
M03 = 0d;
M10 = 2.0*(xy + zw);
M11 = (-sqx + sqy - sqz + sqw);
M12 = 2.0*(yz - xw);
M13 = 0d;
M20 = 2.0*(xz - yw);
M21 = 2.0*(yz + xw);
M22 = (-sqx - sqy + sqz + sqw);
M23 = 0d;
M30 = 0d;
M31 = 0d;
M32 = 0d;
M33 = 1d;
}
private static Matrix GetAbsoluteTransform(BoneManager manager, RigResource.RigResource.Bone b)
{
var transforms = new List <Matrix>();
while (b != null)
{
var q = new Quaternion(b.Orientation.A, b.Orientation.B, b.Orientation.C, b.Orientation.D);
var p = new Vector3(b.Position.X, b.Position.Y, b.Position.Z);
var s = new Vector3(b.Scaling.X, b.Scaling.Y, b.Scaling.Z);
Matrix t = Matrix.CreateTransformMatrix(q, s, p);
transforms.Add(t);
b = manager.GetParent(b);
}
transforms.Reverse();
Matrix absolute = transforms[0];
transforms.RemoveAt(0);
while (transforms.Count > 0)
{
Matrix m = transforms[0];
transforms.RemoveAt(0);
absolute = absolute * m;
}
return(absolute);
}
public static Matrix CreateTransformMatrix(Quaternion rotation, Vector3 scale, Vector3 translate)
{
var r = new Matrix(rotation);
Matrix s = CreateScaleMatrix(scale);
Matrix t = CreateTranslateMatrix(translate);
return t*r*s;
}
private static Matrix GetAbsoluteTransform(BoneManager manager, RigResource.RigResource.Bone b)
{
var transforms = new List<Matrix>();
while (b != null)
{
var q = new Quaternion(b.Orientation.A, b.Orientation.B, b.Orientation.C, b.Orientation.D);
var p = new Vector3(b.Position.X, b.Position.Y, b.Position.Z);
var s = new Vector3(b.Scaling.X, b.Scaling.Y, b.Scaling.Z);
Matrix t = Matrix.CreateTransformMatrix(q, s, p);
transforms.Add(t);
b = manager.GetParent(b);
}
transforms.Reverse();
Matrix absolute = transforms[0];
transforms.RemoveAt(0);
while (transforms.Count > 0)
{
Matrix m = transforms[0];
transforms.RemoveAt(0);
absolute = absolute*m;
}
return absolute;
}
private void SetEulers()
{
var q = new Quaternion(new Matrix(mRotation));
mBone.Orientation.A = (float) q.X;
mBone.Orientation.B = (float) q.Y;
mBone.Orientation.C = (float) q.Z;
mBone.Orientation.D = (float) q.W;
}
public Quaternion(Quaternion q)
: this(q.X, q.Y, q.Z, q.W)
{
}