robotArm

Assets/HybridIK/Scripts.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: a2a7b4351f0a477479c2cf11d971bb3e
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/HybridIK/Scripts/Joint Limits.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: fe1091a74ea67954db55dd73a0be5dab
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimit.cs

@@ -0,0 +1,223 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace AshqarApps.DynamicJoint
+{
+    public class DynamicJointLimit : MonoBehaviour
+    {
+        public bool enable = true;
+
+        [Header("Main")]
+        public Vector3 mainAxis = Vector3.forward;
+
+        [HideInInspector]
+        public Vector3 secondaryAxis = Vector3.up;
+        [HideInInspector]
+        public Vector3 crossAxis { get { return Vector3.Cross(mainAxis, secondaryAxis); } }
+
+        [HideInInspector] public Quaternion zeroRotation;
+        public bool zeroRotationOverride { get; private set; }
+
+        public Vector3 limitsOffset;
+
+        private bool initialized;
+        private bool applicationQuit;
+        private bool zeroRotationSet;
+
+        protected bool isClampedToLimit = false;
+
+        /// <summary>
+        /// The swing limit.
+        /// </summary>
+        [Range(0f, 180f)] public float limitAngle = 45;
+
+        public bool applyEveryFrame = false;
+
+        //[Header("Dynamics")]
+
+        [HideInInspector]
+        public float bounce = 0;
+        [HideInInspector]
+        public float friction = 0;
+
+        /*
+             * Applies uniform twist limit to the rotation
+             * */
+        protected Quaternion LimitTwist(Quaternion rotation, Vector3 axis, Vector3 orthoAxis, float twistLimit)
+        {
+            twistLimit = Mathf.Clamp(twistLimit, 0, 180);
+            if (twistLimit >= 180) return rotation;
+
+            Vector3 normal = rotation * axis;
+            Vector3 orthoTangent = orthoAxis;
+            Vector3.OrthoNormalize(ref normal, ref orthoTangent);
+
+            Vector3 rotatedOrthoTangent = rotation * orthoAxis;
+            Vector3.OrthoNormalize(ref normal, ref rotatedOrthoTangent);
+
+            Quaternion fixedRotation = Quaternion.FromToRotation(rotatedOrthoTangent, orthoTangent) * rotation;
+
+            if (twistLimit <= 0) return fixedRotation;
+
+            float twistAngle = Quaternion.Angle(fixedRotation, rotation);
+            if (twistAngle >= twistLimit)
+            {
+                isClampedToLimit = true;
+            }
+
+            // Rotate from zero twist to free twist by the limited angle
+            return Quaternion.RotateTowards(fixedRotation, rotation, twistLimit);
+        }
+
+        public virtual Vector3 GetStretchAxis()
+        {
+            return Vector3.zero;
+        }
+
+        public virtual Vector3 GetMainAxisWorld()
+        {
+            return Direction(mainAxis);
+        }
+
+        public virtual Quaternion LimitRotation(Quaternion rotation, float jointLimitStrength = 1)
+        {
+            return rotation;
+        }
+
+        public virtual Vector3 GetMidVectorWorld()
+        {
+            return mainAxis;
+        }
+
+        protected Vector3 Direction(Vector3 v)
+        {
+            if (transform.parent == null) return zeroRotation * v;
+            return transform.parent.rotation * (zeroRotation * v);
+        }
+
+        /// <summary>
+        /// Returns the limited local rotation.
+        /// </summary>
+        public Quaternion GetLimitedLocalRotation(Quaternion localRotation, out bool changed, float jointLimitStrength = 1)
+        {
+            // Making sure the Rotation Limit is initiated
+            if (!initialized) Awake();
+
+            // Subtracting defaultLocalRotation
+            Quaternion rotation = Quaternion.Inverse(zeroRotation) * localRotation;
+
+            Quaternion limitedRotation = LimitRotation(rotation);
+#if UNITY_2018_3_OR_NEWER
+            limitedRotation = Quaternion.Normalize(limitedRotation);
+#endif
+            changed = limitedRotation != rotation;
+
+            if (!changed) return localRotation;
+
+            // Apply defaultLocalRotation back on
+            return zeroRotation * limitedRotation;
+        }
+
+        public float EvaluateSwingLimitAtOrthoAngle(float angle)
+        {
+            float sinAngle = Mathf.Abs(Mathf.Sin(Mathf.Deg2Rad * angle));
+            //return sinAngle;
+            return (sinAngle * limitAngle);
+        }
+
+        /// <summary>
+        /// Apply the rotation limit to transform.localRotation. Returns true if the limit has changed the rotation.
+        /// </summary>
+        public virtual bool Apply(float jointLimitStrength = 1)
+        {
+            isClampedToLimit = false;
+
+            if (jointLimitStrength == 0 || !enable)
+                return false;
+
+            bool changed = false;
+
+            Quaternion targetRot = GetLimitedLocalRotation(transform.localRotation, out changed);
+
+            if (changed)
+            {
+                if (jointLimitStrength < 1)
+                {
+                    float angle = Quaternion.Angle(transform.localRotation, targetRot);
+
+                    Quaternion easedRotation = Quaternion.RotateTowards(transform.localRotation, targetRot, angle * jointLimitStrength);
+
+                    transform.localRotation = easedRotation;
+                }
+                else
+                {
+                    transform.localRotation = targetRot;
+                }
+            }
+
+            return changed;
+        }
+
+        /// <summary>
+        /// Disable this instance making sure it is initiated. Use this if you intend to manually control the updating of this Rotation Limit.
+        /// </summary>
+        public void Disable()
+        {
+            if (initialized)
+            {
+                enabled = false;
+                return;
+            }
+
+            Awake();
+            enabled = false;
+        }
+
+        public void SetZeroRotation()
+        {
+            zeroRotation = transform.localRotation;
+            zeroRotationSet = true;
+            zeroRotationOverride = false;
+        }
+
+        /// <summary>
+        /// Map the zero rotation point to the specified rotation.
+        /// </summary>
+        public void SetZeroRotation(Quaternion localRotation)
+        {
+            zeroRotation = localRotation;
+            zeroRotationSet = true;
+            zeroRotationOverride = true;
+        }
+
+        /*
+     * Returns the angle between two vectors on a plane with the specified normal
+     * */
+        public static float GetOrthogonalAngle(Vector3 v1, Vector3 v2, Vector3 normal)
+        {
+            Vector3.OrthoNormalize(ref normal, ref v1);
+            Vector3.OrthoNormalize(ref normal, ref v2);
+            return Vector3.Angle(v1, v2);
+        }
+
+        void LateUpdate()
+        {
+            if (applyEveryFrame)
+                Apply();
+        }
+
+        void Awake()
+        {
+            // Store the local rotation to map the zero rotation point to the current rotation
+            if (!zeroRotationSet) SetZeroRotation();
+
+            if (mainAxis == Vector3.zero)
+            {
+                Debug.LogError("Axis is Vector3.zero.");
+                mainAxis = Vector3.forward;
+            }
+            initialized = true;
+        }
+    }
+}

Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimit.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: f4d18e67185065d4e871e8f6eb899138
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimit.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimitHinge.cs

@@ -0,0 +1,100 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace AshqarApps.DynamicJoint
+{
+    public class DynamicJointLimitHinge : DynamicJointLimit
+    {
+        [Header("Hinge Limits")]
+        [Range(0f, 360f)]
+        public float hingeAngleOffset = 0;
+
+        public Vector3 cross = Vector3.up;
+
+        private Quaternion lastRotation = Quaternion.identity;
+
+        public override Vector3 GetMainAxisWorld()
+        {
+            Quaternion offsetRot = Quaternion.Euler(limitsOffset);
+            Vector3 offsetSwingAxis = (offsetRot * this.mainAxis).normalized;
+            return Direction(offsetSwingAxis);
+        }
+
+        public override Vector3 GetStretchAxis()
+        {
+            return GetMainAxisWorld();
+        }
+
+        public override Vector3 GetMidVectorWorld()
+        {
+            return GetPlaneAxisWorld();
+        }
+
+        public Vector3 GetPlaneAxisWorld()
+        {
+            Quaternion offsetRot = Quaternion.Euler(limitsOffset);
+            Vector3 swing = Direction(offsetRot * mainAxis.normalized);
+
+            Vector3 secondaryAxis = new Vector3(mainAxis.y, mainAxis.z, mainAxis.x);
+            Vector3 cross = Direction(Vector3.Cross(mainAxis, secondaryAxis));
+            cross = Direction(offsetRot * this.cross);
+            Quaternion hingeOffset = Quaternion.AngleAxis(hingeAngleOffset, swing);
+            return hingeOffset * cross;
+        }
+
+        /*
+        * Apply the hinge rotation limit
+        * */
+        public Quaternion LimitHinge(Quaternion rotation)
+        {
+            Quaternion offsetRot = Quaternion.Euler(limitsOffset);
+
+            Vector3 offsetSwingAxis = (offsetRot * this.mainAxis);
+            Vector3 offsetSecondaryAxis = (offsetRot * this.cross);
+            Vector3.OrthoNormalize(ref offsetSwingAxis, ref offsetSecondaryAxis);
+            Vector3 offsetCrossAxis = Vector3.Cross(offsetSwingAxis, offsetSecondaryAxis);
+
+            Quaternion hingeOffset = Quaternion.AngleAxis(hingeAngleOffset, offsetSwingAxis);
+            Quaternion minRotation = Quaternion.AngleAxis(-limitAngle, offsetSwingAxis) * hingeOffset;
+            Quaternion maxRotation = Quaternion.AngleAxis(limitAngle, offsetSwingAxis) * hingeOffset;
+
+            // Get 1 degree of freedom rotation along axis
+            Quaternion free1DOFTarget = Quaternion.FromToRotation(rotation * offsetSwingAxis, offsetSwingAxis) * rotation;
+
+            if (limitAngle >= 180)
+                return free1DOFTarget;
+
+            float midLimit = (limitAngle >= 90f) ? 180 - limitAngle : limitAngle;
+            Quaternion free1DOFMid = Quaternion.RotateTowards(minRotation, maxRotation, midLimit);
+            if (limitAngle >= 90f)
+            {
+                Quaternion flip180 = Quaternion.AngleAxis(180, offsetSwingAxis);
+                free1DOFMid *= flip180;
+            }
+
+            Quaternion lastRotation = free1DOFTarget;
+
+            float angle = Quaternion.Angle(free1DOFTarget, free1DOFMid);
+            Quaternion clampedFree1DOF = Quaternion.RotateTowards(free1DOFMid, free1DOFTarget, limitAngle);
+
+            isClampedToLimit = angle >= limitAngle;
+
+            return clampedFree1DOF;
+
+        }
+
+        public override Quaternion LimitRotation(Quaternion rotation, float jointLimitStrength = 1)
+        {
+            lastRotation = LimitHinge(rotation);
+            return lastRotation;
+        }
+
+        public override bool Apply(float jointLimitStrength = 1)
+        {
+            bool hasChanged = base.Apply(jointLimitStrength);
+            return isClampedToLimit;
+        }
+
+    }
+}

Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimitHinge.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: 2c7a022be76393744b9f1a13d09311ca
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimitHinge.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimitSwingTwist.cs

@@ -0,0 +1,174 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace AshqarApps.DynamicJoint
+{
+    public class DynamicJointLimitSwingTwist : DynamicJointLimit
+    {
+        [Header("Swing Twist Limits")]
+        [Range(0f, 180f)] public float twistLimit = 45;
+
+        [Range(0f, 1f)] public float limitWidthRatio = 1;
+        [Range(0f, 1f)] public float limitHeightRatio = 1;
+
+        public Quaternion LimitSwing(Quaternion rotation, bool visualizeMode = false)
+        {
+            if (this.mainAxis == Vector3.zero) return rotation; // Ignore with zero axes
+                                                                //if (rotation == Quaternion.identity) return rotation; // Assuming initial rotation is in the reachable area
+
+            Quaternion offsetRot = Quaternion.Euler(limitsOffset);
+            if (visualizeMode)
+            {
+                offsetRot = Quaternion.identity;
+            }
+
+            Vector3 offsetSwingAxis = (offsetRot * this.mainAxis).normalized;
+            Vector3 offsetSecondaryAxis = (offsetRot * this.secondaryAxis).normalized;
+            Vector3.OrthoNormalize(ref offsetSwingAxis, ref offsetSecondaryAxis);
+            Vector3 offsetCrossAxis = Vector3.Cross(offsetSwingAxis, offsetSecondaryAxis);
+
+            Vector3 targetSwing = rotation * mainAxis;
+
+            float angle = Vector3.SignedAngle(offsetSwingAxis, targetSwing, Vector3.Cross(offsetSwingAxis, targetSwing));
+            Vector3 unlimitedTargetSwing = targetSwing;
+
+            float maxAngle = angle > 0 ? limitAngle : -limitAngle;
+            Quaternion maxRotation = Quaternion.AngleAxis(maxAngle, Vector3.Cross(offsetSwingAxis, targetSwing));
+
+            Vector3 maxTargetSwing = maxRotation * offsetSwingAxis;
+
+            // Scale Limit Based on Width Height Ratios
+            Vector3 limitCircleCenter = Vector3.ProjectOnPlane(offsetSwingAxis - maxTargetSwing, offsetSwingAxis) + maxTargetSwing;
+            float circleRadius = Vector3.Distance(maxTargetSwing, limitCircleCenter);
+            Debug.DrawLine(transform.position + Direction(maxTargetSwing), transform.position + Direction(maxTargetSwing));
+
+            Vector3 projectToCircle = (maxTargetSwing - limitCircleCenter);
+            float width = circleRadius * limitWidthRatio;
+            float height = circleRadius * limitHeightRatio;
+
+            Vector3 coneX = Vector3.Project(projectToCircle, offsetSecondaryAxis);
+            Vector3 coneY = Vector3.Project(projectToCircle, offsetCrossAxis);
+
+            if (limitHeightRatio < 1 || limitWidthRatio < 1)
+            {
+                //find cartesian ellipse coordinates intersection
+                Vector2 center = Vector2.zero;
+                Vector2 pt = new Vector2(coneX.magnitude, coneY.magnitude);
+                Vector3[] candidateProjectionPoints = FindEllipseSegmentIntersections(width, height, center, pt, true);
+
+                //map cartesian intersection point back to joint space
+                if (candidateProjectionPoints.Length > 0)
+                {
+                    Vector3 projectedX = coneX.normalized * candidateProjectionPoints[0].x;
+                    Vector3 projectedY = coneY.normalized * candidateProjectionPoints[0].y;
+                    maxTargetSwing = (limitCircleCenter + projectedX + projectedY).normalized;
+                }
+            }
+
+            if (Vector3.Angle(offsetSwingAxis, maxTargetSwing) < Vector3.Angle(offsetSwingAxis, targetSwing))
+            {
+                isClampedToLimit = true;
+                targetSwing = maxTargetSwing;
+            }
+
+            // Get the limited swing axis
+            Quaternion limitedSwingRotation = Quaternion.FromToRotation(offsetSwingAxis, targetSwing);
+
+            // Rotation from current(illegal) swing rotation to the limited(legal) swing rotation
+            Quaternion toLimits = Quaternion.FromToRotation(unlimitedTargetSwing, limitedSwingRotation * offsetSwingAxis);
+
+            // Subtract the illegal rotation
+            return toLimits * rotation;
+
+        }
+
+        public override Vector3 GetStretchAxis()
+        {
+            return GetMainAxisWorld();
+        }
+
+        public override Vector3 GetMidVectorWorld()
+        {
+            return GetMainAxisWorld();
+        }
+
+        public override Vector3 GetMainAxisWorld()
+        {
+            Quaternion offsetRot = Quaternion.Euler(limitsOffset);
+            Vector3 offsetSwingAxis = (offsetRot * this.mainAxis).normalized;
+            return Direction(offsetSwingAxis);
+        }
+
+        public override Quaternion LimitRotation(Quaternion rotation, float jointLimitStrength = 1)
+        {
+            Quaternion swing = LimitSwing(rotation);
+            return LimitTwist(swing, mainAxis, secondaryAxis, twistLimit);
+        }
+
+
+
+        // Find the points of intersection between
+        // an ellipse and a line segment.
+        public Vector3[] FindEllipseSegmentIntersections(
+                float width, float height, Vector3 pt1, Vector3 pt2, bool segment_only)
+        {
+            // If the ellipse or line segment are empty, return no intersections.
+            if ((width == 0) || (height == 0) ||
+                ((pt1.x == pt2.x) && (pt1.y == pt2.y)))
+                return new Vector3[] { };
+
+            // Get the semimajor and semiminor axes.
+            float a = width;
+            float b = height;
+
+            // Calculate the quadratic parameters.
+            float A = (pt2.x - pt1.x) * (pt2.x - pt1.x) / a / a +
+                      (pt2.y - pt1.y) * (pt2.y - pt1.y) / b / b;
+            float B = 2 * pt1.x * (pt2.x - pt1.x) / a / a +
+                      2 * pt1.y * (pt2.y - pt1.y) / b / b;
+            float C = pt1.x * pt1.x / a / a + pt1.y * pt1.y / b / b - 1;
+
+            // Make a list of t values.
+            List<float> t_values = new List<float>();
+
+            // Calculate the discriminant.
+            float discriminant = B * B - 4 * A * C;
+            if (discriminant == 0)
+            {
+                // One real solution.
+                t_values.Add(-B / 2 / A);
+            }
+            else if (discriminant > 0)
+            {
+                // Two real solutions.
+                t_values.Add((float)((-B + Mathf.Sqrt(discriminant)) / 2 / A));
+                t_values.Add((float)((-B - Mathf.Sqrt(discriminant)) / 2 / A));
+            }
+
+            // Convert the t values into points.
+            List<Vector3> points = new List<Vector3>();
+            foreach (float t in t_values)
+            {
+                // If the points are on the segment (or we
+                // don't care if they are), add them to the list.
+                if (!segment_only || ((t >= 0f) && (t <= 1f)))
+                {
+                    float x = pt1.x + (pt2.x - pt1.x) * t;
+                    float y = pt1.y + (pt2.y - pt1.y) * t;
+                    points.Add(new Vector3(x, y));
+                }
+            }
+
+            // Return the points.
+            return points.ToArray();
+        }
+
+        public override bool Apply(float jointLimitStrength = 1)
+        {
+            bool hasChanged = base.Apply(jointLimitStrength);
+            return isClampedToLimit;
+        }
+
+    }
+}

Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimitSwingTwist.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: 59495989be03fc34f8db82a27c38e0ca
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Joint Limits/DynamicJointLimitSwingTwist.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Joint Limits/Editor.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: fdbf4dac63e13c14c96f45a9bd20e412
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/HybridIK/Scripts/Joint Limits/Editor/DynamicJointLimitHingeInspector.cs

@@ -0,0 +1,325 @@
+#if UNITY_EDITOR
+
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEditor;
+
+namespace AshqarApps.DynamicJoint
+{
+    [CustomEditor(typeof(DynamicJointLimitHinge))]
+    [CanEditMultipleObjects]
+    public class DynamicJointLimitHingeInspector : Editor
+    {
+        private DynamicJointLimitHinge script { get { return target as DynamicJointLimitHinge; } }
+        private Vector3 lastPoint, zeroPoint;
+
+        float lastHingeOffset = 0;
+
+        public override void OnInspectorGUI()
+        {
+            GUI.changed = false;
+
+            base.OnInspectorGUI();
+            //DrawDefaultInspector();
+
+            int childCount = script.transform.childCount;
+            GameObject childGO = null;
+            for (int i = 0; i < script.transform.childCount; ++i)
+            {
+                if (!script.transform.GetChild(i).tag.Contains("ExcludeFromDynamics"))
+                {
+                    childGO = script.transform.GetChild(i).gameObject;
+                    break;
+                }
+            }
+
+            if (childGO != null)
+            {
+                if (GUILayout.Button("Align To Child Twist"))
+                {
+                    Vector3 toChild = childGO.transform.position - script.transform.position;
+                    script.mainAxis = script.transform.InverseTransformDirection(toChild.normalized);
+                    Vector3 secondaryAxis = new Vector3(script.mainAxis.y, script.mainAxis.z, script.mainAxis.x);
+                    script.cross = Direction(Vector3.Cross(script.mainAxis, secondaryAxis));
+                }
+                else if (GUILayout.Button("Align To Child X"))
+                {
+                    Vector3 toChild = (childGO.transform.position - script.transform.position).normalized;
+                    script.cross = script.transform.InverseTransformDirection(toChild);
+                    //Vector3 secondaryAxis = new Vector3(script.cross.y, script.cross.z, script.cross.x);
+                    Vector3 secondaryAxis = script.transform.InverseTransformDirection(script.transform.right);
+                    Vector3.OrthoNormalize(ref secondaryAxis, ref script.cross);
+                    //Vector3 cross = Direction(Vector3.Cross(script.cross, secondaryAxis));
+                    script.mainAxis = secondaryAxis;
+                }
+                else if (GUILayout.Button("Align To Child Y"))
+                {
+                    Vector3 toChild = (childGO.transform.position - script.transform.position).normalized;
+                    script.cross = script.transform.InverseTransformDirection(toChild);
+                    Vector3 secondaryAxis = script.transform.InverseTransformDirection(script.transform.up);
+                    Vector3.OrthoNormalize(ref secondaryAxis, ref script.cross);
+                    script.mainAxis = secondaryAxis;
+                }
+                else if (GUILayout.Button("Align To Child Z"))
+                {
+                    Vector3 toChild = (childGO.transform.position - script.transform.position).normalized;
+                    script.cross = script.transform.InverseTransformDirection(toChild);
+                    Vector3 secondaryAxis = script.transform.InverseTransformDirection(script.transform.forward);
+                    Vector3.OrthoNormalize(ref secondaryAxis, ref script.cross);
+                    script.mainAxis = secondaryAxis;
+                }
+            }
+            else
+            {
+                if (GUILayout.Button("Align To X"))
+                {
+                    Vector3 toChild = script.transform.right;
+                    script.mainAxis = script.transform.InverseTransformDirection(toChild.normalized);
+                    Vector3 secondaryAxis = new Vector3(script.mainAxis.y, script.mainAxis.z, script.mainAxis.x);
+                    script.cross = Vector3.Cross(script.mainAxis, secondaryAxis);
+                }
+
+                if (GUILayout.Button("Align To Y"))
+                {
+                    Vector3 toChild = script.transform.up;
+                    script.mainAxis = script.transform.InverseTransformDirection(toChild.normalized);
+                    Vector3 secondaryAxis = new Vector3(script.mainAxis.y, script.mainAxis.z, script.mainAxis.x);
+                    script.cross = Vector3.Cross(script.mainAxis, secondaryAxis);
+                }
+
+                if (GUILayout.Button("Align To Z"))
+                {
+                    Vector3 toChild = script.transform.forward;
+                    script.mainAxis = script.transform.InverseTransformDirection(toChild.normalized);
+                    Vector3 secondaryAxis = new Vector3(script.mainAxis.y, script.mainAxis.z, script.mainAxis.x);
+                    script.cross = Vector3.Cross(script.mainAxis, secondaryAxis);
+                }
+            }
+
+            if (GUI.changed) EditorUtility.SetDirty(script);
+
+        }
+
+
+        void UpdateZeroTransform()
+        {
+            if (lastHingeOffset != script.hingeAngleOffset)
+            {
+                lastHingeOffset = script.hingeAngleOffset;
+
+            }
+        }
+
+        void OnSceneGUI()
+        {
+            if (!Application.isPlaying && !script.zeroRotationOverride) script.zeroRotation = script.transform.localRotation;
+            if (script.mainAxis == Vector3.zero || script.cross == Vector3.zero) return;
+
+            float minBoneLength = 0.15f;
+            float boneLength = minBoneLength;
+            if (script.transform.childCount > 0)
+            {
+                boneLength = Vector3.Distance(script.transform.position, script.transform.GetChild(0).position) / 4;
+            }
+            else
+            {
+                if (script.transform.parent != null)
+                {
+                    boneLength = Vector3.Distance(script.transform.position, script.transform.parent.position) / 4;
+                }
+            }
+
+            boneLength = Mathf.Max(boneLength, minBoneLength);
+
+
+            // Display the main axis
+            DrawArrow(script.transform.position, Direction(script.mainAxis), colorDefault, "Axis", 0.02f);
+
+            Handles.color = Color.white;
+            GUI.color = Color.white;
+
+            if (!Application.isPlaying)
+            {
+
+                // Selecting points
+                Handles.color = colorHandles;
+                Handles.Button(script.transform.position + Direction(script.mainAxis), script.transform.rotation, 0.02f, 0.02f, Handles.DotHandleCap);
+
+                Vector3 selectedPoint = script.mainAxis;
+
+                // Store point for undo
+                Vector3 oldPoint = selectedPoint;
+
+                // Manual input for the point position
+                AddVector3(ref selectedPoint, "Point", script, GUILayout.Width(210));
+
+                // Moving Points
+                Vector3 pointWorld = Handles.PositionHandle(script.transform.position + Direction(selectedPoint), Quaternion.identity);
+                Vector3 newPoint = InverseDirection(pointWorld - script.transform.position);
+                if (newPoint != selectedPoint)
+                {
+                    if (!Application.isPlaying) Undo.RecordObject(script, "Move Limit Point");
+                    script.mainAxis = newPoint.normalized;
+                }
+            }
+
+            DrawHingeLimits(boneLength);
+
+        }
+
+
+        public void DrawHingeLimits(float boneLength)
+        {
+            Quaternion offsetRot = Quaternion.Euler(script.limitsOffset);
+
+            Vector3 swing = Direction(offsetRot * script.mainAxis.normalized);
+
+            //Vector3 secondaryAxis = new Vector3(script.mainAxis.y, script.mainAxis.z, script.mainAxis.x);
+            //Vector3 cross = Direction(Vector3.Cross(script.mainAxis, secondaryAxis));
+            if (script.cross.magnitude == 0) return;
+            Vector3 cross = Direction(offsetRot * script.cross);
+            Vector3.OrthoNormalize(ref swing, ref cross);
+            Vector3 secondaryAxis = Vector3.Cross(script.mainAxis, cross);
+
+            Handles.CircleHandleCap(0, script.transform.position, Quaternion.LookRotation(swing, cross), boneLength, EventType.Repaint);
+
+            //DrawArrow(script.transform.position, cross * boneLength, colorDefault, " 0", 0.02f);
+            DrawArrow(script.transform.position, cross * boneLength, colorDefault, " 0", 0.02f);
+
+            Quaternion hingeOffset = Quaternion.AngleAxis(script.hingeAngleOffset, swing);
+
+            // Arcs for the rotation limit
+            Handles.color = colorDefaultTransparent;
+            Handles.DrawSolidArc(script.transform.position, swing, hingeOffset * cross, -script.limitAngle, boneLength);
+            Handles.DrawSolidArc(script.transform.position, swing, hingeOffset * cross, script.limitAngle, boneLength);
+
+            Quaternion minRotation = Quaternion.AngleAxis(-script.limitAngle, swing);
+            Vector3 minHandleDir = minRotation * (hingeOffset * cross);
+            Handles.DrawLine(script.transform.position, script.transform.position + minHandleDir.normalized * boneLength);
+
+            Quaternion maxRotation = Quaternion.AngleAxis(script.limitAngle, swing);
+            Vector3 maxHandleDir = maxRotation * (hingeOffset * cross);
+            Handles.DrawLine(script.transform.position, script.transform.position + maxHandleDir.normalized * boneLength);
+
+            Handles.color = colorHandles;
+            //Draw Editable Handles
+            float originalLimit = script.limitAngle;
+            float limitAngleMin = script.limitAngle;
+            limitAngleMin = DrawLimitHandle(limitAngleMin, script.transform.position + (minHandleDir.normalized * boneLength * 1.25f), Quaternion.identity, 0.5f, "Limit", -10);
+            if (limitAngleMin != script.limitAngle)
+            {
+                if (!Application.isPlaying) Undo.RecordObject(script, "Min Limit");
+                script.limitAngle = limitAngleMin;
+                script.hingeAngleOffset -= (limitAngleMin - originalLimit);
+            }
+
+            originalLimit = script.limitAngle;
+            //Draw Editable Handles
+            float limitAngleMax = script.limitAngle;
+            limitAngleMax = DrawLimitHandle(limitAngleMax, script.transform.position + (maxHandleDir.normalized * boneLength * 1.25f), Quaternion.identity, 0.5f, "Limit", 10);
+            if (limitAngleMax != script.limitAngle)
+            {
+                if (!Application.isPlaying) Undo.RecordObject(script, "Max Limit");
+                script.limitAngle = limitAngleMax;
+                script.hingeAngleOffset += (limitAngleMax - originalLimit);
+            }
+
+            // clamp limits
+            script.limitAngle = Mathf.Clamp(script.limitAngle, 0, 180f);
+            if (script.hingeAngleOffset < 0) script.hingeAngleOffset += 360;
+            if (script.hingeAngleOffset > 360) script.hingeAngleOffset -= 360;
+
+        }
+
+        private Vector3 Direction(Vector3 v)
+        {
+            if (script.transform.parent == null) return script.zeroRotation * v;
+            return script.transform.parent.rotation * (script.zeroRotation * v);
+        }
+
+        private Vector3 InverseDirection(Vector3 v)
+        {
+            if (script.transform.parent == null) return Quaternion.Inverse(script.zeroRotation) * v;
+
+            return Quaternion.Inverse(script.zeroRotation) * Quaternion.Inverse(script.transform.parent.rotation) * v;
+        }
+
+        // Universal color pallettes
+        public static Color colorDefault { get { return new Color(0.0f, 0.0f, 1.0f, 1.0f); } }
+
+        public static Color colorDefaultTransparent
+        {
+            get
+            {
+                Color d = colorDefault;
+                return new Color(d.r, d.g, d.b, 0.2f);
+            }
+        }
+
+        public static Color colorHandles { get { return new Color(1.0f, 0.5f, 0.25f, 1.0f); } }
+        public static Color colorRotationSphere { get { return new Color(1.0f, 1.0f, 1.0f, 0.1f); } }
+        public static Color colorInvalid { get { return new Color(1.0f, 0.3f, 0.3f, 1.0f); } }
+        public static Color colorValid { get { return new Color(0.2f, 1.0f, 0.2f, 1.0f); } }
+
+        /*
+         * Draws a custom arrow to the scene
+         * */
+        public static void DrawArrow(Vector3 position, Vector3 direction, Color color, string label = "", float size = 0.01f)
+        {
+            Handles.color = color;
+            Handles.DrawLine(position, position + direction);
+            Handles.SphereHandleCap(0, position + direction, Quaternion.identity, size, EventType.Repaint);
+            Handles.color = Color.white;
+
+            if (label != "")
+            {
+                GUI.color = color;
+                Handles.Label(position + direction, label);
+                GUI.color = Color.white;
+            }
+        }
+
+        /*
+         * Draws a handle for adjusting rotation limits in the scene
+         * */
+        public static float DrawLimitHandle(float limit, Vector3 position, Quaternion rotation, float radius, string label, float openingValue, bool reverseScaleDirection = false)
+        {
+            float scaleFactor = reverseScaleDirection ? -1 : 1;
+
+            limit = scaleFactor * Handles.ScaleValueHandle(limit, position, rotation, radius, Handles.SphereHandleCap, 1);
+
+            string labelInfo = label + ": " + limit.ToString();
+
+            // If value is 0, draw a button to 'open' the value, because we cant scale 0
+            if (limit == 0)
+            {
+                labelInfo = "Open " + label;
+
+                if (Handles.Button(position, rotation, radius * 0.2f, radius * 0.07f, Handles.SphereHandleCap))
+                {
+                    limit = openingValue;
+                }
+            }
+
+            Handles.Label(position, labelInfo);
+
+            return limit;
+        }
+
+
+        public static void AddVector3(ref Vector3 input, string name, Object undoObject, params GUILayoutOption[] options)
+        {
+            Vector3 newValue = EditorGUILayout.Vector3Field(name, input, options);
+
+            if (newValue != input && !Application.isPlaying)
+            {
+                Undo.RecordObject(undoObject, name);
+            }
+
+            input = newValue;
+        }
+    }
+}
+#endif
+

Assets/HybridIK/Scripts/Joint Limits/Editor/DynamicJointLimitHingeInspector.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: be8d162414d22194399e23501b403f03
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Joint Limits/Editor/DynamicJointLimitHingeInspector.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Joint Limits/Editor/DynamicJointLimitSwingTwistInspector.cs

@@ -0,0 +1,248 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEditor;
+
+namespace AshqarApps.DynamicJoint
+{
+    [CustomEditor(typeof(DynamicJointLimitSwingTwist))]
+    [CanEditMultipleObjects]
+    public class DynamicJointLimitSwingTwistInspector : Editor
+    {
+        private DynamicJointLimitSwingTwist script { get { return target as DynamicJointLimitSwingTwist; } }
+        private int axisComponentSelected = 0;
+        private Vector3 lastPoint, zeroPoint;
+        private int selectedChild = 0;
+
+        public override void OnInspectorGUI()
+        {
+            base.OnInspectorGUI();
+
+            int childCount = script.transform.childCount;
+
+            if (childCount == 1 && GUILayout.Button("Align To Child"))
+            {
+                Vector3 toChild = script.transform.GetChild(0).position - script.transform.position;
+                script.mainAxis = script.transform.InverseTransformDirection(toChild.normalized);
+                if (GUI.changed) EditorUtility.SetDirty(script);
+            }
+            else if (childCount > 1)
+            {
+                List<string> childGOs = new List<string>();
+
+                //if (childGOs == null)
+                {
+                    for (int i = 0; i < script.transform.childCount; ++i)
+                    {
+                        childGOs.Add(script.transform.GetChild(i).name);
+                    }
+                }
+
+                selectedChild = EditorGUILayout.Popup("Selected Child", selectedChild, childGOs.ToArray());
+
+                //int selectedChild = 0;
+                if (GUILayout.Button("Align To Selected Child"))
+                {
+                    GameObject childGO = script.transform.GetChild(selectedChild).gameObject;
+
+                    if (childGO.transform.childCount > 0)
+                    {
+                        childGO = childGO.transform.GetChild(0).gameObject;
+
+                        Vector3 toChild = childGO.transform.position - script.transform.position;
+                        script.mainAxis = script.transform.InverseTransformDirection(toChild.normalized);
+                        if (GUI.changed) EditorUtility.SetDirty(script);
+                    }
+                }
+            }
+        }
+
+        void OnSceneGUI()
+        {
+            // Set defaultLocalRotation so that the initial local rotation will be the zero point for the rotation limit
+            if (!Application.isPlaying && !script.zeroRotationOverride) script.zeroRotation = script.transform.localRotation;
+            if (script.mainAxis == Vector3.zero) return;
+
+            //DrawRotationSphere(script.transform.position);
+
+            Handles.color = Color.white;
+            GUI.color = Color.white;
+
+            float minBoneLength = 0.15f;
+            float boneLength = minBoneLength;
+            if (script.transform.childCount > 0)
+            {
+                boneLength = Vector3.Distance(script.transform.position, script.transform.GetChild(0).position) / 4;
+            }
+            else
+            {
+                if (script.transform.parent != null)
+                {
+                    boneLength = Vector3.Distance(script.transform.position, script.transform.parent.position) / 4;
+                }
+            }
+            boneLength = Mathf.Max(boneLength, minBoneLength);
+
+            // Display the main axis
+            DrawArrow(script.transform.position, Direction(script.mainAxis).normalized * boneLength, colorDefault, "Axis", 0.02f);
+            DrawArrow(script.transform.position, Direction(script.secondaryAxis) * boneLength, colorDefault, "Secondary Axis", 0.02f);
+
+            if (!Application.isPlaying)
+            {
+                // Selecting points
+                Handles.color = colorHandles;
+                if (Handles.Button(script.transform.position + Direction(script.mainAxis.normalized) * boneLength * 1.25f, script.transform.rotation, 0.02f, 0.02f, Handles.DotHandleCap))
+                {
+                    axisComponentSelected = 0;
+                }
+                else if (Handles.Button(script.transform.position + Direction(script.secondaryAxis.normalized) * boneLength * 1.25f, script.transform.rotation, 0.02f, 0.02f, Handles.DotHandleCap))
+                {
+                    axisComponentSelected = 1;
+                }
+
+                Vector3 selectedPoint = axisComponentSelected == 0 ? script.mainAxis : script.secondaryAxis;
+
+                // Store point for undo
+                Vector3 oldPoint = selectedPoint;
+
+                // Manual input for the point position
+                AddVector3(ref selectedPoint, "Point", script, GUILayout.Width(210));
+                //EditorGUILayout.Space();
+
+                // Moving Points
+                Vector3 pointWorld = Handles.PositionHandle(script.transform.position + (Direction(selectedPoint.normalized) * boneLength * 1.25f), Quaternion.identity);
+                Vector3 newPoint = InverseDirection(pointWorld - script.transform.position);
+                if (newPoint != selectedPoint)
+                {
+                    if (!Application.isPlaying) Undo.RecordObject(script, "Move Limit Point");
+                    if (axisComponentSelected == 0)
+                        script.mainAxis = newPoint;
+                    else
+                        script.secondaryAxis = newPoint;
+                }
+            }
+
+            Vector3.OrthoNormalize(ref script.mainAxis, ref script.secondaryAxis);
+
+            DrawSwingTwistLimits(boneLength);
+
+        }
+
+
+        public void DrawSwingTwistLimits(float boneLength)
+        {
+            Vector3 swing = script.mainAxis.normalized;
+            Vector3 secondaryAxis = script.secondaryAxis.normalized; //new Vector3(swing.y, swing.z, swing.x);
+            Vector3.OrthoNormalize(ref swing, ref secondaryAxis);
+            Vector3 cross = Vector3.Cross(swing, secondaryAxis);
+
+            // DRAW TWIST
+            Quaternion offsetRot = Quaternion.Euler(script.limitsOffset);
+            Handles.color = colorTwistLimit;
+            Handles.DrawSolidArc(script.transform.position, Direction(offsetRot * swing), Direction(offsetRot * cross), script.twistLimit, boneLength);
+            Handles.DrawSolidArc(script.transform.position, Direction(offsetRot * swing), Direction(offsetRot * cross), -script.twistLimit, boneLength);
+
+            // Display limits
+            lastPoint = script.transform.position;
+
+            for (int i = 0; i < 360; i += 2)
+            {
+                //float evaluatedLimit = script.EvaluateSwingLimitAtOrthoAngle((float)(i));
+                //float evaluatedLimit = script.curveX.Evaluate(Mathf.Abs(Mathf.Sin()));
+
+                Quaternion offset = Quaternion.AngleAxis(i, swing);
+                Quaternion evaluatedRotation = Quaternion.AngleAxis(script.limitAngle, offset * (cross));
+
+                Color color = colorDefaultTransparent;
+
+                Vector3 swingAxis = script.LimitSwing(evaluatedRotation, visualizeMode: true) * swing;
+                swingAxis = offsetRot * swingAxis;
+
+                Vector3 limitPoint = script.transform.position + Direction(swingAxis).normalized * boneLength;
+                Handles.DrawLine(script.transform.position, script.transform.position + Direction(swingAxis).normalized * boneLength);
+
+                Handles.color = color;
+                if (i == 0) zeroPoint = limitPoint;
+
+                if (i > 0)
+                {
+                    Handles.color = colorDefault;
+                    Handles.DrawLine(limitPoint, lastPoint);
+                    if (i == 358) Handles.DrawLine(limitPoint, zeroPoint);
+                }
+
+                lastPoint = limitPoint;
+            }
+        }
+
+        private Vector3 Direction(Vector3 v)
+        {
+            if (script.transform.parent == null) return script.zeroRotation * v;
+            return script.transform.parent.rotation * (script.zeroRotation * v);
+        }
+
+        private Vector3 InverseDirection(Vector3 v)
+        {
+            if (script.transform.parent == null) return Quaternion.Inverse(script.zeroRotation) * v;
+
+            return Quaternion.Inverse(script.zeroRotation) * Quaternion.Inverse(script.transform.parent.rotation) * v;
+        }
+
+        public static Color colorDefault { get { return new Color(0.0f, 1.0f, 1.0f, 0.5f); } }
+        public static Color colorDefaultTransparent
+        {
+            get
+            {
+                Color d = colorDefault;
+                return new Color(d.r, d.g, d.b, 0.2f);
+            }
+        }
+
+
+        public static Color colorHandles { get { return new Color(1.0f, 0.5f, 0.25f, 1.0f); } }
+        public static Color colorRotationSphere { get { return new Color(1.0f, 1.0f, 1.0f, 0.1f); } }
+        public static Color colorTwistLimit { get { return new Color(1.0f, 1.0f, 0.0f, 0.1f); } }
+        public static Color colorInvalid { get { return new Color(1.0f, 0.3f, 0.3f, 1.0f); } }
+        public static Color colorValid { get { return new Color(0.2f, 1.0f, 0.2f, 1.0f); } }
+
+        /*
+         * Draws the default rotation limit sphere to the scene
+         * */
+        public static void DrawRotationSphere(Vector3 position, float radius)
+        {
+            Handles.color = colorRotationSphere;
+            Handles.SphereHandleCap(0, position, Quaternion.identity, radius, EventType.Repaint);
+            Handles.color = Color.white;
+        }
+
+        /*
+         * Draws a custom arrow to the scene
+         * */
+        public static void DrawArrow(Vector3 position, Vector3 direction, Color color, string label = "", float size = 0.01f)
+        {
+            Handles.color = color;
+            Handles.DrawLine(position, position + direction);
+            Handles.SphereHandleCap(0, position + direction, Quaternion.identity, size, EventType.Repaint);
+            Handles.color = Color.white;
+
+            if (label != "")
+            {
+                GUI.color = color;
+                Handles.Label(position + direction, label);
+                GUI.color = Color.white;
+            }
+        }
+
+        public static void AddVector3(ref Vector3 input, string name, Object undoObject, params GUILayoutOption[] options)
+        {
+            Vector3 newValue = EditorGUILayout.Vector3Field(name, input, options);
+
+            if (newValue != input && !Application.isPlaying)
+            {
+                Undo.RecordObject(undoObject, name);
+            }
+
+            input = newValue;
+        }
+    }
+}

Assets/HybridIK/Scripts/Joint Limits/Editor/DynamicJointLimitSwingTwistInspector.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: 64e32b8cc7ffc8545adde630370be74d
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Joint Limits/Editor/DynamicJointLimitSwingTwistInspector.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Main.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 0b57a501956808744a6f4c77d47aa9ac
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/HybridIK/Scripts/Main/Editor.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 85bff82fc19c984458a58f9f4d097ed8
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/HybridIK/Scripts/Main/Editor/HybridIKNodeInspector.cs

@@ -0,0 +1,312 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEditor;
+
+[CustomEditor(typeof(HybridInverseKinematicsNode))]
+public class HybridIKNodeInspector : Editor
+{
+    private HybridInverseKinematicsNode hybridIKNode { get { return target as HybridInverseKinematicsNode; } }
+    List<string> boneHierarchy = new List<string>();
+
+    private int selectedIKNodeIndex = -1;
+
+    private int selectedIKKeyframe = -1;
+
+    public void DeletePoseKeyframe(int selectedPose)
+    {
+        foreach (HybridIKConstraint c in hybridIKNode.constraints)
+        {
+            if (c.positionKeys.Count > 0)
+            {
+                c.positionKeys.RemoveAt(selectedPose);
+            }
+        }
+        foreach (HybridIKJoint node in hybridIKNode.nodes)
+        {
+            node.keyedPositions.RemoveAt(selectedPose);
+            node.keyedLocalPositions.RemoveAt(selectedPose);
+            node.keyedRotations.RemoveAt(selectedPose);
+        }
+
+        selectedIKKeyframe = -1;
+        hybridIKNode.ResetToZeroPose();
+    }
+
+    public override void OnInspectorGUI()
+    {
+        if (selectedIKKeyframe >= 0)
+        {
+            if (GUILayout.Button("Select Full Chain"))
+            {
+                selectedIKKeyframe = -1;
+            }
+
+            if (selectedIKKeyframe >= 0)
+            {
+                GUILayout.Label("IK Keyframe " + selectedIKKeyframe.ToString() + " selected");
+                if (GUILayout.Button("Delete Pose Keyframe"))
+                {
+                    DeletePoseKeyframe(selectedIKKeyframe);
+                }
+                return;
+            }
+        }
+
+        if (selectedIKNodeIndex >= 0)
+        {
+            HybridIKJoint selectedNode = hybridIKNode.nodes[selectedIKNodeIndex];
+
+            GUILayout.BeginHorizontal();
+            if (selectedNode.jointTransform != null)
+            {
+                if (GUILayout.Button("Select Joint GameObject"))
+                {
+                    Selection.activeGameObject = selectedNode.jointTransform.gameObject;
+                }
+            }
+            if (GUILayout.Button("Select Full Chain"))
+            {
+                selectedIKNodeIndex = -1;
+            }
+            GUILayout.EndHorizontal();
+
+            if (selectedIKNodeIndex >= 0)
+            {
+                GUILayout.Label("IK Node " + selectedIKNodeIndex.ToString() + " selected");
+                selectedNode.jointRadius = EditorGUILayout.FloatField("Node Radius", selectedNode.jointRadius);
+                var oldColor = GUI.backgroundColor;
+                GUILayout.Label("CONSTRAINTS MODE");
+                GUILayout.BeginHorizontal();
+                if (selectedNode.enableKeyframeConstraints && !selectedNode.overrideConstraint) GUI.backgroundColor = Color.green;
+                if (GUILayout.Button("KEYFRAMED"))
+                {
+                    selectedNode.enableKeyframeConstraints = true;
+                    selectedNode.overrideConstraint = false;
+                    hybridIKNode.ReprocessJoints();
+                }
+                GUI.backgroundColor = oldColor;
+                if (selectedNode.overrideConstraint) GUI.backgroundColor = Color.green;
+                if (GUILayout.Button("MANUAL CONSTRAINT"))
+                {
+                    selectedNode.enableKeyframeConstraints = false;
+                    selectedNode.overrideConstraint = true;
+                    hybridIKNode.ReprocessJoints();
+                }
+                GUI.backgroundColor = oldColor;
+                if (!selectedNode.enableKeyframeConstraints && !selectedNode.overrideConstraint) GUI.backgroundColor = Color.green;
+                if (GUILayout.Button("NONE"))
+                {
+                    selectedNode.enableKeyframeConstraints = false;
+                    selectedNode.overrideConstraint = false;
+                    hybridIKNode.ReprocessJoints();
+                }
+                GUILayout.EndHorizontal();
+                GUI.backgroundColor = oldColor;
+
+                if (selectedNode.overrideConstraint)
+                {
+                    //n.position = EditorGUILayout.Vector3Field("Target Position", node.position);
+                    selectedNode.constraint.jointTransform = selectedNode.jointTransform;
+                    selectedNode.constraint.targetTransform = EditorGUILayout.ObjectField("Target Transform", selectedNode.constraint.targetTransform, typeof(Transform), true) as Transform;
+
+                    selectedNode.constraint.constrainPosition = EditorGUILayout.Toggle("Constrain Position", selectedNode.constraint.constrainPosition);
+                    selectedNode.constraint.constrainRotation = EditorGUILayout.Toggle("Constrain Orientation", selectedNode.constraint.constrainRotation);
+                }
+
+                selectedNode.enableStretch = EditorGUILayout.Toggle("Enable Stretch", selectedNode.enableStretch);
+                if (selectedNode.enableStretch)
+                {
+                    if (selectedNode.stretchLimits == null)
+                    {
+                        selectedNode.stretchLimits = new HybridIKNodeStretchLimits();
+                    }
+
+                    selectedNode.stretchLimits.targetSpace = EditorGUILayout.ObjectField("Target Transform", selectedNode.stretchLimits.targetSpace, typeof(Transform), true) as Transform;
+
+                    selectedNode.stretchLimits.minStretchLimits = EditorGUILayout.Vector3Field("Min Stretch Limits", selectedNode.stretchLimits.minStretchLimits);
+                    selectedNode.stretchLimits.maxStretchLimits = EditorGUILayout.Vector3Field("Max Stretch Limits", selectedNode.stretchLimits.maxStretchLimits);
+                }
+
+                if (GUI.changed) EditorUtility.SetDirty(hybridIKNode);
+                return;
+            }
+        }
+
+        if (GUILayout.Button("Process IK Joint Chain"))
+        {
+            hybridIKNode.ResetAll();
+            hybridIKNode.ProcessChain();
+        }
+        if (hybridIKNode.nodes != null)
+        {
+            if (GUILayout.Button("Set Zero Rotations to Current Pose"))
+            {
+                foreach (HybridIKJoint node in hybridIKNode.nodes)
+                {
+                    node.zeroRotation = node.jointTransform.localRotation;
+                    node.zeroPosition = node.jointTransform.localPosition;
+                }
+            }
+        }
+
+        base.OnInspectorGUI();
+        if (hybridIKNode.nodes == null) return;
+
+        if (hybridIKNode.constraints == null || hybridIKNode.constraints.Count == 0)
+        {
+            hybridIKNode.constraints = new List<HybridIKConstraint>();
+            foreach (HybridIKJoint node in hybridIKNode.nodes)
+            {
+                node.keyedLocalPositions = new List<Vector3>();
+                node.keyedPositions = new List<Vector3>();
+                node.keyedRotations = new List<Quaternion>();
+            }
+        }
+
+        if (hybridIKNode.constraints != null)// && hybridIKNode.constraints.Count > 0)
+        {
+            //EditorGUILayout.LabelField("Keyframe Constraints", EditorStyles.boldLabel);
+            if (GUILayout.Button("Add Joints Pose Keyframe"))
+            {
+                List<int> keyframeState = new List<int>();
+                foreach(HybridIKJoint j in hybridIKNode.nodes)
+                {
+                    keyframeState.Add(j.enableKeyframeConstraints ? 0 : j.overrideConstraint ? 1 : 2);
+                    j.enableKeyframeConstraints = true; j.overrideConstraint = false;
+                }
+
+                hybridIKNode.ReprocessJoints();
+
+                HybridIKJoint root = hybridIKNode.nodes[0];
+                Transform parent = root.jointTransform.parent;
+
+                foreach (HybridIKJoint node in hybridIKNode.nodes)
+                {
+                    // encode rotation
+                    if (node.keyedRotations == null)
+                        node.keyedRotations = new List<Quaternion>();
+                    node.keyedRotations.Add(node.jointTransform.localRotation);
+
+                    //encode local positions used for stretch
+                    if (node.keyedLocalPositions == null)
+                        node.keyedLocalPositions = new List<Vector3>();
+                    node.keyedLocalPositions.Add(node.jointTransform.localPosition);
+
+                    // encode position in parent space
+                    if (node.keyedPositions == null)
+                        node.keyedPositions = new List<Vector3>();
+                    node.keyedPositions.Add(parent != null ? parent.InverseTransformPoint(node.GetCurrentPositionWorld()) : node.GetCurrentPositionWorld());
+
+                    HybridIKConstraint constraint = hybridIKNode.constraints.Find(c => c.jointTransform == node.jointTransform);
+                    if (constraint == null)
+                    {
+                        constraint = new HybridIKConstraint();
+                        constraint.jointTransform = node.jointTransform;
+                        constraint.constrainPosition = true;
+                        hybridIKNode.constraints.Add(constraint);
+                    }
+                }
+
+                foreach (HybridIKConstraint c in hybridIKNode.constraints)
+                {
+                    ConstraintPositionKey key = new ConstraintPositionKey();
+                    if (c.positionKeys == null) c.positionKeys = new List<ConstraintPositionKey>();
+                    key.constraintPositionValue = c.jointTransform.position;
+
+                    Vector3 endPos = parent != null ? parent.InverseTransformPoint(hybridIKNode.endNode.position) : hybridIKNode.endNode.position;
+                    key.SetEndTargetPosition(hybridIKNode.endNode.position, parent);
+                    c.positionKeys.Add(key);
+                }
+                hybridIKNode.ResetToZeroPose();
+            }
+
+            if (hybridIKNode.nodes != null && hybridIKNode.nodes.Count > 0 && hybridIKNode.nodes[0] != null && hybridIKNode.nodes[0].keyedPositions != null)
+            {
+                for (int p = 0; p < hybridIKNode.nodes[0].keyedPositions.Count; ++p)
+                {
+                    GUILayout.BeginHorizontal();
+                    GUILayout.Label("Key Pose " + (p + 1).ToString());
+                    if (GUILayout.Button("Select"))
+                    {
+                        //selectedIKKeyframe = p;
+                        hybridIKNode.PoseToKeyframe(p);
+                        Repaint();
+                    }
+                    if (GUILayout.Button("Delete Key"))
+                    {
+                        DeletePoseKeyframe(p);
+                    }
+                    GUILayout.EndHorizontal();
+                }
+            }
+        }
+
+        if (GUI.changed) EditorUtility.SetDirty(hybridIKNode);
+    }
+
+    private void OnDestroy()
+    {
+        if (!Application.isPlaying)
+            hybridIKNode.ResetToZeroPose();
+    }
+
+    public void OnSceneGUI()
+    {
+        if (hybridIKNode.nodes == null || hybridIKNode.nodes.Count == 0) return;
+
+        Color originalColor = Handles.color;
+        if (hybridIKNode.IsInitialized())
+        {
+            for (int i = 0; i < hybridIKNode.nodes.Count; ++i)
+            //foreach (SweepIKNode n in hybridIKNode.nodes)
+            {
+                HybridIKJoint n = hybridIKNode.nodes[i];
+                Color c = n.enableKeyframeConstraints ? Color.blue : n.overrideConstraint ? Color.cyan : originalColor;
+                Handles.color = selectedIKNodeIndex == i ? Color.green : c;
+                float nRadius = Mathf.Max(n.jointRadius, 0.05f, n.jointRadius);
+                if (Handles.Button(n.GetCurrentPositionWorld(), Quaternion.identity, nRadius, nRadius,Handles.SphereHandleCap))
+                {
+                    selectedIKNodeIndex = i;
+                    selectedIKKeyframe = -1;
+                    if (!Application.isPlaying)
+                        hybridIKNode.ResetToZeroPose();
+                    Repaint();
+                }
+            }
+
+            HybridIKJoint root = hybridIKNode.GetRootIKNode();
+            HybridIKJoint endIKNode = hybridIKNode.GetEndIKNode();
+
+            Transform parentSpace = root.jointTransform.parent;
+
+            foreach (HybridIKConstraint c in hybridIKNode.constraints)
+            {
+                if (c.positionKeys.Count > 0)
+                {
+                    for (int i = 0; i < c.positionKeys.Count; ++i)
+                    {
+                        ConstraintPositionKey k = c.positionKeys[i];
+                        Handles.color = selectedIKKeyframe == i ? Color.green : originalColor;
+                        if (i % 4 == 0) Handles.color = Color.green;
+                        if (i % 4 == 1) Handles.color = Color.blue;
+                        if (i % 4 == 2) Handles.color = Color.yellow;
+                        if (i % 4 == 3) Handles.color = Color.magenta;
+
+                        Vector3 buttonPos = k.GetEndTargetPosition(endIKNode.targetIKSpace != null ? endIKNode.targetIKSpace : parentSpace);
+
+                        if (Handles.Button(buttonPos, Quaternion.identity, endIKNode.jointRadius, endIKNode.jointRadius, Handles.SphereHandleCap))
+                        {
+                            selectedIKKeyframe = i;
+                            selectedIKNodeIndex = -1;
+
+                            hybridIKNode.PoseToKeyframe(selectedIKKeyframe);
+                            Repaint();
+                        }
+                    }
+                }
+            }
+        }
+        Handles.color = originalColor;
+    }
+}

Assets/HybridIK/Scripts/Main/Editor/HybridIKNodeInspector.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: 4d33a09095add3f4cb5e399c4bc4b2d1
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Main/Editor/HybridIKNodeInspector.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Main/HybridIKConstraint.cs

@@ -0,0 +1,85 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+
+[System.Serializable]
+public class HybridIKConstraint
+{
+    public Transform jointTransform;
+    public Transform targetTransform;
+
+    public Vector3 targetPosition;
+    public Vector3 targetLocalPosition; //used for stretch constraints
+    public Quaternion targetRotation = Quaternion.identity;
+
+    public bool constrainPosition = true;
+    public bool constrainRotation = false;
+
+    public Vector3 GetPosition()
+    {
+        if (targetTransform != null)
+            return targetTransform.position;
+        return targetPosition;
+    }
+
+    public Vector3 GetLocalPosition()
+    {
+        //if (targetTransform != null)
+        //    return targetTransform.position;
+        return targetLocalPosition;
+    }
+
+    public Quaternion GetRotation()
+    {
+        if (targetTransform != null)
+            return targetTransform.rotation;
+        return targetRotation;
+    }
+
+    public List<ConstraintPositionKey> positionKeys;
+}
+
+[System.Serializable]
+public class ConstraintPositionKey
+{
+    public Transform endTargetKeyTransform;
+    public Transform constraintKeyTransform;
+
+    [HideInInspector]
+    public Vector3 endTargetPositionKey;
+    [HideInInspector]
+    public Vector3 constraintPositionValue;
+
+    [HideInInspector]
+    public Quaternion constraintRotationValue;
+    [HideInInspector]
+    public float cachedWeight = 0;
+    [HideInInspector]
+    public float cachedFade = 0;
+
+    public Transform targetSpace = null;
+
+    public Vector3 GetEndTargetPosition(Transform rootSpace = null)
+    {
+        return endTargetKeyTransform != null ? endTargetKeyTransform.position :
+            (rootSpace != null ? rootSpace.TransformPoint(endTargetPositionKey) : endTargetPositionKey);
+    }
+
+    public void SetEndTargetPosition(Vector3 newPos, Transform rootSpace = null)
+    {
+        endTargetPositionKey = rootSpace != null ? rootSpace.InverseTransformPoint(newPos) : newPos;
+        targetSpace = rootSpace;
+    }
+
+    public Vector3 GetConstraintKeyPosition()
+    {
+        return constraintKeyTransform != null ? constraintKeyTransform.position : constraintPositionValue;
+    }
+
+    public Quaternion GetConstraintKeyRotation()
+    {
+        return constraintKeyTransform != null ? constraintKeyTransform.rotation : constraintRotationValue;
+    }
+
+}

Assets/HybridIK/Scripts/Main/HybridIKConstraint.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: 945cb199022265a4d879826f1cf92986
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Main/HybridIKConstraint.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Main/HybridIKJoint.cs

@@ -0,0 +1,181 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using AshqarApps.DynamicJoint;
+
+[System.Serializable]
+public class HybridIKJoint
+{
+    public Transform jointTransform;
+    public Quaternion initialRotation;
+    public Vector3 initialPosition;
+    public Vector3 initialPositionWorld;
+    public Vector3 previousPositionWorld;
+
+    public Quaternion cachedInitialRotation;
+    public Vector3 cachedLocalPosition;
+
+
+    public Vector3 targetPosition;
+    public Vector3 targetLocalPosition;
+    public Vector3 stretchedTargetPosition;
+    public bool isStretchNode;
+
+    private Vector3 ikTargetPosition;
+
+    public bool IsEndNode = false;
+
+    public HybridIKJoint child = null;
+    public HybridIKJoint parent = null;
+
+    [HideInInspector]
+    public int childIndex = -1;
+    [HideInInspector]
+    public int parentIndex = -1;
+
+    public float boneLength;
+    public Vector3 toParent;
+
+    public bool oldVecInitialized;
+    public Vector3 oldVec;
+    public Quaternion rotationIK;
+
+    public float jointRadius;
+
+    public DynamicJointLimit jointLimit;
+
+    public bool isPositionConstrained = false;
+    public bool isRotationConstrained = false;
+
+    public bool overrideConstraint;
+    public HybridIKConstraint constraint;
+    public Transform targetIKSpace = null;
+
+    public bool enableKeyframeConstraints = true;
+
+    public Vector3 cachedPosition = Vector3.zero;
+    public Quaternion cachedRotation = Quaternion.identity;
+    public Quaternion targetRotation = Quaternion.identity;
+    public List<Quaternion> keyedRotations;
+    public List<Vector3> keyedPositions;
+    public List<Vector3> keyedLocalPositions;
+
+    public Quaternion zeroRotation = Quaternion.identity;
+    public Vector3 zeroPosition = Vector3.zero;
+
+    public Quaternion lastFrameRotation = Quaternion.identity;
+    public Vector3 lastFramePosition = Vector3.zero;
+    public float currentAngularVelocity = 0;
+    public float currentStretchVelocity = 0;
+
+    public bool enableStretch = false;
+    public float inwardsStretch = 0;
+    public float outwardsStretch = 0;
+    public Vector3 stretchAxis = Vector3.forward;
+    public HybridIKNodeStretchLimits stretchLimits;
+
+    // Angular Motion
+    public bool overrideAngularMotion = false;
+    public float maxAngularMotionVelocity = 30f;
+    public float angularMotionAcceleration = 5f;
+
+    public void ResetToZeroTransforms()
+    {
+        if (parent != null)
+            parent.jointTransform.localRotation = parent.zeroRotation;
+        this.jointTransform.localPosition = zeroPosition;
+    }
+
+    public bool HasIKConstraint()
+    {
+        return ((enableKeyframeConstraints || overrideConstraint)) && constraint != null && constraint.jointTransform != null && (constraint.constrainPosition || constraint.constrainRotation);
+    }
+
+    public bool HasPositionConstraint()
+    {
+        return (enableKeyframeConstraints || overrideConstraint) && constraint != null && constraint.jointTransform != null && constraint.constrainPosition;
+    }
+
+    public bool HasRotationConstraint()
+    {
+        return (enableKeyframeConstraints || overrideConstraint) && constraint != null && constraint.jointTransform != null && constraint.constrainRotation;
+    }
+
+    public HybridIKJoint(Transform t, HybridIKJoint child, float radius = 0.1f)
+    {
+        if (child != null)
+        {
+            child.boneLength = (t.position - child.jointTransform.position).magnitude;
+            child.parent = this;
+            this.child = child;
+        }
+
+        lastFrameRotation = t.localRotation;
+        initialPosition = t.localPosition;
+        initialRotation = t.localRotation;
+        initialPositionWorld = t.position;
+        targetPosition = t.position;
+
+        previousPositionWorld = initialPositionWorld;
+
+        jointTransform = t;
+
+        jointRadius = radius;
+
+        oldVec = Vector3.up;
+
+        keyedRotations = new List<Quaternion>();
+        keyedPositions = new List<Vector3>();
+        keyedLocalPositions = new List<Vector3>();
+
+        this.jointLimit = t.gameObject.GetComponent<DynamicJointLimit>();
+    }
+
+    public Vector3 GetCurrentPositionWorld()
+    {
+        return jointTransform.position;
+    }
+
+    public Vector3 GetInitialPositionWorld()
+    {
+        return initialPositionWorld;
+    }
+}
+
+[System.Serializable]
+public class HybridIKNodeStretchLimits
+{
+    public Transform targetSpace;
+
+    public Vector3 minStretchLimits;
+    public Vector3 maxStretchLimits;
+
+    public float maxForward = 0;
+    public float maxBackward = 0;
+
+    public float maxLeft = 0;
+    public float maxRight = 0;
+
+    public float maxDown = 0;
+    public float maxUp = 0;
+
+    public Vector3 ApplyStretchLimit(Vector3 targetVec, Vector3 localOrigin)
+    {
+        if (targetSpace == null)
+            return Vector3.zero;
+
+        Vector3 currentPos = targetSpace.position;
+        targetSpace.localPosition = localOrigin;
+        Vector3 diff = currentPos - targetSpace.position;
+        targetVec += diff;
+
+        Vector3 vec = targetSpace.InverseTransformDirection(targetVec);
+        vec.x = Mathf.Clamp(vec.x, minStretchLimits.x, maxStretchLimits.x);
+        vec.y = Mathf.Clamp(vec.y, minStretchLimits.y, maxStretchLimits.y);
+        vec.z = Mathf.Clamp(vec.z, minStretchLimits.z, maxStretchLimits.z);
+
+        vec = targetSpace.TransformDirection(vec);
+
+        return vec;
+    }
+}

Assets/HybridIK/Scripts/Main/HybridIKJoint.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: 2022210cf5df74d4dbdb63767a36f578
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Main/HybridIKJoint.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Main/HybridIKStretchJunction.cs

@@ -0,0 +1,8 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class HybridIKStretchJunction : MonoBehaviour
+{
+
+}

Assets/HybridIK/Scripts/Main/HybridIKStretchJunction.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: b8a651a183a8c1443a30fc1d5487ea05
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Main/HybridIKStretchJunction.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Main/HybridInverseKinematicsNode.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: ec18f272cdf3e704196131d48566ee83
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Main/HybridInverseKinematicsNode.cs
+  uploadId: 532533

Assets/HybridIK/Scripts/Main/RobotController.cs

@@ -0,0 +1,144 @@
+using NUnit.Framework;
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Net.Sockets;
+using System.Threading.Tasks;
+using UnityEngine;
+using UnityEngine.UIElements;
+
+public class RobotController : MonoBehaviour
+{
+    [Header("IK")]
+    [SerializeField] private HybridInverseKinematicsNode kinematicsNode;
+
+    [Header("Motor State")]
+    [SerializeField] private GameObject motorStatusIndicator1;
+    [SerializeField] private GameObject motorStatusIndicator2;
+
+    [SerializeField] private Material indicatorMaterial1; // 기본색(회색)
+    [SerializeField] private Material indicatorMaterial2; // 초록
+
+    [SerializeField] public GameObject toolEndpoint;
+    [SerializeField] public Material toolEndpointHighlight;
+    [SerializeField] public Material toolEndpointErrorHighlight;
+    [SerializeField] public Material toolEndpointOriginal;
+
+    [SerializeField] public GameObject testSphere; // 타겟 위치에 표시할 오브젝트
+
+    public event Action OnPoseUpdateRequest;
+    public event Action OnPoseUpdateReceive;
+
+    public Vector3 movementPosition;
+    private Quaternion movementRotation;
+
+    private bool isMotorOn;
+    public bool IsMovementRunning = false;
+
+    void Start()
+    {
+        if (motorStatusIndicator1 != null)
+        {
+            motorStatusIndicator1.GetComponent<MeshRenderer>().material = indicatorMaterial1;
+        }
+        if (motorStatusIndicator2 != null)
+        {
+            motorStatusIndicator2.GetComponent<MeshRenderer>().material = indicatorMaterial1;
+        }
+    }
+
+    private void FixedUpdate()
+    {
+        OnPoseUpdateRequest?.Invoke();
+    }
+
+    private void LateUpdate()
+    {
+        if (kinematicsNode != null && kinematicsNode.enabled == false)
+        {
+            UpdateHandleToEndPoint();
+        }
+    }
+
+    public void SetMotorState(bool isOn)
+    {
+        isMotorOn = isOn;
+
+
+        if (isMotorOn)
+        {
+            if (indicatorMaterial2 != null)
+            {
+                motorStatusIndicator1.GetComponent<MeshRenderer>().material = indicatorMaterial2;
+                motorStatusIndicator2.GetComponent<MeshRenderer>().material = indicatorMaterial2;
+            }
+        }
+        else
+        {
+            if (indicatorMaterial1 != null)
+            {
+                motorStatusIndicator1.GetComponent<MeshRenderer>().material = indicatorMaterial1;
+                motorStatusIndicator2.GetComponent<MeshRenderer>().material = indicatorMaterial1;
+            }
+        }
+    }
+
+    public void EnableIK()
+    {
+        if (kinematicsNode != null) kinematicsNode.enabled = true;
+    }
+
+    public void DisableIK()
+    {
+        if (kinematicsNode != null) kinematicsNode.enabled = false;
+    }
+
+    /*
+    public void SetRobotPosition(RobotData robotData) // 가상 로봇 위치 업데이트
+    {
+        // x, y, z, rx, ry, rz => endpoint값
+        // j1, ..., j6 => 6개 축의 회전값
+        if (robotData == null)
+        {
+            return; // 데이터가 없으면 아무것도 하지 않음
+        }
+
+        List<Quaternion> list_jAngle = new List<Quaternion>();
+
+        list_jAngle.Add(Quaternion.AngleAxis(-1 * robotData.j1, Vector3.forward));
+        list_jAngle.Add(Quaternion.AngleAxis((robotData.j2 - 90), Vector3.up));
+        list_jAngle.Add(Quaternion.AngleAxis(robotData.j3, Vector3.up));
+        list_jAngle.Add(Quaternion.AngleAxis(robotData.j4, Vector3.right));
+        list_jAngle.Add(Quaternion.AngleAxis(robotData.j5, Vector3.up));
+        list_jAngle.Add(Quaternion.AngleAxis(robotData.j6, Vector3.right));
+
+        kinematicsNode.SetJointTargetRotations(list_jAngle);
+    }
+    */
+
+    private void UpdateHandleToEndPoint()
+    {
+        if (kinematicsNode == null || kinematicsNode.targetTransform == null ||
+            kinematicsNode.nodes == null || kinematicsNode.nodes.Count == 0)
+        {
+            return;
+        }
+
+        Transform endPoint = kinematicsNode.nodes[kinematicsNode.nodes.Count - 1].jointTransform;
+        if (endPoint == null) return;
+
+        kinematicsNode.targetTransform.position = endPoint.position;
+        kinematicsNode.targetTransform.rotation = endPoint.rotation;
+    }
+
+    public Transform GetEndPoint()
+    {
+        Transform endPoint = kinematicsNode.nodes[kinematicsNode.nodes.Count - 1].jointTransform;
+        return endPoint;
+    }
+
+    void OnDestroy()
+    {
+        IsMovementRunning = false;
+    }
+}

Assets/HybridIK/Scripts/Main/RobotController.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: c12f4ab377ddfdc46820089b240eaf27

Assets/HybridIK/Scripts/Main/TargetRangeLimiter.cs

@@ -0,0 +1,43 @@
+using UnityEngine;
+
+/// 이 Transform의 위치를 지정된 중심점으로부터 최대 거리를 벗어나지 않도록 제한
+public class TargetRangeLimiter : MonoBehaviour
+{
+    [Header("Range Limit")]
+    [SerializeField] private Transform centerPoint;
+
+    [SerializeField] private float maxDistance = 2.0f;
+
+    // LateUpdate를 사용하여 다른 스크립트에 의해 위치가 변경된 후 최종적으로 위치 보정
+    private void LateUpdate()
+    {
+        if (centerPoint == null)
+        {
+            return;
+        }
+
+        // 1. 중심점에서 현재 핸들 위치까지의 벡터와 거리를 계산
+        Vector3 directionFromCenter = transform.position - centerPoint.position;
+        float currentDistance = directionFromCenter.magnitude;
+
+        // 2. 현재 거리가 최대 거리를 초과했는지 확인
+        if (currentDistance > maxDistance)
+        {
+            // 3. 방향은 유지한 채, 거리를 최대 거리로 제한하는 새로운 위치 계산
+            Vector3 newPosition = centerPoint.position + directionFromCenter.normalized * maxDistance;
+
+            // 4. 핸들의 위치를 계산된 새 위치로 강제 설정
+            transform.position = newPosition;
+        }
+    }
+
+    //// 기즈모를 그려서 작업 범위를 시각적으로 보여줌 (에디터 전용)
+    //private void OnDrawGizmosSelected()
+    //{
+    //    if (centerPoint != null)
+    //    {
+    //        Gizmos.color = Color.green;
+    //        Gizmos.DrawWireSphere(centerPoint.position, maxDistance);
+    //    }
+    //}
+}

Assets/HybridIK/Scripts/Main/TargetRangeLimiter.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: e6dac70e0e196a043852640887710f7b

Assets/HybridIK/Scripts/Utils.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: b509378e1379bb54cbbbb7ce530a5fe9
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/HybridIK/Scripts/Utils/GeometryMathHelperUtils.cs

@@ -0,0 +1,727 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class GeometryMathHelperUtils
+{
+    //Find the line of intersection between two planes.	The planes are defined by a normal and a point on that plane.
+    //The outputs are a point on the line and a vector which indicates it's direction. If the planes are not parallel, 
+    //the function outputs true, otherwise false.
+    public static bool PlanePlaneIntersection(out Vector3 linePoint, out Vector3 lineVec, Vector3 plane1Normal, Vector3 plane1Position, Vector3 plane2Normal, Vector3 plane2Position)
+    {
+
+        linePoint = Vector3.zero;
+        lineVec = Vector3.zero;
+
+        //We can get the direction of the line of intersection of the two planes by calculating the 
+        //cross product of the normals of the two planes. Note that this is just a direction and the line
+        //is not fixed in space yet. We need a point for that to go with the line vector.
+        lineVec = Vector3.Cross(plane1Normal, plane2Normal);
+
+        //Next is to calculate a point on the line to fix it's position in space. This is done by finding a vector from
+        //the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding
+        //errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
+        //the cross product of the normal of plane2 and the lineDirection.		
+        Vector3 ldir = Vector3.Cross(plane2Normal, lineVec);
+
+        float denominator = Vector3.Dot(plane1Normal, ldir);
+
+        //Prevent divide by zero and rounding errors by requiring about 5 degrees angle between the planes.
+        if (Mathf.Abs(denominator) > 0.006f)
+        {
+
+            Vector3 plane1ToPlane2 = plane1Position - plane2Position;
+            float t = Vector3.Dot(plane1Normal, plane1ToPlane2) / denominator;
+            linePoint = plane2Position + t * ldir;
+
+            return true;
+        }
+
+        //output not valid
+        else
+        {
+            return false;
+        }
+    }
+
+    //create a vector of direction "vector" with length "size"
+    public static Vector3 SetVectorLength(Vector3 vector, float size)
+    {
+
+        //normalize the vector
+        Vector3 vectorNormalized = Vector3.Normalize(vector);
+
+        //scale the vector
+        return vectorNormalized *= size;
+    }
+
+    //Get the intersection between a line and a plane. 
+    //If the line and plane are not parallel, the function outputs true, otherwise false.
+    public static bool LinePlaneIntersection(out Vector3 intersection, Vector3 linePoint, Vector3 lineVec, Vector3 planeNormal, Vector3 planePoint)
+    {
+
+        float length;
+        float dotNumerator;
+        float dotDenominator;
+        Vector3 vector;
+        intersection = Vector3.zero;
+
+        //calculate the distance between the linePoint and the line-plane intersection point
+        dotNumerator = Vector3.Dot((planePoint - linePoint), planeNormal);
+        dotDenominator = Vector3.Dot(lineVec, planeNormal);
+
+        //line and plane are not parallel
+        if (dotDenominator != 0.0f)
+        {
+            length = dotNumerator / dotDenominator;
+
+            //create a vector from the linePoint to the intersection point
+            vector = SetVectorLength(lineVec, length);
+
+            //get the coordinates of the line-plane intersection point
+            intersection = linePoint + vector;
+
+            return true;
+        }
+
+        //output not valid
+        else
+        {
+            return false;
+        }
+    }
+
+    //Calculate the intersection point of two lines. Returns true if lines intersect, otherwise false.
+    //Note that in 3d, two lines do not intersect most of the time. So if the two lines are not in the 
+    //same plane, use ClosestPointsOnTwoLines() instead.
+    public static bool LineLineIntersection(out Vector3 intersection, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2)
+    {
+
+        Vector3 lineVec3 = linePoint2 - linePoint1;
+        Vector3 crossVec1and2 = Vector3.Cross(lineVec1, lineVec2);
+        Vector3 crossVec3and2 = Vector3.Cross(lineVec3, lineVec2);
+
+        float planarFactor = Vector3.Dot(lineVec3, crossVec1and2);
+
+        //is coplanar, and not parrallel
+        if (Mathf.Abs(planarFactor) < 0.0001f && crossVec1and2.sqrMagnitude > 0.0001f)
+        {
+            float s = Vector3.Dot(crossVec3and2, crossVec1and2) / crossVec1and2.sqrMagnitude;
+            intersection = linePoint1 + (lineVec1 * s);
+            return true;
+        }
+        else
+        {
+            intersection = Vector3.zero;
+            return false;
+        }
+    }
+
+    //Two non-parallel lines which may or may not touch each other have a point on each line which are closest
+    //to each other. This function finds those two points. If the lines are not parallel, the function 
+    //outputs true, otherwise false.
+    public static bool ClosestPointsOnTwoLines(out Vector3 closestPointLine1, out Vector3 closestPointLine2, Vector3 linePoint1, Vector3 lineVec1, Vector3 linePoint2, Vector3 lineVec2)
+    {
+
+        closestPointLine1 = Vector3.zero;
+        closestPointLine2 = Vector3.zero;
+
+        float a = Vector3.Dot(lineVec1, lineVec1);
+        float b = Vector3.Dot(lineVec1, lineVec2);
+        float e = Vector3.Dot(lineVec2, lineVec2);
+
+        float d = a * e - b * b;
+
+        //lines are not parallel
+        if (d != 0.0f)
+        {
+
+            Vector3 r = linePoint1 - linePoint2;
+            float c = Vector3.Dot(lineVec1, r);
+            float f = Vector3.Dot(lineVec2, r);
+
+            float s = (b * f - c * e) / d;
+            float t = (a * f - c * b) / d;
+
+            closestPointLine1 = linePoint1 + lineVec1 * s;
+            closestPointLine2 = linePoint2 + lineVec2 * t;
+
+            return true;
+        }
+
+        else
+        {
+            return false;
+        }
+    }
+
+    //This function returns a point which is a projection from a point to a line.
+    //The line is regarded infinite. If the line is finite, use ProjectPointOnLineSegment() instead.
+    public static Vector3 ProjectPointOnLine(Vector3 linePoint, Vector3 lineVec, Vector3 point)
+    {
+
+        //get vector from point on line to point in space
+        Vector3 linePointToPoint = point - linePoint;
+
+        float t = Vector3.Dot(linePointToPoint, lineVec);
+
+        return linePoint + lineVec * t;
+    }
+
+    //This function returns a point which is a projection from a point to a line segment.
+    //If the projected point lies outside of the line segment, the projected point will 
+    //be clamped to the appropriate line edge.
+    //If the line is infinite instead of a segment, use ProjectPointOnLine() instead.
+    public static Vector3 ProjectPointOnLineSegment(Vector3 linePoint1, Vector3 linePoint2, Vector3 point)
+    {
+        Vector3 projectedP = Vector3.Project(point - linePoint1, linePoint2 - linePoint1) + linePoint1;
+
+        float lineD = Vector3.Distance(linePoint1, linePoint2);
+        float d1 = Vector3.Distance(linePoint1, projectedP);
+        float d2 = Vector3.Distance(linePoint2, projectedP);
+
+        // On the segment
+        if (d1 < lineD && d2 < lineD)
+            return projectedP;
+
+        // not on segment, evaluate against points instead
+        if (Vector3.Distance(point, linePoint1) < Vector3.Distance(point, linePoint2))
+            return linePoint1;
+        else
+            return linePoint2;
+    }
+
+    //This function finds out on which side of a line segment the point is located.
+    //The point is assumed to be on a line created by linePoint1 and linePoint2. If the point is not on
+    //the line segment, project it on the line using ProjectPointOnLine() first.
+    //Returns 0 if point is on the line segment.
+    //Returns 1 if point is outside of the line segment and located on the side of linePoint1.
+    //Returns 2 if point is outside of the line segment and located on the side of linePoint2.
+    public static int PointOnWhichSideOfLineSegment(Vector3 linePoint1, Vector3 linePoint2, Vector3 point)
+    {
+
+        Vector3 lineVec = linePoint2 - linePoint1;
+        Vector3 pointVec = point - linePoint1;
+
+        float dot = Vector3.Dot(pointVec, lineVec);
+
+        //point is on side of linePoint2, compared to linePoint1
+        if (dot > 0)
+        {
+
+            //point is on the line segment
+            if (pointVec.magnitude <= lineVec.magnitude)
+            {
+
+                return 0;
+            }
+
+            //point is not on the line segment and it is on the side of linePoint2
+            else
+            {
+
+                return 2;
+            }
+        }
+
+        //Point is not on side of linePoint2, compared to linePoint1.
+        //Point is not on the line segment and it is on the side of linePoint1.
+        else
+        {
+
+            return 1;
+        }
+    }
+
+    public static Vector3 ProjectPointOnTriangle (Vector3 A, Vector3 B, Vector3 C, Vector3 sourceP)
+    {
+        // Compute vectors        
+        Vector3 v0 = C - A;
+        Vector3 v1 = B - A;
+
+        Vector3 projectedP = Vector3.ProjectOnPlane(sourceP - A, Vector3.Cross(v0.normalized, v1.normalized)) + A;
+        Vector3 v2 = projectedP - A;
+
+        // Compute dot products
+        float dot00 = Vector3.Dot(v0, v0);
+        float dot01 = Vector3.Dot(v0, v1);
+        float dot02 = Vector3.Dot(v0, v2);
+        float dot11 = Vector3.Dot(v1, v1);
+        float dot12 = Vector3.Dot(v1, v2);
+
+        // Compute barycentric coordinates
+        float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
+        float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+        float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+        // Check if point is in triangle
+        if ((u >= 0) && (v >= 0) && (u + v < 1))
+        {
+            return A + (u * v0) + (v * v1);
+        }
+
+        // Evaluate closest point to triangle linesegments
+        Vector3 p1 = ProjectPointOnLineSegment(A, B, projectedP);
+        Vector3 p2 = ProjectPointOnLineSegment(A, C, projectedP);
+        Vector3 p3 = ProjectPointOnLineSegment(C, B, projectedP);
+
+        float d1 = Vector3.Distance(projectedP, p1);
+        float d2 = Vector3.Distance(projectedP, p2);
+        float d3 = Vector3.Distance(projectedP, p3);
+
+        if (d1 <= d2 && d1 <= d3) return p1;
+        if (d2 <= d1 && d2 <= d3) return p2;
+        if (d3 <= d2 && d3 <= d1) return p3;
+
+        return projectedP;
+    }
+
+    public static Vector3 FindAveragePoint(List<Vector3> points)
+    {
+        Vector3 avgPoint = Vector3.zero;
+        foreach (Vector3 p in points)
+        {
+            avgPoint += p;
+        }
+        avgPoint /= points.Count;
+
+        return avgPoint;
+    }
+
+    public static Vector3 FindClosestPointToConvexHull(List<Vector3> hullPoints, List<int> hullTris, Vector3 targetPoint)
+    {
+        if (hullPoints == null || hullPoints.Count < 3 || hullTris == null || hullTris.Count < 2)
+            return targetPoint;
+
+        float closestD = Mathf.Infinity;
+        Vector3 closestProjection = targetPoint;
+
+        for(int t = 0; t < hullTris.Count; t += 3)
+        {
+            Vector3 v1 = hullPoints[hullTris[t]];
+            Vector3 v2 = hullPoints[hullTris[t + 1]];
+            Vector3 v3 = hullPoints[hullTris[t + 2]];
+
+            Vector3 projectedP = ProjectPointOnTriangle(v1, v2, v3, targetPoint);
+            float d = Vector3.Distance(projectedP, targetPoint);
+            if (Vector3.Distance(projectedP, targetPoint) < closestD)
+            {
+                d = closestD;
+                closestProjection = projectedP; 
+            }
+        }
+
+        return closestProjection;
+    }
+
+    public static Vector3 ProjectPointOnConvexHull(List<Vector3> hullPoints, List<int> hullTris, Vector3 targetPoint, out bool isInside)
+    {
+        isInside = false;
+        if (hullPoints == null || hullPoints.Count < 3 || hullTris == null || hullTris.Count < 2)
+            return targetPoint;
+
+        Vector3 origin = FindAveragePoint(hullPoints);
+        Vector3 dir = targetPoint - origin;
+        Ray ray = new Ray(origin, dir);
+
+        Vector3 intersectionPoint;
+
+        for (int t = 0; t < hullTris.Count; t += 3)
+        {
+            Vector3 v1 = hullPoints[hullTris[t]];
+            Vector3 v2 = hullPoints[hullTris[t + 1]];
+            Vector3 v3 = hullPoints[hullTris[t + 2]];
+
+            if (RayTriangleIntersection(v1, v2, v3, ray, out intersectionPoint))
+            {
+                //Debug.DrawLine(origin, intersectionPoint);
+                if (Vector3.Distance(origin, intersectionPoint) < Vector3.Distance(origin, targetPoint))
+                {
+                    return intersectionPoint;
+                }
+                else
+                {
+                    isInside = true;
+                    return targetPoint;
+                }
+            }
+        }
+
+        return targetPoint;
+    }
+
+    /// <summary>
+    /// Checks if the specified ray hits the triagnlge descibed by p1, p2 and p3.
+    /// Möller–Trumbore ray-triangle intersection algorithm implementation.
+    /// </summary>
+    /// <param name="p1">Vertex 1 of the triangle.</param>
+    /// <param name="p2">Vertex 2 of the triangle.</param>
+    /// <param name="p3">Vertex 3 of the triangle.</param>
+    /// <param name="ray">The ray to test hit for.</param>
+    /// <returns><c>true</c> when the ray hits the triangle, otherwise <c>false</c></returns>
+    public static bool RayTriangleIntersection(Vector3 p1, Vector3 p2, Vector3 p3, Ray ray, out Vector3 intersectionPoint)
+    {
+        intersectionPoint = ray.origin;
+
+        // Vectors from p1 to p2/p3 (edges)
+        Vector3 e1, e2;
+
+        Vector3 p, q, t;
+        float det, invDet, u, v;
+
+
+        //Find vectors for two edges sharing vertex/point p1
+        e1 = p2 - p1;
+        e2 = p3 - p1;
+
+        // calculating determinant 
+        p = Vector3.Cross(ray.direction, e2);
+
+        //Calculate determinat
+        det = Vector3.Dot(e1, p);
+
+        //if determinant is near zero, ray lies in plane of triangle otherwise not
+        if (det > - Mathf.Epsilon && det < Mathf.Epsilon) { return false; }
+        invDet = 1.0f / det;
+
+        //calculate distance from p1 to ray origin
+        t = ray.origin - p1;
+
+        //Calculate u parameter
+        u = Vector3.Dot(t, p) * invDet;
+
+        //Check for ray hit
+        if (u < 0 || u > 1) { return false; }
+
+        //Prepare to test v parameter
+        q = Vector3.Cross(t, e1);
+
+        //Calculate v parameter
+        v = Vector3.Dot(ray.direction, q) * invDet;
+
+        //Check for ray hit
+        if (v < 0 || u + v > 1) { return false; }
+
+        if ((Vector3.Dot(e2, q) * invDet) > Mathf.Epsilon)
+        {
+            intersectionPoint = p1 + (u * e1) + (v * e2);
+            //ray does intersect
+            return true;
+        }
+
+        // No hit at all
+        return false;
+    }
+
+ 
+
+    private static void Sort(Vector2 v)
+    {
+        if (v.x > v.y)
+        {
+            float c;
+            c = v.x;
+            v.x = v.y;
+            v.y = c;
+        }
+    }
+
+    /// <summary>
+    /// This edge to edge test is based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202 
+    /// </summary>
+    private static bool EdgeEdgeTest(Vector3 v0, Vector3 v1, Vector3 u0, Vector3 u1, int i0, int i1)
+    {
+        float Ax, Ay, Bx, By, Cx, Cy, e, d, f;
+        Ax = v1[i0] - v0[i0];
+        Ay = v1[i1] - v0[i1];
+
+        Bx = u0[i0] - u1[i0];
+        By = u0[i1] - u1[i1];
+        Cx = v0[i0] - u0[i0];
+        Cy = v0[i1] - u0[i1];
+        f = Ay * Bx - Ax * By;
+        d = By * Cx - Bx * Cy;
+        if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
+        {
+            e = Ax * Cy - Ay * Cx;
+            if (f > 0)
+            {
+                if (e >= 0 && e <= f) { return true; }
+            }
+            else
+            {
+                if (e <= 0 && e >= f) { return true; }
+            }
+        }
+
+        return false;
+    }
+
+    private static bool EdgeAgainstTriEdges(Vector3 v0, Vector3 v1, Vector3 u0, Vector3 u1, Vector3 u2, short i0, short i1)
+    {
+        // test edge u0,u1 against v0,v1
+        if (EdgeEdgeTest(v0, v1, u0, u1, i0, i1)) { return true; }
+
+        // test edge u1,u2 against v0,v1 
+        if (EdgeEdgeTest(v0, v1, u1, u2, i0, i1)) { return true; }
+
+        // test edge u2,u1 against v0,v1 
+        if (EdgeEdgeTest(v0, v1, u2, u0, i0, i1)) { return true; }
+
+        return false;
+    }
+
+    private static bool PointInTri(Vector3 v0, Vector3 u0, Vector3 u1, Vector3 u2, short i0, short i1)
+    {
+        float a, b, c, d0, d1, d2;
+
+        // is T1 completly inside T2?
+        // check if v0 is inside tri(u0,u1,u2)
+        a = u1[i1] - u0[i1];
+        b = -(u1[i0] - u0[i0]);
+        c = -a * u0[i0] - b * u0[i1];
+        d0 = a * v0[i0] + b * v0[i1] + c;
+
+        a = u2[i1] - u1[i1];
+        b = -(u2[i0] - u1[i0]);
+        c = -a * u1[i0] - b * u1[i1];
+        d1 = a * v0[i0] + b * v0[i1] + c;
+
+        a = u0[i1] - u2[i1];
+        b = -(u0[i0] - u2[i0]);
+        c = -a * u2[i0] - b * u2[i1];
+        d2 = a * v0[i0] + b * v0[i1] + c;
+
+        if (d0 * d1 > 0.0f)
+        {
+            if (d0 * d2 > 0.0f) { return true; }
+        }
+
+        return false;
+    }
+
+    private static bool TriTriCoplanar(Vector3 N, Vector3 v0, Vector3 v1, Vector3 v2, Vector3 u0, Vector3 u1, Vector3 u2)
+    {
+        float[] A = new float[3];
+        short i0, i1;
+
+        // first project onto an axis-aligned plane, that maximizes the area
+        // of the triangles, compute indices: i0,i1. 
+        A[0] = Mathf.Abs(N[0]);
+        A[1] = Mathf.Abs(N[1]);
+        A[2] = Mathf.Abs(N[2]);
+        if (A[0] > A[1])
+        {
+            if (A[0] > A[2])
+            {
+                // A[0] is greatest
+                i0 = 1;
+                i1 = 2;
+            }
+            else
+            {
+                // A[2] is greatest
+                i0 = 0;
+                i1 = 1;
+            }
+        }
+        else
+        {
+            if (A[2] > A[1])
+            {
+                // A[2] is greatest 
+                i0 = 0;
+                i1 = 1;
+            }
+            else
+            {
+                // A[1] is greatest 
+                i0 = 0;
+                i1 = 2;
+            }
+        }
+
+        // test all edges of triangle 1 against the edges of triangle 2 
+        if (EdgeAgainstTriEdges(v0, v1, u0, u1, u2, i0, i1)) { return true; }
+        if (EdgeAgainstTriEdges(v1, v2, u0, u1, u2, i0, i1)) { return true; }
+        if (EdgeAgainstTriEdges(v2, v0, u0, u1, u2, i0, i1)) { return true; }
+
+        // finally, test if tri1 is totally contained in tri2 or vice versa 
+        if (PointInTri(v0, u0, u1, u2, i0, i1)) { return true; }
+        if (PointInTri(u0, v0, v1, v2, i0, i1)) { return true; }
+
+        return false;
+    }
+
+
+
+    private static bool ComputeIntervals(float VV0, float VV1, float VV2,
+                              float D0, float D1, float D2, float D0D1, float D0D2,
+                              ref float A, ref float B, ref float C, ref float X0, ref float X1)
+    {
+        if (D0D1 > 0.0f)
+        {
+            // here we know that D0D2<=0.0 
+            // that is D0, D1 are on the same side, D2 on the other or on the plane 
+            A = VV2; B = (VV0 - VV2) * D2; C = (VV1 - VV2) * D2; X0 = D2 - D0; X1 = D2 - D1;
+        }
+        else if (D0D2 > 0.0f)
+        {
+            // here we know that d0d1<=0.0 
+            A = VV1; B = (VV0 - VV1) * D1; C = (VV2 - VV1) * D1; X0 = D1 - D0; X1 = D1 - D2;
+        }
+        else if (D1 * D2 > 0.0f || D0 != 0.0f)
+        {
+            // here we know that d0d1<=0.0 or that D0!=0.0 
+            A = VV0; B = (VV1 - VV0) * D0; C = (VV2 - VV0) * D0; X0 = D0 - D1; X1 = D0 - D2;
+        }
+        else if (D1 != 0.0f)
+        {
+            A = VV1; B = (VV0 - VV1) * D1; C = (VV2 - VV1) * D1; X0 = D1 - D0; X1 = D1 - D2;
+        }
+        else if (D2 != 0.0f)
+        {
+            A = VV2; B = (VV0 - VV2) * D2; C = (VV1 - VV2) * D2; X0 = D2 - D0; X1 = D2 - D1;
+        }
+        else
+        {
+            return true;
+        }
+
+        return false;
+    }
+
+    /// <summary>
+    /// Checks if the triangle V(v0, v1, v2) intersects the triangle U(u0, u1, u2).
+    /// </summary>
+    /// <param name="v0">Vertex 0 of V</param>
+    /// <param name="v1">Vertex 1 of V</param>
+    /// <param name="v2">Vertex 2 of V</param>
+    /// <param name="u0">Vertex 0 of U</param>
+    /// <param name="u1">Vertex 1 of U</param>
+    /// <param name="u2">Vertex 2 of U</param>
+    /// <returns>Returns <c>true</c> if V intersects U, otherwise <c>false</c></returns>
+    public static bool TriTriIntersect(Vector3 v0, Vector3 v1, Vector3 v2, Vector3 u0, Vector3 u1, Vector3 u2)
+    {
+        Vector3 e1, e2;
+        Vector3 n1, n2;
+        Vector3 dd;
+        Vector2 isect1 = Vector2.zero, isect2 = Vector2.zero;
+
+        float du0, du1, du2, dv0, dv1, dv2, d1, d2;
+        float du0du1, du0du2, dv0dv1, dv0dv2;
+        float vp0, vp1, vp2;
+        float up0, up1, up2;
+        float bb, cc, max;
+
+        short index;
+
+        // compute plane equation of triangle(v0,v1,v2) 
+        e1 = v1 - v0;
+        e2 = v2 - v0;
+        n1 = Vector3.Cross(e1, e2);
+        d1 = -Vector3.Dot(n1, v0);
+        // plane equation 1: N1.X+d1=0 */
+
+        // put u0,u1,u2 into plane equation 1 to compute signed distances to the plane
+        du0 = Vector3.Dot(n1, u0) + d1;
+        du1 = Vector3.Dot(n1, u1) + d1;
+        du2 = Vector3.Dot(n1, u2) + d1;
+
+        // coplanarity robustness check 
+        if (Mathf.Abs(du0) < Mathf.Epsilon) { du0 = 0.0f; }
+        if (Mathf.Abs(du1) < Mathf.Epsilon) { du1 = 0.0f; }
+        if (Mathf.Abs(du2) < Mathf.Epsilon) { du2 = 0.0f; }
+
+        du0du1 = du0 * du1;
+        du0du2 = du0 * du2;
+
+        // same sign on all of them + not equal 0 ? 
+        if (du0du1 > 0.0f && du0du2 > 0.0f)
+        {
+            // no intersection occurs
+            return false;
+        }
+
+        // compute plane of triangle (u0,u1,u2)
+        e1 = u1 - u0;
+        e2 = u2 - u0;
+        n2 = Vector3.Cross(e1, e2);
+        d2 = -Vector3.Dot(n2, u0);
+
+        // plane equation 2: N2.X+d2=0 
+        // put v0,v1,v2 into plane equation 2
+        dv0 = Vector3.Dot(n2, v0) + d2;
+        dv1 = Vector3.Dot(n2, v1) + d2;
+        dv2 = Vector3.Dot(n2, v2) + d2;
+
+        if (Mathf.Abs(dv0) < Mathf.Epsilon) { dv0 = 0.0f; }
+        if (Mathf.Abs(dv1) < Mathf.Epsilon) { dv1 = 0.0f; }
+        if (Mathf.Abs(dv2) < Mathf.Epsilon) { dv2 = 0.0f; }
+
+
+        dv0dv1 = dv0 * dv1;
+        dv0dv2 = dv0 * dv2;
+
+        // same sign on all of them + not equal 0 ? 
+        if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
+        {
+            // no intersection occurs
+            return false;
+        }
+
+        // compute direction of intersection line 
+        dd = Vector3.Cross(n1, n2);
+
+        // compute and index to the largest component of D 
+        max = (float)Mathf.Abs(dd[0]);
+        index = 0;
+        bb = (float)Mathf.Abs(dd[1]);
+        cc = (float)Mathf.Abs(dd[2]);
+        if (bb > max) { max = bb; index = 1; }
+        if (cc > max) { max = cc; index = 2; }
+
+        // this is the simplified projection onto L
+        vp0 = v0[index];
+        vp1 = v1[index];
+        vp2 = v2[index];
+
+        up0 = u0[index];
+        up1 = u1[index];
+        up2 = u2[index];
+
+        // compute interval for triangle 1 
+        float a = 0, b = 0, c = 0, x0 = 0, x1 = 0;
+        if (ComputeIntervals(vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, ref a, ref b, ref c, ref x0, ref x1))
+        {
+            return TriTriCoplanar(n1, v0, v1, v2, u0, u1, u2);
+        }
+
+        // compute interval for triangle 2 
+        float d = 0, e = 0, f = 0, y0 = 0, y1 = 0;
+        if (ComputeIntervals(up0, up1, up2, du0, du1, du2, du0du1, du0du2, ref d, ref e, ref f, ref y0, ref y1))
+        {
+            return TriTriCoplanar(n1, v0, v1, v2, u0, u1, u2);
+        }
+
+        float xx, yy, xxyy, tmp;
+        xx = x0 * x1;
+        yy = y0 * y1;
+        xxyy = xx * yy;
+
+        tmp = a * xxyy;
+        isect1[0] = tmp + b * x1 * yy;
+        isect1[1] = tmp + c * x0 * yy;
+
+        tmp = d * xxyy;
+        isect2[0] = tmp + e * xx * y1;
+        isect2[1] = tmp + f * xx * y0;
+
+        Sort(isect1);
+        Sort(isect2);
+
+        return !(isect1[1] < isect2[0] || isect2[1] < isect1[0]);
+    }
+}
+ 
+

Assets/HybridIK/Scripts/Utils/GeometryMathHelperUtils.cs.meta

@@ -0,0 +1,18 @@
+fileFormatVersion: 2
+guid: 128a79f08e4a2d8418595cce65d7e047
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
+AssetOrigin:
+  serializedVersion: 1
+  productId: 174069
+  packageName: Hybrid IK
+  packageVersion: 1.02
+  assetPath: Assets/HybridIK/Scripts/Utils/GeometryMathHelperUtils.cs
+  uploadId: 532533