static void Main(string[] args) { Console.WriteLine("Hello World!"); ElementFactory.Instance.LoadFolderDefinition(@".\Definitions\Elements"); JointFactory.Instance.LoadFolderDefinition(@".\Definitions\Joints"); MultibodySystem MBS = MultibodySystem.LoadFromFile(@".\Definitions\Systems\SimplePendulum.xml"); double[] vec = new double[MBS.Elements.Count * 6 * 2]; StateVector SV = new StateVector(vec, MBS.GenerateMappings()); MBS.GetGlobalKeepMatrix(); MBS.SetupElements(); MBS.UpdateElements(SV); Matrix <double> GlobalMassMatrix = CreateMatrix.Dense <double>(MBS.Elements.Count * 6, MBS.Elements.Count * 6); GlobalMassMatrix = MBS.GetGlobalMassMatrix(GlobalMassMatrix); Matrix <double> GlobalJacobian = CreateMatrix.Dense <double>(MBS.Elements.Count * 6, MBS.Elements.Count * 6); Vector <double> GlobalAngularVelocity = CreateVector.Dense <double>(6); GlobalJacobian = MBS.GetGlobalJacobian(GlobalJacobian); }
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); }