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