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