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