internal static MultibodySystem Parse(string data)
{
var document = new XmlDocument();
document.LoadXml(data);
var system = new MultibodySystem();
var root = document.DocumentElement;
// Parse the gravity vector.
var gravityNode = root.SelectSingleNode("Gravity");
if (gravityNode == null)
{
system.GravitationVector = CreateVector.Dense <double>(new double[] { 0, 0, -9.81 });
}
else
{
system.GravitationVector = DataParser.ParseVector(gravityNode.FirstChild).Resolve(null);
}
// Parse global parameter.
var globalParameter = ParseParameter(root.SelectSingleNode("Parameters"));
// Parse elements
var elementEntry = new List <(BodyEntry, Element)>();
var elementNode = root.SelectSingleNode("Bodies");
if (elementNode == null)
{
throw new BadDefinitionException($"Error while parsing bodies. \"Bodies\" entry is missing.");
}
if (elementNode.ChildNodes.Count == 0)
{
throw new BadDefinitionException($"Error while parsing bodies. \"Bodies\" entry must have at least one body defined.");
}
foreach (var child in elementNode.ChildNodes.Cast <XmlNode>())
{
var bodyEntry = ParseBody(child);
var element = ElementFactory.Instance.Create(bodyEntry.Type, globalParameter + bodyEntry.LocalParameters);
elementEntry.Add((bodyEntry, element));
}
// Connect the elements and create joints.
var elements = new List <Element>();
foreach (var(entry, element) in elementEntry)
{
// Create the joint.
var joint = JointFactory.Instance.Create(entry.LinkType);
// Find the parent element.
if (entry.RemoteBody == "base") // If the remote body is base, use the systems base frame.
{
joint.BaseFrame = system.BaseFrame;
element.Parent = null;
}
else // Find the correct body.
{
var parentEntry = elementEntry.Where(x => x.Item1.Name == entry.RemoteBody).FirstOrDefault();
element.Parent = parentEntry.Item2;
if (element.Parent == null)
{
throw new BadDefinitionException($"Error while parsing link. Parent \"{entry.RemoteBody}\" was not found.");
}
if (element.Parent == element)
{
throw new BadDefinitionException($"Error while parsing link. Link can not have same local and remote body.");
}
joint.BaseFrame = element.Parent.Frames.Where(x => x.Name == entry.RemoteFrame).FirstOrDefault();
if (joint.BaseFrame == null)
{
throw new BadDefinitionException($"Error while creating joint. Frame\"{entry.RemoteFrame}\" was not found on body of type \"{parentEntry.Item1.Type}\".");
}
}
// Assign the joint properties.
joint.FollowerFrame = element.Frames.Where(x => x.Name == entry.LocalFrame).FirstOrDefault();
if (joint.FollowerFrame == null)
{
throw new BadDefinitionException($"Error while creating joint. Frame\"{entry.LocalFrame}\" was not found on body of type \"{entry.Type}\".");
}
element.BaseJoint = joint;
elements.Add(element);
}
// Sort elements
var resultList = new List <Element>(elements.Count);
int index = 0;
while (elements.Count > 0)
{
if (resultList.Contains(elements[index].Parent) || elements[index].Parent == null)
{
resultList.Add(elements[index]);
elements.Remove(elements[index]);
index = 0;
}
if (index >= elements.Count)
{
index = 0;
}
else
{
index++;
}
}
system.Elements = resultList;
system.InitilizeSystem();
return(system);
}
internal static ElementFactoryEntry Parse(XmlDocument document)
{
var entry = new ElementFactoryEntry();
var root = document.DocumentElement;
// Load the header data
entry.Name = root.SelectSingleNode("Name").InnerText;
entry.Author = root.SelectSingleNode("Author").InnerText;
entry.Description = root.SelectSingleNode("Description")?.InnerText ?? "";
entry.Url = root.SelectSingleNode("URL")?.InnerText ?? "";
// Load the properties.
var propertyNode = root.SelectSingleNode("Properties");
var massChildrenCount = propertyNode.SelectSingleNode("Mass")?.ChildNodes.Count;
if (massChildrenCount == null || massChildrenCount != 1)
{
throw new BadDefinitionException($"Error during parsing mass property. Either the property is missing, has zero or more the one entry.");
}
entry.MassOperation = DataParser.ParseScalar(propertyNode.SelectSingleNode("Mass").FirstChild);
var inertiaChildrenCount = propertyNode.SelectSingleNode("Inertia")?.ChildNodes.Count;
if (inertiaChildrenCount == null || inertiaChildrenCount != 1)
{
throw new BadDefinitionException($"Error during parsing inertia property. Either the property is missing, has zero or more the one entry.");
}
entry.InertiaOperation = DataParser.ParseMatrix(propertyNode.SelectSingleNode("Inertia").FirstChild);
// Parse frames
entry.Frames = new List <ElementFactoryEntryFrame>();
var framesNode = root.SelectSingleNode("Frames");
if (framesNode != null)
{
foreach (var frame in framesNode.ChildNodes.Cast <XmlNode>())
{
var frameEntry = new ElementFactoryEntryFrame();
frameEntry.Name = frame.Attributes["name"].Value?.ToLower();
frameEntry.Reference = frame.Attributes["reference"]?.Value?.ToLower() ?? "origin";
var translationNode = frame.SelectSingleNode("Translation");
if (translationNode != null)
{
frameEntry.TranslationOperation = DataParser.ParseVector(translationNode.FirstChild);
}
else
{
frameEntry.TranslationOperation = new ZeroVectorOperation(3);
}
var rotationNode = frame.SelectSingleNode("Rotation");
if (rotationNode != null)
{
frameEntry.RotationOperation = DataParser.ParseMatrix(rotationNode.FirstChild);
}
else
{
frameEntry.RotationOperation = new IdentityMatrixOperation(3);
}
entry.Frames.Add(frameEntry);
}
}
return(entry);
}
