Skip to content

Instantly share code, notes, and snippets.

@samloeschen
Last active July 27, 2017 16:22
Show Gist options
  • Save samloeschen/93134ef7f5178f91f6885cd7534ad547 to your computer and use it in GitHub Desktop.
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.
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