Last active
July 27, 2017 16:22
-
-
Save samloeschen/93134ef7f5178f91f6885cd7534ad547 to your computer and use it in GitHub Desktop.
Simple 3D forward kinematics with easing. Place on any normal Transform and set fkParent in the inspector to the Transform you want it to follow.
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; | |
public class SmoothFKMember : MonoBehaviour { | |
public Transform fkParent; | |
public float rotationSpeed = 10f; | |
private Vector3 initialPositionOffset; | |
private Quaternion initialParentRotation; | |
private Quaternion initialRotation; | |
[SerializeField] | |
private bool _drawDebug = true; | |
private void OnEnable(){ | |
initialPositionOffset = transform.position - fkParent.position; | |
initialRotation = transform.rotation; | |
initialParentRotation = fkParent.rotation; | |
} | |
private void LateUpdate(){ | |
transform.position = GetFKPosition(); | |
transform.rotation = GetFKRotation(); | |
if(_drawDebug){ | |
Quaternion targetQuat = (Quaternion.Inverse(initialParentRotation) * fkParent.rotation).Normalize(); | |
//this to parent | |
Debug.DrawLine(transform.position, fkParent.position, Color.red); | |
//this to target | |
Debug.DrawLine(fkParent.position + targetQuat * initialPositionOffset, fkParent.position, Color.green); | |
} | |
} | |
private Quaternion _currentParentRotation = Quaternion.identity; | |
private Vector3 GetFKPosition(){ | |
Quaternion targetQuat = (Quaternion.Inverse(initialParentRotation) * fkParent.rotation); | |
Quaternion deltaQuat = (Quaternion.Inverse(_currentParentRotation) * targetQuat); | |
Vector3 axis = Vector3.zero; | |
float angle = 0f; | |
deltaQuat.Normalize().ToAngleAxis(out angle, out axis); | |
Quaternion velQuat = Quaternion.AngleAxis(angle * Time.deltaTime * rotationSpeed, axis * Time.deltaTime * rotationSpeed); | |
_currentParentRotation = _currentParentRotation * velQuat; | |
return fkParent.position + _currentParentRotation * initialPositionOffset; | |
} | |
private Quaternion GetFKRotation(){ | |
Quaternion currentQuat = transform.rotation; | |
Quaternion targetQuat = (Quaternion.Inverse(initialParentRotation) * fkParent.rotation); | |
Quaternion targetRelativeToInitial = ((initialRotation) * targetQuat); | |
Quaternion deltaQuat = (Quaternion.Inverse(currentQuat) * targetRelativeToInitial); | |
Vector3 axis = Vector3.zero; | |
float angle = 0f; | |
deltaQuat.Normalize().ToAngleAxis(out angle, out axis); | |
Quaternion velQuat = Quaternion.AngleAxis(angle * Time.deltaTime * rotationSpeed, axis * Time.deltaTime * rotationSpeed); | |
currentQuat = currentQuat * velQuat; | |
return currentQuat; | |
} | |
} | |
public static class QuaternionExtensions{ | |
public static Quaternion Normalize(this Quaternion q){ | |
Quaternion result; | |
float sq = q.x * q.x; | |
sq += q.y * q.y; | |
sq += q.z * q.z; | |
sq += q.w * q.w; | |
//detect badness | |
float inv = 1.0f / Mathf.Sqrt(sq); | |
result.x = q.x * inv; | |
result.y = q.y * inv; | |
result.z = q.z * inv; | |
result.w = q.w * inv; | |
return result; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment