PhysicsInertia.cs 8.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264
  1. /*
  2. * Copyright (c) Contributors, http://opensimulator.org/
  3. * See CONTRIBUTORS.TXT for a full list of copyright holders.
  4. *
  5. * Redistribution and use in source and binary forms, with or without
  6. * modification, are permitted provided that the following conditions are met:
  7. * * Redistributions of source code must retain the above copyright
  8. * notice, this list of conditions and the following disclaimer.
  9. * * Redistributions in binary form must reproduce the above copyright
  10. * notice, this list of conditions and the following disclaimer in the
  11. * documentation and/or other materials provided with the distribution.
  12. * * Neither the name of the OpenSimulator Project nor the
  13. * names of its contributors may be used to endorse or promote products
  14. * derived from this software without specific prior written permission.
  15. *
  16. * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
  17. * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18. * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19. * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
  20. * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21. * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23. * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24. * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25. * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26. */
  27. using System;
  28. using System.Collections.Generic;
  29. using OpenMetaverse;
  30. using System.Text;
  31. using System.IO;
  32. using System.Xml;
  33. namespace OpenSim.Framework
  34. {
  35. public class PhysicsInertiaData
  36. {
  37. public float TotalMass; // the total mass of a linkset
  38. public Vector3 CenterOfMass; // the center of mass position relative to root part position
  39. public Vector3 Inertia; // (Ixx, Iyy, Izz) moment of inertia relative to center of mass and principal axis in local coords
  40. public Vector4 InertiaRotation; // if principal axis don't match local axis, the principal axis rotation
  41. // or the upper triangle of the inertia tensor
  42. // Ixy (= Iyx), Ixz (= Izx), Iyz (= Izy))
  43. public PhysicsInertiaData()
  44. {
  45. }
  46. public PhysicsInertiaData(PhysicsInertiaData source)
  47. {
  48. TotalMass = source.TotalMass;
  49. CenterOfMass = source.CenterOfMass;
  50. Inertia = source.Inertia;
  51. InertiaRotation = source.InertiaRotation;
  52. }
  53. private XmlTextWriter writer;
  54. private void XWint(string name, int i)
  55. {
  56. writer.WriteElementString(name, i.ToString());
  57. }
  58. private void XWfloat(string name, float f)
  59. {
  60. writer.WriteElementString(name, f.ToString(Culture.FormatProvider));
  61. }
  62. private void XWVector(string name, Vector3 vec)
  63. {
  64. writer.WriteStartElement(name);
  65. writer.WriteElementString("X", vec.X.ToString(Culture.FormatProvider));
  66. writer.WriteElementString("Y", vec.Y.ToString(Culture.FormatProvider));
  67. writer.WriteElementString("Z", vec.Z.ToString(Culture.FormatProvider));
  68. writer.WriteEndElement();
  69. }
  70. private void XWVector4(string name, Vector4 quat)
  71. {
  72. writer.WriteStartElement(name);
  73. writer.WriteElementString("X", quat.X.ToString(Culture.FormatProvider));
  74. writer.WriteElementString("Y", quat.Y.ToString(Culture.FormatProvider));
  75. writer.WriteElementString("Z", quat.Z.ToString(Culture.FormatProvider));
  76. writer.WriteElementString("W", quat.W.ToString(Culture.FormatProvider));
  77. writer.WriteEndElement();
  78. }
  79. public void ToXml2(XmlTextWriter twriter)
  80. {
  81. writer = twriter;
  82. writer.WriteStartElement("PhysicsInertia");
  83. XWfloat("MASS", TotalMass);
  84. XWVector("CM", CenterOfMass);
  85. XWVector("INERTIA", Inertia);
  86. XWVector4("IROT", InertiaRotation);
  87. writer.WriteEndElement();
  88. writer = null;
  89. }
  90. XmlReader reader;
  91. private int XRint()
  92. {
  93. return reader.ReadElementContentAsInt();
  94. }
  95. private float XRfloat()
  96. {
  97. return reader.ReadElementContentAsFloat();
  98. }
  99. public Vector3 XRvector()
  100. {
  101. Vector3 vec;
  102. reader.ReadStartElement();
  103. vec.X = reader.ReadElementContentAsFloat();
  104. vec.Y = reader.ReadElementContentAsFloat();
  105. vec.Z = reader.ReadElementContentAsFloat();
  106. reader.ReadEndElement();
  107. return vec;
  108. }
  109. public Vector4 XRVector4()
  110. {
  111. Vector4 q;
  112. reader.ReadStartElement();
  113. q.X = reader.ReadElementContentAsFloat();
  114. q.Y = reader.ReadElementContentAsFloat();
  115. q.Z = reader.ReadElementContentAsFloat();
  116. q.W = reader.ReadElementContentAsFloat();
  117. reader.ReadEndElement();
  118. return q;
  119. }
  120. public static bool EReadProcessors(
  121. Dictionary<string, Action> processors,
  122. XmlReader xtr)
  123. {
  124. bool errors = false;
  125. string nodeName = string.Empty;
  126. while (xtr.NodeType != XmlNodeType.EndElement)
  127. {
  128. nodeName = xtr.Name;
  129. Action p = null;
  130. if (processors.TryGetValue(xtr.Name, out p))
  131. {
  132. try
  133. {
  134. p();
  135. }
  136. catch
  137. {
  138. errors = true;
  139. if (xtr.NodeType == XmlNodeType.EndElement)
  140. xtr.Read();
  141. }
  142. }
  143. else
  144. {
  145. xtr.ReadOuterXml(); // ignore
  146. }
  147. }
  148. return errors;
  149. }
  150. public string ToXml2()
  151. {
  152. using (StringWriter sw = new StringWriter())
  153. {
  154. using (XmlTextWriter xwriter = new XmlTextWriter(sw))
  155. {
  156. ToXml2(xwriter);
  157. }
  158. return sw.ToString();
  159. }
  160. }
  161. public static PhysicsInertiaData FromXml2(string text)
  162. {
  163. if (text == String.Empty)
  164. return null;
  165. bool error;
  166. PhysicsInertiaData v;
  167. UTF8Encoding enc = new UTF8Encoding();
  168. using(MemoryStream ms = new MemoryStream(enc.GetBytes(text)))
  169. using(XmlTextReader xreader = new XmlTextReader(ms))
  170. {
  171. xreader.DtdProcessing = DtdProcessing.Prohibit;
  172. xreader.XmlResolver = null;
  173. v = new PhysicsInertiaData();
  174. v.FromXml2(xreader, out error);
  175. }
  176. if (error)
  177. return null;
  178. return v;
  179. }
  180. public static PhysicsInertiaData FromXml2(XmlReader reader)
  181. {
  182. PhysicsInertiaData data = new PhysicsInertiaData();
  183. bool errors = false;
  184. data.FromXml2(reader, out errors);
  185. if (errors)
  186. return null;
  187. return data;
  188. }
  189. private void FromXml2(XmlReader _reader, out bool errors)
  190. {
  191. errors = false;
  192. reader = _reader;
  193. Dictionary<string, Action> m_XmlProcessors = new Dictionary<string, Action>();
  194. m_XmlProcessors.Add("MASS", ProcessXR_Mass);
  195. m_XmlProcessors.Add("CM", ProcessXR_CM);
  196. m_XmlProcessors.Add("INERTIA", ProcessXR_Inertia);
  197. m_XmlProcessors.Add("IROT", ProcessXR_InertiaRotation);
  198. reader.ReadStartElement("PhysicsInertia", String.Empty);
  199. errors = EReadProcessors(
  200. m_XmlProcessors,
  201. reader);
  202. reader.ReadEndElement();
  203. reader = null;
  204. }
  205. private void ProcessXR_Mass()
  206. {
  207. TotalMass = XRfloat();
  208. }
  209. private void ProcessXR_CM()
  210. {
  211. CenterOfMass = XRvector();
  212. }
  213. private void ProcessXR_Inertia()
  214. {
  215. Inertia = XRvector();
  216. }
  217. private void ProcessXR_InertiaRotation()
  218. {
  219. InertiaRotation = XRVector4();
  220. }
  221. }
  222. }