/// <summary>
/// Grabs the joints and groups from the Inventor results,
/// </summary>
/// <param name="results"></param>
public CustomRigidResults(RigidBodyResults results)
{
Console.WriteLine("Building custom dataset");
groups = new List <CustomRigidGroup>(results.RigidBodyGroups.Count);
joints = new List <CustomRigidJoint>(results.RigidBodyJoints.Count);
foreach (RigidBodyGroup group in results.RigidBodyGroups)
{
CustomRigidGroup tmp = new CustomRigidGroup(group);
if ((!(groupIDToCustom.ContainsKey(tmp.fullQualifier))))
{
groups.Add(tmp);
groupIDToCustom.Add(tmp.fullQualifier, tmp);
}
else
{
Console.WriteLine("GroupID Collision: " + groupIDToCustom[CustomRigidGroup.GetGroupQualifier(group)].ToString() + " and " + tmp.ToString());
}
Console.Write("Group " + groups.Count + "/" + results.RigidBodyGroups.Count + "\tJoint " + joints.Count + "/" + results.RigidBodyJoints.Count);
}
foreach (RigidBodyJoint joint in results.RigidBodyJoints)
{
joints.Add(new CustomRigidJoint(joint, groupIDToCustom[CustomRigidGroup.GetGroupQualifier(joint.GroupOne)], groupIDToCustom[CustomRigidGroup.GetGroupQualifier(joint.GroupTwo)]));
Console.Write("Group " + groups.Count + "/" + results.RigidBodyGroups.Count + "\tJoint " + joints.Count + "/" + results.RigidBodyJoints.Count);
}
Console.WriteLine();
Console.WriteLine("Built custom dataset");
RigidBodyCleaner.CleanMeaningless(this);
}
/// <summary>
/// Merges all the grounded bodies within the given rigid results into a single group.
/// </summary>
/// <remarks>
/// This also updates the references of all the rigid joints to the new groups.
/// </remarks>
/// <exception cref="InvalidOperationException">No grounded bodies exist to merge.</exception>
/// <param name="results">Rigid results to clean</param>
public static void CleanGroundedBodies(CustomRigidResults results)
{
CustomRigidGroup firstRoot = null;
//Bundles all the grounded components together into one CustomRigidGroup.
foreach (CustomRigidGroup group in results.groups)
{
if (group.grounded)
{
if (firstRoot == null)
{
firstRoot = group;
}
else
{
firstRoot.occurrences.AddRange(group.occurrences);
group.occurrences.Clear();
}
}
}
if (firstRoot != null)
{
foreach (CustomRigidJoint joint in results.joints)
{
//If a group has no occurrences, its because they were transferred to the firstRoot group. Updates joints to attach to that joint.
if (joint.groupOne.occurrences.Count == 0)
{
joint.groupOne = firstRoot;
}
if (joint.groupTwo.occurrences.Count == 0)
{
joint.groupTwo = firstRoot;
}
}
}
else
{
throw new SkeletonBuilder.NoGroundException();
}
CleanMeaningless(results);
}
public CustomRigidJoint(RigidBodyJoint joint, CustomRigidGroup groupOnez, CustomRigidGroup groupTwoz)
{
foreach (AssemblyJoint aj in joint.Joints)
{
joints.Add(aj);
}
foreach (AssemblyConstraint cj in joint.Constraints)
{
constraints.Add(cj);
}
groupOne = groupOnez;
groupTwo = groupTwoz;
type = joint.JointType;
joint.GetJointData(out geomOne, out geomTwo, out options);
try
{
jointBased = options.get_Value("FromJoint");
}
catch
{
jointBased = false;
}
}
/// <summary>
/// Merges any groups that are connected only with constraints and generate a node tree.
/// </summary>
/// <remarks>
/// This starts at whichever rigid group is grounded, then branches out along rigid joints from there.
/// If the rigid joint is movable (made of assembly joint(s)) then another node is created, if the joint
/// is constraint-only then the leaf node is merged into the current branch.
/// </remarks>
/// <param name="results">Rigid results to clean</param>
public static RigidNode BuildAndCleanDijkstra(CustomRigidResults results)
{
Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> > constraints = new Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> >();
Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> > joints = new Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> >();
GenerateJointMaps(results, joints, constraints);
// Mapping rigid group to merge-into group
Dictionary <CustomRigidGroup, CustomRigidGroup> mergePattern = new Dictionary <CustomRigidGroup, CustomRigidGroup>();
// Mapping rigid group to skeletal node
Dictionary <CustomRigidGroup, RigidNode> baseNodes = new Dictionary <CustomRigidGroup, RigidNode>();
// Deffered joint creation. Required so merge can take place.
List <PlannedJoint> plannedJoints = new List <PlannedJoint>();
// The base of the skeletal tree
RigidNode baseRoot = null;
// All the currently open groups as an array {currentGroup, mergeIntoGroup}
List <CustomRigidGroup[]> openNodes = new List <CustomRigidGroup[]>();
// All the groups that have been processed. (Closed nodes)
HashSet <CustomRigidGroup> closedNodes = new HashSet <CustomRigidGroup>();
// Find the first grounded group, the start point for dijkstra's algorithm.
foreach (CustomRigidGroup grp in results.groups)
{
if (grp.grounded)
{
openNodes.Add(new CustomRigidGroup[] { grp, grp });
closedNodes.Add(grp);
baseNodes.Add(grp, baseRoot = new RigidNode(Guid.NewGuid(), grp));
break; //Should only contain one grounded group, as they have all been merged together.
}
}
Console.WriteLine("Determining merge commands");
while (openNodes.Count > 0)
{
List <CustomRigidGroup[]> newOpen = new List <CustomRigidGroup[]>();
foreach (CustomRigidGroup[] node in openNodes)
{
// Get all connections
HashSet <CustomRigidGroup> cons = constraints[node[0]];
HashSet <CustomRigidGroup> jons = joints[node[0]];
foreach (CustomRigidGroup jonConn in jons)
{
if (!closedNodes.Add(jonConn)) //Moves on to next if the connected component is already in closedNodes.
{
continue;
}
RigidNode rnode = new RigidNode(Guid.NewGuid(), jonConn); //Makes a new rigid node for the connected component.
baseNodes.Add(jonConn, rnode);
//Find the actual joint between the two components.
foreach (CustomRigidJoint jnt in results.joints)
{
if (jnt.joints.Count > 0 && ((jnt.groupOne.Equals(jonConn) && jnt.groupTwo.Equals(node[0])) ||
(jnt.groupOne.Equals(node[0]) && jnt.groupTwo.Equals(jonConn))))
{
PlannedJoint pJoint = new PlannedJoint();
pJoint.joint = jnt;
pJoint.parentNode = baseNodes[node[1]];
pJoint.node = rnode;
plannedJoints.Add(pJoint);
newOpen.Add(new CustomRigidGroup[] { jonConn, jonConn });
break;
}
}
}
foreach (CustomRigidGroup consConn in cons)
{
if (!closedNodes.Add(consConn))
{
continue;
}
mergePattern.Add(consConn, node[1]);
newOpen.Add(new CustomRigidGroup[] { consConn, node[1] }); //Uses node[1] to ensure all constrained groups are merged into the same group.
}
}
openNodes = newOpen;
}
Console.WriteLine("Do " + mergePattern.Count + " merge commands");
//Transfers components between constrained groups.
foreach (KeyValuePair <CustomRigidGroup, CustomRigidGroup> pair in mergePattern)
{
pair.Value.occurrences.AddRange(pair.Key.occurrences); //Transfers key components to related value.
pair.Key.occurrences.Clear();
pair.Value.grounded = pair.Value.grounded || pair.Key.grounded; //Is it possible for the key to be grounded? Would there have to be a loop of groups?
}
Console.WriteLine("Resolve broken joints");
//Goes through each joint and sees if it was merged. If it was, it attaches the group left behind to the group that was merged into.
foreach (CustomRigidJoint joint in results.joints)
{
CustomRigidGroup updatedGroup = null; //Stores the group that the previous groupOne/Two was merged into.
if (mergePattern.TryGetValue(joint.groupOne, out updatedGroup))
{
joint.groupOne = updatedGroup;
}
if (mergePattern.TryGetValue(joint.groupTwo, out updatedGroup))
{
joint.groupTwo = updatedGroup;
}
}
Console.WriteLine("Creating planned skeletal joints");
foreach (PlannedJoint pJoint in plannedJoints)
{
SkeletalJoint_Base sJ = SkeletalJoint.Create(pJoint.joint, pJoint.parentNode.group);
pJoint.parentNode.AddChild(sJ, pJoint.node);
}
Console.WriteLine("Cleanup remainders");
CleanMeaningless(results);
return(baseRoot);
}
public RigidNode(Guid guid, CustomRigidGroup grp)
: base(guid)
{
this.group = grp;
}
public RigidNode(Guid guid)
: base(guid)
{
group = null;
}
