Last active
February 25, 2019 00:39
-
-
Save hvent90/d191a02254b87ccb325b9296b2e1065d to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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()); | |
} | |
} | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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); | |
} | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | |
} | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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