511 lines
20 KiB
C#
511 lines
20 KiB
C#
using System;
|
|
using UnityEngine;
|
|
|
|
namespace FIMSpace.FTools
|
|
{
|
|
|
|
/// <summary>
|
|
/// FC: Class for processing IK logics for multiple bones inverse kinematics
|
|
/// </summary>
|
|
[System.Serializable]
|
|
public class FIK_CCDProcessor : FIK_ProcessorBase
|
|
{
|
|
#region CCDIK Processor
|
|
|
|
public CCDIKBone[] IKBones; // { get; private set; }
|
|
public CCDIKBone StartIKBone { get { return IKBones[0]; } }
|
|
public CCDIKBone EndIKBone { get { return IKBones[IKBones.Length - 1]; } }
|
|
|
|
public bool ContinousSolving = true;
|
|
[Range( 0f, 1f )]
|
|
public float SyncWithAnimator = 1f;
|
|
[Range( 1, 12 )]
|
|
public int ReactionQuality = 2;
|
|
[Range( 0f, 1f )]
|
|
public float Smoothing = 0f;
|
|
[Range( 0f, 1.5f )]
|
|
public float StretchToTarget = 0f;
|
|
public AnimationCurve StretchCurve = AnimationCurve.EaseInOut( 0f, 0f, 1f, 1f );
|
|
|
|
public bool Use2D = false;
|
|
public bool Invert = false;
|
|
|
|
public float ActiveLength { get; private set; }
|
|
|
|
/// <summary> Assigning bones for IK processor with CCD IK logics (unlimited bone count) </summary>
|
|
public FIK_CCDProcessor( Transform[] bonesChain )
|
|
{
|
|
IKBones = new CCDIKBone[bonesChain.Length];
|
|
Bones = new CCDIKBone[IKBones.Length];
|
|
|
|
for( int i = 0; i < bonesChain.Length; i++ )
|
|
{
|
|
IKBones[i] = new CCDIKBone( bonesChain[i] );
|
|
Bones[i] = IKBones[i];
|
|
}
|
|
|
|
IKTargetPosition = EndBone.transform.position; IKTargetRotation = EndBone.transform.rotation;
|
|
}
|
|
|
|
|
|
public override void Init( Transform root )
|
|
{
|
|
if( Initialized ) return;
|
|
|
|
fullLength = 0f;
|
|
for( int i = 0; i < Bones.Length; i++ )
|
|
{
|
|
CCDIKBone b = IKBones[i];
|
|
CCDIKBone child = null, parent = null;
|
|
|
|
if( i > 0 ) parent = IKBones[i - 1];
|
|
if( i < Bones.Length - 1 )
|
|
{
|
|
child = IKBones[i + 1];
|
|
}
|
|
|
|
|
|
if( i < Bones.Length - 1 )
|
|
{
|
|
IKBones[i].Init( child, parent );
|
|
fullLength += b.BoneLength;
|
|
b.ForwardOrientation = Quaternion.Inverse( b.transform.rotation ) * ( IKBones[i + 1].transform.position - b.transform.position );
|
|
}
|
|
else
|
|
{
|
|
IKBones[i].Init( child, parent );
|
|
b.ForwardOrientation = Quaternion.Inverse( b.transform.rotation ) * ( IKBones[IKBones.Length - 1].transform.position - IKBones[0].transform.position );
|
|
}
|
|
}
|
|
|
|
Initialized = true;
|
|
}
|
|
|
|
|
|
#region Methods
|
|
|
|
|
|
/// <summary> Updating processor with n-bones oriented inverse kinematics </summary>
|
|
public override void Update()
|
|
{
|
|
if( !Initialized ) return;
|
|
if( IKWeight <= 0f ) return;
|
|
CCDIKBone wb = IKBones[0];
|
|
|
|
// Restoring previous IK progress for continous solving
|
|
if( ContinousSolving )
|
|
{
|
|
while( wb != null )
|
|
{
|
|
wb.LastKeyLocalRotation = wb.transform.localRotation;
|
|
wb.transform.localPosition = wb.LastIKLocPosition;
|
|
wb.transform.localRotation = wb.LastIKLocRotation;
|
|
wb = wb.IKChild;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( SyncWithAnimator > 0f )
|
|
// Memory for animator syncing
|
|
while( wb != null )
|
|
{
|
|
wb.LastKeyLocalRotation = wb.transform.localRotation;
|
|
wb = wb.IKChild;
|
|
}
|
|
}
|
|
|
|
if( ReactionQuality < 0 ) ReactionQuality = 1;
|
|
|
|
Vector3 goalPivotOffset = Vector3.zero;
|
|
if( ReactionQuality > 1 ) goalPivotOffset = GetGoalPivotOffset();
|
|
|
|
for( int itr = 0; itr < ReactionQuality; itr++ )
|
|
{
|
|
// Restrictions for multiple interations
|
|
if( itr >= 1 )
|
|
if( goalPivotOffset.sqrMagnitude == 0 )
|
|
if( Smoothing > 0 )
|
|
if( GetVelocityDifference() < Smoothing * Smoothing ) break;
|
|
|
|
LastLocalDirection = RefreshLocalDirection();
|
|
|
|
Vector3 ikGoal = IKTargetPosition + goalPivotOffset;
|
|
|
|
// Going in iterations in reversed way, from pre end child to root parent
|
|
wb = IKBones[IKBones.Length - 2];
|
|
|
|
if( !Use2D ) // Full 3D space rotations calculations
|
|
{
|
|
if( !Invert )
|
|
{
|
|
while( wb != null )
|
|
{
|
|
float weight = wb.MotionWeight * IKWeight;
|
|
|
|
if( weight > 0f )
|
|
{
|
|
Quaternion targetRotation = Quaternion.FromToRotation( Bones[Bones.Length - 1].transform.position - wb.transform.position /*fromThisToEndChildBone*/, ikGoal - wb.transform.position /*fromThisToIKGoal*/) * wb.transform.rotation;
|
|
if( weight < 1f ) wb.transform.rotation = Quaternion.Lerp( wb.transform.rotation, targetRotation, weight );
|
|
else wb.transform.rotation = targetRotation;
|
|
}
|
|
|
|
wb.AngleLimiting();
|
|
wb = wb.IKParent;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
while( wb != null )
|
|
{
|
|
wb.AngleLimiting();
|
|
wb = wb.IKParent;
|
|
}
|
|
|
|
wb = IKBones[0];
|
|
|
|
while( wb != null )
|
|
{
|
|
float weight = wb.MotionWeight * IKWeight;
|
|
|
|
if( weight > 0f )
|
|
{
|
|
Quaternion targetRotation = Quaternion.FromToRotation( Bones[Bones.Length - 1].transform.position - wb.transform.position /*fromThisToEndChildBone*/, ikGoal - wb.transform.position /*fromThisToIKGoal*/) * wb.transform.rotation;
|
|
if( weight < 1f ) wb.transform.rotation = Quaternion.Lerp( wb.transform.rotation, targetRotation, weight );
|
|
else wb.transform.rotation = targetRotation;
|
|
}
|
|
|
|
wb = wb.IKChild;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( !Invert )
|
|
{
|
|
// Going in while() loop is 2x faster than for(i;i;i;) when there is more iterations
|
|
while( wb != null )
|
|
{
|
|
float weight = wb.MotionWeight * IKWeight;
|
|
|
|
if( weight > 0f )
|
|
{
|
|
Vector3 fromThisToEndChildBone = Bones[Bones.Length - 1].transform.position - wb.transform.position;
|
|
Vector3 fromThisToIKGoal = ikGoal - wb.transform.position;
|
|
wb.transform.rotation = Quaternion.AngleAxis( Mathf.DeltaAngle( Mathf.Atan2( fromThisToEndChildBone.x, fromThisToEndChildBone.y ) * Mathf.Rad2Deg /* Angle to last bone */, Mathf.Atan2( fromThisToIKGoal.x, fromThisToIKGoal.y ) * Mathf.Rad2Deg /* Angle to goal position */) * weight, Vector3.back ) * wb.transform.rotation;
|
|
}
|
|
|
|
wb.AngleLimiting();
|
|
wb = wb.IKParent;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
while( wb != null )
|
|
{
|
|
wb.AngleLimiting();
|
|
wb = wb.IKParent;
|
|
}
|
|
|
|
wb = IKBones[0];
|
|
|
|
while( wb != null )
|
|
{
|
|
float weight = wb.MotionWeight * IKWeight;
|
|
|
|
if( weight > 0f )
|
|
{
|
|
Vector3 fromThisToEndChildBone = Bones[Bones.Length - 1].transform.position - wb.transform.position;
|
|
Vector3 fromThisToIKGoal = ikGoal - wb.transform.position;
|
|
wb.transform.rotation = Quaternion.AngleAxis( Mathf.DeltaAngle( Mathf.Atan2( fromThisToEndChildBone.x, fromThisToEndChildBone.y ) * Mathf.Rad2Deg /* Angle to last bone */, Mathf.Atan2( fromThisToIKGoal.x, fromThisToIKGoal.y ) * Mathf.Rad2Deg /* Angle to goal position */) * weight, Vector3.back ) * wb.transform.rotation;
|
|
}
|
|
|
|
wb = wb.IKChild;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
LastLocalDirection = RefreshLocalDirection();
|
|
|
|
// Support for stretching
|
|
if( StretchToTarget > 0f )
|
|
{
|
|
float remainingDist = ( IKTargetPosition - EndIKBone.transform.position ).magnitude;
|
|
|
|
ActiveLength = Mathf.Epsilon;
|
|
wb = IKBones[0];
|
|
int ind = 0;
|
|
float boneMul = Mathf.Max( 1f, StretchToTarget );
|
|
|
|
while( wb.IKChild != null )
|
|
{
|
|
if( remainingDist <= 0f )
|
|
{
|
|
break;
|
|
}
|
|
|
|
Vector3 toTarget = ( IKTargetPosition - wb.transform.position );
|
|
Vector3 toTargetN = toTarget.normalized;
|
|
|
|
Vector3 prePos = wb.transform.position;
|
|
Vector3 preChildPos = wb.IKChild.transform.position;
|
|
Vector3 toNext = preChildPos - prePos;
|
|
Vector3 norm = toNext.normalized;
|
|
|
|
float dot = Vector3.Dot( norm, toTargetN );
|
|
|
|
if( dot > 0f )
|
|
{
|
|
float moveBy = wb.BoneLength * boneMul * dot;
|
|
if( moveBy > remainingDist ) moveBy = remainingDist;
|
|
|
|
Vector3 newChildPos = preChildPos + norm * ( moveBy );
|
|
wb.IKChild.transform.position = Vector3.Lerp( preChildPos, newChildPos, StretchToTarget );
|
|
wb.transform.rotation = wb.transform.rotation * Quaternion.FromToRotation( preChildPos - prePos, wb.Child.transform.position - wb.transform.position );
|
|
|
|
remainingDist -= Vector3.Distance( preChildPos, newChildPos );
|
|
}
|
|
|
|
wb = wb.IKChild;
|
|
ind += 1;
|
|
}
|
|
|
|
//if (stretch > 1f) for (int i = 1; i < IKBones.Length; ++i) IKBones[i].transform.position += (toGoal.normalized) * ((IKBones[i - 1].BoneLength ) * StretchCurve.Evaluate(-(1f - stretch)));
|
|
}
|
|
|
|
|
|
wb = IKBones[0];
|
|
while( wb != null )
|
|
{
|
|
// Storing final rotations for animator offset
|
|
wb.LastIKLocRotation = wb.transform.localRotation;
|
|
wb.LastIKLocPosition = wb.transform.localPosition;
|
|
|
|
// Offset based rotation sync with animator
|
|
Quaternion ikDiff = wb.LastIKLocRotation * Quaternion.Inverse( wb.InitialLocalRotation );
|
|
wb.transform.localRotation = Quaternion.Lerp( wb.LastIKLocRotation, ikDiff * wb.LastKeyLocalRotation, SyncWithAnimator );
|
|
|
|
if( IKWeight < 1f )
|
|
wb.transform.localRotation = Quaternion.Lerp( wb.LastKeyLocalRotation, wb.transform.localRotation, IKWeight );
|
|
|
|
wb = wb.IKChild;
|
|
}
|
|
}
|
|
|
|
|
|
protected Vector3 GetGoalPivotOffset()
|
|
{
|
|
if( !GoalPivotOffsetDetected() ) return Vector3.zero;
|
|
|
|
Vector3 IKDirection = ( IKTargetPosition - IKBones[0].transform.position ).normalized;
|
|
Vector3 secondaryDirection = new Vector3( IKDirection.y, IKDirection.z, IKDirection.x );
|
|
|
|
if( IKBones[IKBones.Length - 2].AngleLimit < 180 || IKBones[IKBones.Length - 2].TwistAngleLimit < 180 )
|
|
secondaryDirection = IKBones[IKBones.Length - 2].transform.rotation * IKBones[IKBones.Length - 2].ForwardOrientation;
|
|
|
|
return Vector3.Cross( IKDirection, secondaryDirection ) * IKBones[IKBones.Length - 2].BoneLength * 0.5f;
|
|
}
|
|
|
|
private bool GoalPivotOffsetDetected()
|
|
{
|
|
if( !Initialized ) return false;
|
|
|
|
Vector3 toLastDirection = Bones[Bones.Length - 1].transform.position - Bones[0].transform.position;
|
|
Vector3 toGoalDirection = IKTargetPosition - Bones[0].transform.position;
|
|
|
|
float toLastMagn = toLastDirection.magnitude;
|
|
float toGoalMagn = toGoalDirection.magnitude;
|
|
|
|
if( toGoalMagn == 0 ) return false;
|
|
if( toLastMagn == 0 ) return false;
|
|
if( toLastMagn < toGoalMagn ) return false;
|
|
if( toLastMagn < fullLength - ( Bones[Bones.Length - 2].BoneLength * 0.1f ) ) return false;
|
|
if( toGoalMagn > toLastMagn ) return false;
|
|
|
|
float dot = Vector3.Dot( toLastDirection / toLastMagn, toGoalDirection / toGoalMagn );
|
|
if( dot < 0.999f ) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
Vector3 RefreshLocalDirection()
|
|
{
|
|
LocalDirection = Bones[0].transform.InverseTransformDirection( Bones[Bones.Length - 1].transform.position - Bones[0].transform.position );
|
|
return LocalDirection;
|
|
}
|
|
|
|
float GetVelocityDifference()
|
|
{ return Vector3.SqrMagnitude( LocalDirection - LastLocalDirection ); }
|
|
|
|
|
|
|
|
/// <summary> Limiting angle for all IK bones </summary>
|
|
public void AutoLimitAngle( float angleLimit = 60f, float twistAngleLimit = 50f )
|
|
{
|
|
if( IKBones == null ) return;
|
|
|
|
float step = 1f / (float)IKBones.Length;
|
|
|
|
if( Invert )
|
|
{
|
|
for( int i = 0; i < IKBones.Length; i++ )
|
|
{
|
|
IKBones[i].AngleLimit = angleLimit * Mathf.Min( 1f, ( 1f - ( ( i + 1 ) * step ) ) * 3f );
|
|
IKBones[i].TwistAngleLimit = twistAngleLimit * Mathf.Min( 1f, ( 1f - ( ( i + 1 ) * step ) ) * 4.5f );
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
for( int i = 0; i < IKBones.Length; i++ )
|
|
{
|
|
IKBones[i].AngleLimit = angleLimit * Mathf.Min( 1f, ( i + 1 ) * step * 3f );
|
|
IKBones[i].TwistAngleLimit = twistAngleLimit * Mathf.Min( 1f, ( i + 1 ) * step * 4.5f );
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary> Spreading weight over IK bones automatically </summary>
|
|
public void AutoWeightBones( float baseValue = 1f )
|
|
{
|
|
float step = baseValue / (float)( Bones.Length * 1.3f );
|
|
|
|
if( Invert )
|
|
{
|
|
for( int i = 0; i < Bones.Length; i++ )
|
|
{
|
|
Bones[i].MotionWeight = 1f - ( baseValue - step * i );
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
for( int i = 0; i < Bones.Length; i++ )
|
|
{
|
|
Bones[i].MotionWeight = baseValue - step * i;
|
|
//Bones[i].MotionWeight *= Mathf.Min(1f, (i + 1) * step1 * 3f);
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary> Spreading weight over IK bones with curve (Clamped01) </summary>
|
|
public void AutoWeightBones( AnimationCurve weightCurve )
|
|
{
|
|
if( Invert )
|
|
{
|
|
for( int i = 0; i < Bones.Length; i++ )
|
|
Bones[i].MotionWeight = Mathf.Clamp( 1f - weightCurve.Evaluate( (float)i / (float)Bones.Length ), 0f, 1f );
|
|
return;
|
|
}
|
|
|
|
for( int i = 0; i < Bones.Length; i++ )
|
|
Bones[i].MotionWeight = Mathf.Clamp( weightCurve.Evaluate( (float)i / (float)Bones.Length ), 0f, 1f );
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
#endregion
|
|
|
|
|
|
[System.Serializable]
|
|
public class CCDIKBone : FIK_IKBoneBase
|
|
{
|
|
public CCDIKBone IKParent { get; private set; }
|
|
public CCDIKBone IKChild { get; private set; }
|
|
|
|
[Range( 0f, 180f )] public float AngleLimit = 45f;
|
|
[Range( 0f, 180f )] public float TwistAngleLimit = 5f;
|
|
|
|
/// <summary> Defined at Init() of CCD IK processor </summary>
|
|
public Vector3 ForwardOrientation;
|
|
public float FrameWorldLength = 1f;
|
|
|
|
public Vector2 HingeLimits = Vector2.zero;
|
|
public Quaternion PreviousHingeRotation;
|
|
public float PreviousHingeAngle;
|
|
|
|
public Vector3 LastIKLocPosition;
|
|
public Quaternion LastIKLocRotation;
|
|
|
|
public CCDIKBone( Transform t ) : base( t ) { }
|
|
|
|
public void Init( CCDIKBone child, CCDIKBone parent )
|
|
{
|
|
LastIKLocPosition = transform.localPosition;
|
|
IKParent = parent;
|
|
if( child != null ) SetChild( child );
|
|
IKChild = child;
|
|
}
|
|
|
|
public override void SetChild( FIK_IKBoneBase child )
|
|
{
|
|
base.SetChild( child );
|
|
}
|
|
|
|
#region CCD IK Methods
|
|
|
|
public void AngleLimiting()
|
|
{
|
|
Quaternion localRotation = Quaternion.Inverse( LastKeyLocalRotation ) * transform.localRotation;
|
|
Quaternion limitedRotation = localRotation;
|
|
|
|
if( FEngineering.VIsZero( HingeLimits ) )
|
|
{
|
|
if( AngleLimit < 180 ) limitedRotation = LimitSpherical( limitedRotation );
|
|
if( TwistAngleLimit < 180 ) limitedRotation = LimitZ( limitedRotation );
|
|
}
|
|
else limitedRotation = LimitHinge( limitedRotation );
|
|
|
|
if( FEngineering.QIsSame( limitedRotation, localRotation ) ) return;
|
|
|
|
transform.localRotation = LastKeyLocalRotation * limitedRotation;
|
|
}
|
|
|
|
private Quaternion LimitSpherical( Quaternion rotation )
|
|
{
|
|
if( FEngineering.QIsZero( rotation ) ) return rotation;
|
|
Vector3 currentForward = rotation * ForwardOrientation;
|
|
Quaternion limitAngle = Quaternion.RotateTowards( Quaternion.identity, Quaternion.FromToRotation( ForwardOrientation, currentForward ), AngleLimit );
|
|
return Quaternion.FromToRotation( currentForward, limitAngle * ForwardOrientation ) * rotation;
|
|
}
|
|
|
|
private Quaternion LimitZ( Quaternion currentRotation )
|
|
{
|
|
Vector3 orthoOrientation = new Vector3( ForwardOrientation.y, ForwardOrientation.z, ForwardOrientation.x );
|
|
Vector3 normal = currentRotation * ForwardOrientation;
|
|
Vector3 tangent = orthoOrientation;
|
|
Vector3.OrthoNormalize( ref normal, ref tangent );
|
|
|
|
orthoOrientation = currentRotation * orthoOrientation;
|
|
Vector3.OrthoNormalize( ref normal, ref orthoOrientation );
|
|
|
|
Quaternion limitRot = Quaternion.FromToRotation( orthoOrientation, tangent ) * currentRotation;
|
|
if( TwistAngleLimit <= 0 ) return limitRot;
|
|
|
|
return Quaternion.RotateTowards( limitRot, currentRotation, TwistAngleLimit );
|
|
}
|
|
|
|
private Quaternion LimitHinge( Quaternion rotation )
|
|
{
|
|
Quaternion addRotation = ( Quaternion.FromToRotation( rotation * ForwardOrientation, ForwardOrientation ) * rotation ) * Quaternion.Inverse( PreviousHingeRotation );
|
|
float addAngle = Quaternion.Angle( Quaternion.identity, addRotation );
|
|
|
|
Vector3 orthoOrientation = new Vector3( ForwardOrientation.z, ForwardOrientation.x, ForwardOrientation.y );
|
|
Vector3 cross = Vector3.Cross( orthoOrientation, ForwardOrientation );
|
|
if( Vector3.Dot( addRotation * orthoOrientation, cross ) > 0f ) addAngle = -addAngle;
|
|
|
|
PreviousHingeAngle = Mathf.Clamp( PreviousHingeAngle + addAngle, HingeLimits.x, HingeLimits.y );
|
|
PreviousHingeRotation = Quaternion.AngleAxis( PreviousHingeAngle, ForwardOrientation );
|
|
return PreviousHingeRotation;
|
|
}
|
|
|
|
#endregion
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
}
|