public Chain(Transform start, Transform end)
{
List <Transform> segments = new List <Transform>();
List <KinematicJoint> joints = new List <KinematicJoint>();
Length = 0f;
DoF = 0;
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
{
Vector3 reference = Joints[0].ComputeConnectionInWorldSpace();
for (int i = 1; i < Joints.Length; i++)
{
Length += Vector3.Distance(reference, Joints[i].ComputeConnectionInWorldSpace());
reference = Joints[i].ComputeConnectionInWorldSpace();
}
Length += Vector3.Distance(reference, end.position);
}
for (int i = 0; i < Joints.Length; i++)
{
DoF += Joints[i].GetDOF();
}
}
private void AddNode(Transform segment)
{
if (FindNode(segment) == null)
{
KinematicJoint joint = segment.GetComponent <KinematicJoint>();
MotionPtr[] motions = new MotionPtr[3];
Node node = new Node(this, FindNode(segment.parent), segment, joint, motions);
if (joint != null)
{
if (joint.GetDOF() == 0)
{
joint = null;
}
else
{
if (joint.XMotion.State != JointState.Fixed)
{
MotionPtr motionPtr = new MotionPtr(joint.XMotion, node, Motions.Length);
System.Array.Resize(ref Motions, Motions.Length + 1);
Motions[Motions.Length - 1] = motionPtr;
motions[0] = motionPtr;
}
if (joint.YMotion.State != JointState.Fixed)
{
MotionPtr motionPtr = new MotionPtr(joint.YMotion, node, Motions.Length);
System.Array.Resize(ref Motions, Motions.Length + 1);
Motions[Motions.Length - 1] = motionPtr;
motions[1] = motionPtr;
}
if (joint.ZMotion.State != JointState.Fixed)
{
MotionPtr motionPtr = new MotionPtr(joint.ZMotion, node, Motions.Length);
System.Array.Resize(ref Motions, Motions.Length + 1);
Motions[Motions.Length - 1] = motionPtr;
motions[2] = motionPtr;
}
}
}
IKTip tip = segment.GetComponent <IKTip>();
if (tip != null)
{
System.Array.Resize(ref Tips, Tips.Length + 1);
Tips[Tips.Length - 1] = new TipPtr(segment.GetComponent <IKTip>(), node, Tips.Length);
}
System.Array.Resize(ref Nodes, Nodes.Length + 1);
Nodes[Nodes.Length - 1] = node;
}
}
