static void TestMath()
{
var rand = new Random();
for (int i = 0; i < 1000; i++)
{
var u = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5).Normalized();
var t1 = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5) * 10;
var t2 = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5) * 10;
var p1 = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5) * 10;
var q = new Quaternion(rand.NextDouble() * Math.PI, u);
var m = q.ToMatrix();
var mt = TMatrix.Translation(t1) * m * TMatrix.Translation(t2);
var q2 = mt.GetRotation();
var m2 = q2.ToMatrix();
if (((m * p1) - (m2 * p1)) * (m * p1 - m2 * p1) > .0001)
{
Console.WriteLine("math error.");
}
}
var badq = new Quaternion(3.1414934204542249,
new Vector(-0.920567453, -0.390583634, -0));
badq = badq.ToMatrix().GetRotation();
Console.WriteLine(badq.Angle);
Console.WriteLine(badq.Axis);
}
public TMatrix Translated(Vector v)
{
var x = new TMatrix((double[])this.m.Clone());
x.m[12] += v.x;
x.m[13] += v.y;
x.m[14] += v.z;
return(x);
}
public static TMatrix operator *(TMatrix left, TMatrix right)
{
TMatrix res = new TMatrix();
res.m[15] = 0;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
for (int r = 0; r < 4; r++)
{
res.m[i * 4 + j] += left.m[r * 4 + j] * right.m[i * 4 + r];
}
}
}
return(res);
}
public TMatrix Inverse()
{
// Since this matrix has the form (T * R), the inverse is
// R^-1 * T^-1 = (R^T) * -T = T' * (R^T), where
// T' = [ I R^T * (-t) ]
TMatrix res = new TMatrix();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
res.m[i * 4 + j] = m[i + j * 4];
}
}
Vector t = res * new Vector(this.m[12], this.m[13], this.m[14]);
for (int i = 0; i < 3; i++)
{
res.m[12 + i] = -t[i];
}
return(res);
}
static void ReadAnim(RW4Model model, string outname, string ogreSkeletonFile)
{
var anims = (from anim in model.GetObjects(Anim.type_code) select(Anim) anim).ToArray();
var skeletons = (from s in model.GetObjects(RW4Skeleton.type_code) select(RW4Skeleton) s).ToArray();
if (anims.Length == 0 && skeletons.Length == 0)
{
return;
}
var only = new int[] { 1 };
var x_skels =
new XElement("skeletons",
from skel in skeletons
select new XElement("skeleton",
new XAttribute("name", SporeMaster.NameRegistry.Files.toName(skel.jointInfo.id)),
new XAttribute("unk1", "0x" + skel.unk1.ToString("X")),
new XElement("joints",
from joint in skel.jointInfo.items
let mat4 = skel.mat4.items[joint.index]
let inv_bind = TMatrix.FromMat4(mat4)
let bind = inv_bind.Inverse()
let rotation = bind.Inverse().GetRotation() //< TODO: Why inverse?
select new XElement("joint",
new XAttribute("name", SporeMaster.NameRegistry.Files.toName(joint.name_fnv)),
new XAttribute("flags", "0x" + joint.flags.ToString("X")),
from _ in only where joint.parent != null
select new XAttribute("parent", SporeMaster.NameRegistry.Files.toName(joint.parent.name_fnv)),
new XElement("Transform",
new XElement("position", bind.GetColumn(3).toXML()),
new XElement("rotation", new XAttribute("angle", rotation.Angle), rotation.Axis.toXML())
),
new XElement("RawInverseBindMatrix",
new XElement("rx", writeVector(mat4.m[0], mat4.m[1], mat4.m[2])),
new XElement("ry", writeVector(mat4.m[4], mat4.m[5], mat4.m[6])),
new XElement("rz", writeVector(mat4.m[8], mat4.m[9], mat4.m[10])),
new XElement("t", writeVector(mat4.m[12], mat4.m[13], mat4.m[15]))
)
)
)
)
);
var x_anims =
new XElement("anims",
from anim in anims
select new XElement("anim",
new XAttribute("name", SporeMaster.NameRegistry.Files.toName(anim.hash_name)),
new XAttribute("flags", "0x" + anim.flags.ToString("X")),
new XAttribute("length", anim.length),
new XAttribute("skeleton", SporeMaster.NameRegistry.Files.toName(anim.skeleton_id)),
//new XAttribute("padding", anim.padding),
new XAttribute("components", "0x" + anim.pose_components.ToString("X")),
new XElement("channels",
from i in Enumerable.Range(0, anim.channel_names.Length)
let name = anim.channel_names[i]
select new XElement("channel",
new XAttribute("name", SporeMaster.NameRegistry.Files.toName(name)),
new XAttribute("padding", anim.padding[i]),
new XElement("frames",
from pose in anim.channel_frame_pose[i]
select new XElement("frame",
new XAttribute("time", pose.time),
new XElement("Translation",
new XAttribute("x", pose.tx),
new XAttribute("y", pose.ty),
new XAttribute("z", pose.tz)),
new XElement("Rotation",
new XAttribute("x", pose.qx),
new XAttribute("y", pose.qy),
new XAttribute("z", pose.qz),
new XAttribute("s", pose.qs)),
new XElement("Scale",
new XAttribute("x", pose.sx),
new XAttribute("y", pose.sy),
new XAttribute("z", pose.sz))
)
)
)
)
)
);
var output = new XElement("raw_animation_info",
x_anims,
x_skels);
output.Save(outname);
WriteOgreSkeleton(model).Save(ogreSkeletonFile);
foreach (var s in skeletons)
{
foreach (var mat4 in s.mat4.items)
{
if (mat4.m[3] != 0 || mat4.m[7] != 0 || mat4.m[15] != 0)
{
throw new Exception("Extra matrix cells nonzero.");
}
}
}
}
static XElement WriteOgreSkeleton(RW4Model model)
{
var anims = (from anim in model.GetObjects(Anim.type_code) select(Anim) anim).ToArray();
var rskels = model.GetObjects(RW4Skeleton.type_code);
if (rskels.Count != 1)
{
throw new NotSupportedException("Exactly one skeleton required.");
}
var skel = (RW4Skeleton)rskels[0];
Dictionary <uint, int> bone_name_lookup = new Dictionary <uint, int>();
foreach (var j in skel.jointInfo.items)
{
bone_name_lookup[j.name_fnv] = j.index;
}
var inv_binds = (from m in skel.mat4.items select TMatrix.FromMat4(m)).ToArray();
var rel_bind = (
from joint in skel.jointInfo.items
let inv_bind = TMatrix.FromMat4(skel.mat4.items[joint.index])
let bind = inv_bind.Inverse()
let parent_inv_bind = joint.parent == null ? null : TMatrix.FromMat4(skel.mat4.items[joint.parent.index])
select parent_inv_bind == null ? bind : parent_inv_bind * bind
).ToArray();
return(new XElement("skeleton",
new XElement("bones",
from joint in skel.jointInfo.items
let rel = rel_bind[joint.index]
let rotation = -rel.GetRotation() // < TODO: Why inverse?
select new XElement("bone",
new XAttribute("id", joint.index),
new XAttribute("name", SporeMaster.NameRegistry.Files.toName(joint.name_fnv)),
new XElement("position", rel.GetColumn(3).toXML()),
new XElement("rotation",
new XAttribute("angle", rotation.Angle),
new XElement("axis", rotation.Axis.toXML()))
)
),
new XElement("bonehierarchy",
from joint in skel.jointInfo.items
where joint.parent != null
select new XElement("boneparent",
new XAttribute("bone", SporeMaster.NameRegistry.Files.toName(joint.name_fnv)),
new XAttribute("parent", SporeMaster.NameRegistry.Files.toName(joint.parent.name_fnv)))),
new XElement("animations",
from anim in anims
select new XElement("animation",
new XAttribute("name", SporeMaster.NameRegistry.Files.toName(anim.hash_name)),
new XAttribute("length", anim.length),
new XElement("tracks",
from i in Enumerable.Range(0, anim.channel_names.Length)
let name = anim.channel_names[i]
let poses = anim.channel_frame_pose[i]
let rel = rel_bind[bone_name_lookup[name]]
let bind_rot = rel.Inverse()
let bind_pos = rel.GetColumn(3)
select new XElement("track",
new XAttribute("bone", SporeMaster.NameRegistry.Files.toName(name)),
new XElement("keyframes",
from f in poses
let scale = new Vector(f.sx, f.sy, f.sz)
let pose = TMatrix.Translation(new Vector(f.tx, f.ty, f.tz)) *
(-new Quaternion {
x = f.qx, y = f.qy, z = f.qz, w = f.qs
}).ToMatrix()
let pose_world = bind_rot * pose
let rot = -pose_world.GetRotation()
let pos = pose.GetColumn(3) - bind_pos
select new XElement("keyframe",
new XAttribute("time", f.time),
new XElement("translate", pos.toXML()),
new XElement("rotate",
new XAttribute("angle", rot.Angle),
new XElement("axis", rot.Axis.toXML())),
new XElement("scale", scale.toXML())))))))));
}
public TMatrix Translated(Vector v)
{
var x = new TMatrix( (double[])this.m.Clone() );
x.m[12] += v.x;
x.m[13] += v.y;
x.m[14] += v.z;
return x;
}
public TMatrix Inverse()
{
// Since this matrix has the form (T * R), the inverse is
// R^-1 * T^-1 = (R^T) * -T = T' * (R^T), where
// T' = [ I R^T * (-t) ]
TMatrix res = new TMatrix();
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
res.m[i * 4 + j] = m[i + j * 4];
Vector t = res * new Vector(this.m[12], this.m[13], this.m[14]);
for (int i = 0; i < 3; i++)
res.m[12 + i] = -t[i];
return res;
}
public static TMatrix operator *(TMatrix left, TMatrix right)
{
TMatrix res = new TMatrix();
res.m[15] = 0;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
for (int r = 0; r < 4; r++)
res.m[i * 4 + j] += left.m[r * 4 + j] * right.m[i * 4 + r];
return res;
}