//Adds a segment node into the OFKT data structure
private void AddNode(Transform segment)
{
if (FindNode(segment) == null)
{
KinematicJoint joint = segment.GetComponent <KinematicJoint>();
Objective objective = segment.GetComponent <Objective>();
Node node = new Node(this, FindNode(segment.parent), segment, joint, objective);
if (joint != null)
{
if (joint.GetDoF() == 0)
{
joint = null;
}
else
{
if (joint.GetXMotion().IsEnabled())
{
MotionPtr motionPtr = new MotionPtr(joint.GetXMotion(), node, MotionPtrs.Length);
System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
node.XEnabled = true;
node.XIndex = motionPtr.Index;
}
if (joint.GetYMotion().IsEnabled())
{
MotionPtr motionPtr = new MotionPtr(joint.GetYMotion(), node, MotionPtrs.Length);
System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
node.YEnabled = true;
node.YIndex = motionPtr.Index;
}
if (joint.GetZMotion().IsEnabled())
{
MotionPtr motionPtr = new MotionPtr(joint.GetZMotion(), node, MotionPtrs.Length);
System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
node.ZEnabled = true;
node.ZIndex = motionPtr.Index;
}
}
}
if (objective != null)
{
System.Array.Resize(ref ObjectivePtrs, ObjectivePtrs.Length + 1);
ObjectivePtrs[ObjectivePtrs.Length - 1] = new ObjectivePtr(segment.GetComponent <Objective>(), node);
}
System.Array.Resize(ref Nodes, Nodes.Length + 1);
Nodes[Nodes.Length - 1] = node;
}
}
public Chain(Transform start, Transform end)
{
List <Transform> segments = new List <Transform>();
List <KinematicJoint> joints = new List <KinematicJoint>();
Length = 0f;
Transform t = end;
while (true)
{
segments.Add(t);
KinematicJoint joint = t.GetComponent <KinematicJoint>();
if (joint != null)
{
if (joint.GetDoF() != 0)
{
joints.Add(joint);
}
}
if (t == start)
{
break;
}
else
{
t = t.parent;
}
}
segments.Reverse();
joints.Reverse();
Segments = segments.ToArray();
Joints = joints.ToArray();
if (Joints.Length == 0)
{
Length = 0f;
}
else
{
for (int i = 1; i < Joints.Length; i++)
{
Length += Vector3.Distance(Joints[i - 1].GetAnchorInWorldSpace(), Joints[i].GetAnchorInWorldSpace());
}
Length += Vector3.Distance(Joints[Joints.Length - 1].GetAnchorInWorldSpace(), end.position);
}
}
