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);
}
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())))))))));
}