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