Skip to content

Instantly share code, notes, and snippets.

@hvent90
Last active February 25, 2019 00:39
Show Gist options
  • Save hvent90/d191a02254b87ccb325b9296b2e1065d to your computer and use it in GitHub Desktop.
Save hvent90/d191a02254b87ccb325b9296b2e1065d to your computer and use it in GitHub Desktop.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace IK_V3
{
public class Fabrik : MonoBehaviour
{
public Transform rootSubject;
public Transform target;
public float EPS = 0.05f;
[Header("Debug")]
public bool showConstraints = true;
public bool showLimbs = true;
private Joint[] joints = new Joint[0];
private Limb[] limbs = new Limb[0];
private Vector3 rootPosition;
void Start()
{
Transform[] subjects = rootSubject.GetComponentsInChildren<Transform>();
// Create joints
joints = new Joint[subjects.Length];
for (int i = 0; i < joints.Length; i++)
{
if (i == subjects.Length - 1)
{
joints[i] = JointData.CreateJoint(subjects[i], target, true);
break;
}
joints[i] = JointData.CreateJoint(subjects[i], subjects[i + 1]);
}
// Create limbs
limbs = new Limb[joints.Length - 1];
for (int i = 0; i < limbs.Length; i++)
{
limbs[i] = new Limb(joints[i], joints[i + 1]);
}
for (int i = 0; i < limbs.Length; i++)
{
if (i == limbs.Length - 1 && i > 0)
{
limbs[i].inboardLimb = limbs[i - 1];
continue;
}
if (i == 0 && limbs.Length > 0)
{
limbs[i].outboardLimb = limbs[i + 1];
continue;
}
limbs[i].inboardLimb = limbs[i - 1];
limbs[i].outboardLimb = limbs[i + 1];
}
}
void Update()
{
rootPosition = joints[0].transform.position;
int i = 0;
float dist = Vector3.Distance(EndEffectorPosition(), GoalPosition());
while (Mathf.Abs(dist) > EPS && i < 50)
{
FinalToRoot();
RootToFinal();
dist = Vector3.Distance(EndEffectorPosition(), GoalPosition());
i++;
}
}
void FinalToRoot()
{
Vector3 currentGoal = GoalPosition();
Limb currentLimb = FinalLimb();
currentLimb.outboardJoint.desiredPosition = currentGoal;
while (currentLimb != null)
{
Vector3 dir = Vector3.Normalize(
currentLimb.InboardPosition() - currentLimb.OutboardPosition(true));
currentLimb.inboardJoint.desiredPosition =
currentLimb.OutboardPosition(true) + (dir * currentLimb.limbLength);
currentLimb = currentLimb.inboardLimb;
}
}
void RootToFinal()
{
Limb currentLimb = RootLimb();
while (currentLimb != null)
{
// Get the next position
Vector3 dir = Vector3.Normalize(
currentLimb.OutboardPosition(true) - currentLimb.InboardPosition());
Vector3 nextPos = currentLimb.InboardPosition(true) + (dir * currentLimb.limbLength);
// Get the angle between the inboard joint's position
// and the outboard joint's desired position
Quaternion desiredRotation = Quaternion.LookRotation(
nextPos - currentLimb.InboardPosition());
currentLimb.inboardJoint.SetRotation(desiredRotation);
// Go to next limb
currentLimb = currentLimb.outboardLimb;
}
}
Vector3 EndEffectorPosition()
{
return joints[joints.Length - 1].transform.position;
}
Vector3 GoalPosition()
{
return target.position;
}
Limb RootLimb()
{
return limbs[0];
}
Limb FinalLimb()
{
return limbs[limbs.Length - 1];
}
private void OnDrawGizmos()
{
foreach (Joint joint in joints)
{
joint.drawLinesForConstraints = showConstraints;
}
if (!showLimbs)
return;
Gizmos.color = Color.green;
for (int i = 0; i < limbs.Length; i++)
{
Gizmos.DrawLine(limbs[i].InboardPosition(), limbs[i].OutboardPosition());
}
}
}
}
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace IK_V3
{
public class Joint : MonoBehaviour
{
[Header("Debug")]
public bool drawLinesForConstraints = true;
public float lineLength = 0.5f;
public bool debugRotation = false;
[Header("Constraints")]
public float maxPositiveX = 45;
public float maxNegativeX = 45;
public float maxPositiveY = 10;
public float maxNegativeY = 10;
public float maxPositiveZ = 10;
public float maxNegativeZ = 10;
[Header("Rotation")]
public float rotateX = 0f;
public float rotateY = 0f;
public float rotateZ = 0f;
// The next attempted position
public Vector3 desiredPosition;
public Quaternion initialLocalRotation;
public Quaternion initialWorldRotation;
// Start is called before the first frame update
void Start()
{
initialLocalRotation = transform.localRotation;
initialWorldRotation = transform.rotation;
}
// Update is called once per frame
void Update()
{
if (debugRotation)
SetAngle(new Vector3(rotateX, rotateY, rotateZ), true);
}
public void SetRotation(Quaternion worldRotation)
{
SetRotation(worldRotation, false);
}
public void SetRotation(Quaternion worldRotation, bool overrideDebug)
{
if (debugRotation && !overrideDebug)
return;
Quaternion localRotation = (
Quaternion.Inverse(initialLocalRotation) *
(transform.localRotation *
(Quaternion.Inverse(transform.rotation) * worldRotation)));
Vector3 angles = localRotation.eulerAngles;
angles.x = (angles.x > 180) ? angles.x - 360 : angles.x;
angles.y = (angles.y > 180) ? angles.y - 360 : angles.y;
angles.z = (angles.z > 180) ? angles.z - 360 : angles.z;
float x = Mathf.Clamp(angles.x, -maxNegativeX, maxPositiveX);
float y = Mathf.Clamp(angles.y, -maxNegativeY, maxPositiveY);
float z = Mathf.Clamp(angles.z, -maxNegativeZ, maxPositiveZ);
transform.localRotation = initialLocalRotation * Quaternion.Euler(x, y, z);
}
public void SetAngle(Vector3 angles)
{
SetAngle(angles, false);
}
public void SetAngle(Vector3 angles, bool overrideDebug)
{
if (debugRotation && !overrideDebug)
return;
float x = angles.x;
if (x > maxPositiveX)
x = maxPositiveX;
if (x < -maxNegativeX)
x = -maxNegativeX;
float y = angles.y;
if (y > maxPositiveY)
y = maxPositiveY;
if (y < -maxNegativeY)
y = -maxNegativeY;
float z = angles.z;
if (z > maxPositiveZ)
z = maxPositiveZ;
if (z < -maxNegativeZ)
z = -maxNegativeZ;
transform.localRotation = initialLocalRotation * Quaternion.Euler(x, y, z);
}
Quaternion RotationOffset()
{
return Quaternion.Inverse(transform.localRotation) * initialLocalRotation;
}
Vector3 MaxPositiveDirection(float distance)
{
return Quaternion.Euler(maxPositiveX, maxPositiveY, maxPositiveZ) * RotationOffset() * (transform.forward * distance);
}
Vector3 MaxNegativeDirection(float distance)
{
return Quaternion.Euler(-maxNegativeX, -maxNegativeY, -maxNegativeZ) * RotationOffset() * (transform.forward * distance);
}
private void OnDrawGizmos()
{
if (!drawLinesForConstraints)
return;
Vector3 start = transform.position;
Draw(
start,
start + MaxPositiveDirection(lineLength),
Color.cyan);
Draw(
start,
start + MaxNegativeDirection(lineLength),
Color.magenta);
}
private void Draw(Vector3 start, Vector3 end, Color color)
{
Gizmos.color = color;
Gizmos.DrawLine(start, end);
}
}
}
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace IK_V3
{
public class JointData : MonoBehaviour
{
[Header("Debug")]
public bool drawLinesForConstraints = true;
public float lineLength = 0.5f;
public bool forceRotation = true;
[Header("Constraints")]
public float maxPositiveX = 45;
public float maxNegativeX = 45;
public float maxPositiveY = 10;
public float maxNegativeY = 10;
public float maxPositiveZ = 10;
public float maxNegativeZ = 10;
[Header("Rotation")]
public float rotateX = 0f;
public float rotateY = 0f;
public float rotateZ = 0f;
public static Joint CreateJoint(Transform subject, Transform target)
{
return CreateJoint(subject, target, false);
}
public static Joint CreateJoint(Transform subject, Transform target, bool isEndEffector)
{
GameObject obj = new GameObject("Joint");
obj.transform.parent = subject.parent;
Joint joint = obj.AddComponent<Joint>();
JointData jointData = subject.GetComponent<JointData>();
joint.drawLinesForConstraints = jointData.drawLinesForConstraints;
joint.lineLength = jointData.lineLength;
joint.debugRotation = jointData.forceRotation;
joint.maxPositiveX = jointData.maxPositiveX;
joint.maxNegativeX = jointData.maxNegativeX;
joint.maxPositiveY = jointData.maxPositiveY;
joint.maxNegativeY = jointData.maxNegativeY;
joint.maxPositiveZ = jointData.maxPositiveZ;
joint.maxNegativeZ = jointData.maxNegativeZ;
joint.transform.position = subject.transform.position;
joint.transform.LookAt(target.position);
subject.parent = joint.transform;
if (!isEndEffector)
{
target.parent = joint.transform;
}
return joint;
}
}
}
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace IK_V3
{
public class Limb
{
public Limb inboardLimb;
public Limb outboardLimb;
public Joint inboardJoint;
public Joint outboardJoint;
public float limbLength;
public Limb(Joint inJoint, Joint outJoint)
{
inboardJoint = inJoint;
outboardJoint = outJoint;
limbLength = Vector3.Distance(OutboardPosition(), InboardPosition());
}
public Vector3 InboardPosition()
{
return InboardPosition(false);
}
public Vector3 InboardPosition(bool desiredPosition)
{
if (desiredPosition && inboardJoint.desiredPosition != null)
return inboardJoint.desiredPosition;
return inboardJoint.transform.position;
}
public Vector3 OutboardPosition()
{
return OutboardPosition(false);
}
public Vector3 OutboardPosition(bool desiredPosition)
{
if (desiredPosition && outboardJoint.desiredPosition != null)
return outboardJoint.desiredPosition;
return outboardJoint.transform.position;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment