1 files changed, 262 insertions, 0 deletions

diff --git a/OpenSim/Framework/PhysicsInertia.cs b/OpenSim/Framework/PhysicsInertia.cs
new file mode 100644
index 0000000..af70634
--- /dev/null
+++ b/OpenSim/Framework/PhysicsInertia.cs
@@ -0,0 +1,262 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using OpenMetaverse;
+using System.Text;
+using System.IO;
+using System.Xml;
+
+namespace OpenSim.Framework
+{
+    public class PhysicsInertiaData
+    {
+        public float TotalMass; // the total mass of a linkset
+        public Vector3 CenterOfMass;  // the center of mass position relative to root part position
+        public Vector3 Inertia; //  (Ixx, Iyy, Izz) moment of inertia relative to center of mass and principal axis in local coords
+        public Vector4 InertiaRotation; // if principal axis don't match local axis, the principal axis rotation
+                                        // or the upper triangle of the inertia tensor 
+                                        // Ixy (= Iyx), Ixz (= Izx), Iyz (= Izy))
+
+        public PhysicsInertiaData()
+        {
+        }
+
+        public PhysicsInertiaData(PhysicsInertiaData source)
+        {
+           TotalMass = source.TotalMass;
+           CenterOfMass = source.CenterOfMass;
+           Inertia = source.Inertia;
+           InertiaRotation = source.InertiaRotation;
+        }
+
+        private XmlTextWriter writer;
+
+        private void XWint(string name, int i)
+        {
+            writer.WriteElementString(name, i.ToString());
+        }
+
+        private void XWfloat(string name, float f)
+        {
+            writer.WriteElementString(name, f.ToString(Utils.EnUsCulture));
+        }
+
+        private void XWVector(string name, Vector3 vec)
+        {
+            writer.WriteStartElement(name);
+            writer.WriteElementString("X", vec.X.ToString(Utils.EnUsCulture));
+            writer.WriteElementString("Y", vec.Y.ToString(Utils.EnUsCulture));
+            writer.WriteElementString("Z", vec.Z.ToString(Utils.EnUsCulture));
+            writer.WriteEndElement();
+        }
+
+        private void XWVector4(string name, Vector4 quat)
+        {
+            writer.WriteStartElement(name);
+            writer.WriteElementString("X", quat.X.ToString(Utils.EnUsCulture));
+            writer.WriteElementString("Y", quat.Y.ToString(Utils.EnUsCulture));
+            writer.WriteElementString("Z", quat.Z.ToString(Utils.EnUsCulture));
+            writer.WriteElementString("W", quat.W.ToString(Utils.EnUsCulture));
+            writer.WriteEndElement();
+        }
+
+        public void ToXml2(XmlTextWriter twriter)
+        {
+            writer = twriter;
+            writer.WriteStartElement("PhysicsInertia");
+
+            XWfloat("MASS", TotalMass);
+            XWVector("CM", CenterOfMass);
+            XWVector("INERTIA", Inertia);
+            XWVector4("IROT", InertiaRotation);
+
+            writer.WriteEndElement();
+            writer = null;
+        }
+
+        XmlReader reader;
+
+        private int XRint()
+        {
+            return reader.ReadElementContentAsInt();
+        }
+
+        private float XRfloat()
+        {
+            return reader.ReadElementContentAsFloat();
+        }
+
+        public Vector3 XRvector()
+        {
+            Vector3 vec;
+            reader.ReadStartElement();
+            vec.X = reader.ReadElementContentAsFloat();
+            vec.Y = reader.ReadElementContentAsFloat();
+            vec.Z = reader.ReadElementContentAsFloat();
+            reader.ReadEndElement();
+            return vec;
+        }
+
+        public Vector4 XRVector4()
+        {
+            Vector4 q;
+            reader.ReadStartElement();
+            q.X = reader.ReadElementContentAsFloat();
+            q.Y = reader.ReadElementContentAsFloat();
+            q.Z = reader.ReadElementContentAsFloat();
+            q.W = reader.ReadElementContentAsFloat();
+            reader.ReadEndElement();
+            return q;
+        }
+
+        public static bool EReadProcessors(
+            Dictionary<string, Action> processors,
+            XmlReader xtr)
+        {
+            bool errors = false;
+
+            string nodeName = string.Empty;
+            while (xtr.NodeType != XmlNodeType.EndElement)
+            {
+                nodeName = xtr.Name;
+
+                Action p = null;
+                if (processors.TryGetValue(xtr.Name, out p))
+                {
+                    try
+                    {
+                        p();
+                    }
+                    catch
+                    {
+                        errors = true;
+                        if (xtr.NodeType == XmlNodeType.EndElement)
+                            xtr.Read();
+                    }
+                }
+                else
+                {
+                    xtr.ReadOuterXml(); // ignore
+                }
+            }
+
+            return errors;
+        }
+
+        public string ToXml2()
+        {
+            using (StringWriter sw = new StringWriter())
+            {
+                using (XmlTextWriter xwriter = new XmlTextWriter(sw))
+                {
+                    ToXml2(xwriter);
+                }
+
+                return sw.ToString();
+            }
+        }
+
+        public static PhysicsInertiaData FromXml2(string text)
+        {
+            if (text == String.Empty)
+                return null;
+
+            UTF8Encoding enc = new UTF8Encoding();
+            MemoryStream ms = new MemoryStream(enc.GetBytes(text));
+            XmlTextReader xreader = new XmlTextReader(ms);
+
+            PhysicsInertiaData v = new PhysicsInertiaData();
+            bool error;
+
+            v.FromXml2(xreader, out error);
+
+            xreader.Close();
+
+            if (error)
+                return null;
+
+            return v;
+        }
+
+        public static PhysicsInertiaData FromXml2(XmlReader reader)
+        {
+            PhysicsInertiaData data = new PhysicsInertiaData();
+
+            bool errors = false;
+
+            data.FromXml2(reader, out errors);
+            if (errors)
+                return null;
+
+            return data;
+        }
+
+        private void FromXml2(XmlReader _reader, out bool errors)
+        {
+            errors = false;
+            reader = _reader;
+
+            Dictionary<string, Action> m_XmlProcessors = new Dictionary<string, Action>();
+
+            m_XmlProcessors.Add("MASS", ProcessXR_Mass);
+            m_XmlProcessors.Add("CM", ProcessXR_CM);
+            m_XmlProcessors.Add("INERTIA", ProcessXR_Inertia);
+            m_XmlProcessors.Add("IROT", ProcessXR_InertiaRotation);
+
+            reader.ReadStartElement("PhysicsInertia", String.Empty);
+
+            errors = EReadProcessors(
+                m_XmlProcessors,
+                reader);
+
+            reader.ReadEndElement();
+            reader = null;
+        }
+
+        private void ProcessXR_Mass()
+        {
+            TotalMass = XRfloat();
+        }
+
+        private void ProcessXR_CM()
+        {
+            CenterOfMass = XRvector();
+        }
+
+        private void ProcessXR_Inertia()
+        {
+            Inertia = XRvector();
+        }
+
+        private void ProcessXR_InertiaRotation()
+        {
+            InertiaRotation = XRVector4();
+        }
+    }
+}