void _SyncDisplacement(FullBodyIK fullBodyIK)
{
// Measure bone length.(Using worldPosition)
// Force execution on 1st time. (Ignore case _settings.syncDisplacement == SyncDisplacement.Disable)
if (_settings.syncDisplacement == SyncDisplacement.Everyframe || !_isSyncDisplacementAtLeastOnce)
{
_isSyncDisplacementAtLeastOnce = true;
if (_headBone != null && _headBone.transformIsAlive)
{
if (_headEffector != null)
{
SAFBIKQuatMultInv0(out _headEffectorToWorldRotation, ref _headEffector._defaultRotation, ref _headBone._defaultRotation);
}
if (_leftEyeBone != null && _leftEyeBone.transformIsAlive)
{
SAFBIKQuatMultInv0(out _headToLeftEyeRotation, ref _headBone._defaultRotation, ref _leftEyeBone._defaultRotation);
}
if (_rightEyeBone != null && _rightEyeBone.transformIsAlive)
{
SAFBIKQuatMultInv0(out _headToRightEyeRotation, ref _headBone._defaultRotation, ref _rightEyeBone._defaultRotation);
}
}
_isEnabledCustomEyes = fullBodyIK._PrepareCustomEyes(ref _headToLeftEyeRotation, ref _headToRightEyeRotation);
}
}
void _BoneField(string boneName, ref FullBodyIK.Bone bone, bool isOptional)
{
FullBodyIK.SafeNew(ref bone);
var fbik = this.target as FullBodyIKBehaviourBase;
EditorGUILayout.BeginHorizontal();
if (bone.transform == null)
{
if (isOptional)
{
GUILayout.Label(boneName + " *", _guiStyle_boneName_Unselected_Optional, GUILayout.Width(boneNameFieldSize));
}
else
{
GUILayout.Label(boneName, _guiStyle_boneName_Unselected, GUILayout.Width(boneNameFieldSize));
}
}
else
{
GUILayout.Label(boneName, GUILayout.Width(boneNameFieldSize));
}
EditorUtil.GUI.PushEnabled(!Application.isPlaying);
EditorUtil.GUI.TransformField(fbik, "", ref bone.transform, true);
EditorUtil.GUI.PopEnabled();
EditorGUILayout.EndHorizontal();
}
protected virtual void OnDestroy()
{
if( _cache_fullBodyIK == null ) {
_cache_fullBodyIK = fullBodyIK;
}
if( _cache_fullBodyIK != null ) {
_cache_fullBodyIK.Destroy();
}
}
void OnDrawGizmos()
{
if( _cache_fullBodyIK == null ) {
_cache_fullBodyIK = fullBodyIK;
}
if( _cache_fullBodyIK != null ) {
_cache_fullBodyIK.DrawGizmos();
}
}
// Excecutable in Inspector.
public virtual void Prefix()
{
if( _cache_fullBodyIK == null ) {
_cache_fullBodyIK = fullBodyIK;
}
if( _cache_fullBodyIK != null ) {
_cache_fullBodyIK.Prefix( this.transform );
}
}
// Excecutable in Inspector.
public void Prefix()
{
if (_fullBodyIK == null)
{
_fullBodyIK = new FullBodyIK();
}
fullBodyIK.Prefix(this.transform);
}
public virtual void Prefix()
{
if (_cache_fullBodyIK == null)
{
_cache_fullBodyIK = fullBodyIK;
}
if (_cache_fullBodyIK != null)
{
_cache_fullBodyIK.Prefix(this.transform);
}
}
protected virtual void OnDestroy()
{
if (_cache_fullBodyIK == null)
{
_cache_fullBodyIK = fullBodyIK;
}
if (_cache_fullBodyIK != null)
{
_cache_fullBodyIK.Destroy();
}
}
void OnDrawGizmos()
{
if (_cache_fullBodyIK == null)
{
_cache_fullBodyIK = fullBodyIK;
}
if (_cache_fullBodyIK != null)
{
_cache_fullBodyIK.DrawGizmos();
}
}
public HeadIK( FullBodyIK fullBodyIK )
{
_settings = fullBodyIK.settings;
_internalValues = fullBodyIK.internalValues;
_neckBone = _PrepareBone( fullBodyIK.headBones.neck );
_headBone = _PrepareBone( fullBodyIK.headBones.head );
_leftEyeBone = _PrepareBone( fullBodyIK.headBones.leftEye );
_rightEyeBone = _PrepareBone( fullBodyIK.headBones.rightEye );
_headEffector = fullBodyIK.headEffectors.head;
_eyesEffector = fullBodyIK.headEffectors.eyes;
}
public HeadIK(FullBodyIK fullBodyIK)
{
_settings = fullBodyIK.settings;
_internalValues = fullBodyIK.internalValues;
_neckBone = _PrepareBone(fullBodyIK.headBones.neck);
_headBone = _PrepareBone(fullBodyIK.headBones.head);
_leftEyeBone = _PrepareBone(fullBodyIK.headBones.leftEye);
_rightEyeBone = _PrepareBone(fullBodyIK.headBones.rightEye);
_headEffector = fullBodyIK.headEffectors.head;
_eyesEffector = fullBodyIK.headEffectors.eyes;
}
public void PostSyncDisplacement(FullBodyIK fullBodyIK)
{
if (_boneLocation == BoneLocation.Hips)
{
_defaultPosition = fullBodyIK.boneCaches.defaultHipsPosition + fullBodyIK.boneCaches.hipsOffset;
}
else if (_parentBone != null)
{
_defaultPosition = _parentBone._defaultPosition + _defaultLocalTranslate;
}
_ComputeLocalAxis(fullBodyIK); // Require PostPrepare()
}
protected virtual void Awake()
{
#if UNITY_EDITOR
if( !Application.isPlaying ) {
return;
}
#endif
if( _cache_fullBodyIK == null ) {
_cache_fullBodyIK = fullBodyIK;
}
if( _cache_fullBodyIK != null ) {
_cache_fullBodyIK.Awake( this.transform );
}
}
protected virtual void LateUpdate()
{
#if UNITY_EDITOR
if( !Application.isPlaying ) {
return;
}
#endif
if( _cache_fullBodyIK == null ) {
_cache_fullBodyIK = fullBodyIK;
}
if( _cache_fullBodyIK != null ) {
_cache_fullBodyIK.Update();
}
}
void LateUpdate()
{
#if UNITY_EDITOR
if (!Application.isPlaying)
{
return;
}
#endif
FullBodyIK.Assert(_fullBodyIK != null);
if (_fullBodyIK != null)
{
_fullBodyIK.Update();
}
}
void Awake()
{
#if UNITY_EDITOR
if (!Application.isPlaying)
{
return;
}
#endif
if (_fullBodyIK == null)
{
_fullBodyIK = new FullBodyIK();
}
_fullBodyIK.Initialize(this.transform);
}
protected virtual void Awake()
{
#if UNITY_EDITOR
if (!Application.isPlaying)
{
return;
}
#endif
if (_cache_fullBodyIK == null)
{
_cache_fullBodyIK = fullBodyIK;
}
if (_cache_fullBodyIK != null)
{
_cache_fullBodyIK.Awake(this.transform);
}
}
protected virtual void LateUpdate()
{
#if UNITY_EDITOR
if (!Application.isPlaying)
{
return;
}
#endif
if (_cache_fullBodyIK == null)
{
_cache_fullBodyIK = fullBodyIK;
}
if (_cache_fullBodyIK != null)
{
_cache_fullBodyIK.Update();
}
}
void _BoneField( string boneName, ref FullBodyIK.Bone bone, bool isOptional )
{
FullBodyIK.SafeNew( ref bone );
var fbik = this.target as FullBodyIKBehaviourBase;
EditorGUILayout.BeginHorizontal();
if( bone.transform == null ) {
if( isOptional ) {
GUILayout.Label( boneName + " *", _guiStyle_boneName_Unselected_Optional, GUILayout.Width( boneNameFieldSize ) );
} else {
GUILayout.Label( boneName, _guiStyle_boneName_Unselected, GUILayout.Width( boneNameFieldSize ) );
}
} else {
GUILayout.Label( boneName, GUILayout.Width( boneNameFieldSize ) );
}
EditorUtil.GUI.PushEnabled( !Application.isPlaying );
EditorUtil.GUI.TransformField( fbik, "", ref bone.transform, true );
EditorUtil.GUI.PopEnabled();
EditorGUILayout.EndHorizontal();
}
void _SyncDisplacement( FullBodyIK fullBodyIK )
{
// Measure bone length.(Using worldPosition)
// Force execution on 1st time. (Ignore case _settings.syncDisplacement == SyncDisplacement.Disable)
if( _settings.syncDisplacement == SyncDisplacement.Everyframe || !_isSyncDisplacementAtLeastOnce ) {
_isSyncDisplacementAtLeastOnce = true;
if( _headBone != null && _headBone.transformIsAlive ) {
if( _headEffector != null ) {
SAFBIKQuatMultInv0( out _headEffectorToWorldRotation, ref _headEffector._defaultRotation, ref _headBone._defaultRotation );
}
if( _leftEyeBone != null && _leftEyeBone.transformIsAlive ) {
SAFBIKQuatMultInv0( out _headToLeftEyeRotation, ref _headBone._defaultRotation, ref _leftEyeBone._defaultRotation );
}
if( _rightEyeBone != null && _rightEyeBone.transformIsAlive ) {
SAFBIKQuatMultInv0( out _headToRightEyeRotation, ref _headBone._defaultRotation, ref _rightEyeBone._defaultRotation );
}
}
_isEnabledCustomEyes = fullBodyIK._PrepareCustomEyes( ref _headToLeftEyeRotation, ref _headToRightEyeRotation );
}
}
public void Prepare(FullBodyIK fullBodyIK)
{
Assert(fullBodyIK != null);
_ClearInternal();
_ComputeDefaultTransform(fullBodyIK);
// Reset transform.
if (this.transformIsAlive)
{
if (_effectorType == EffectorType.Eyes)
{
this.transform.position = _defaultPosition + fullBodyIK.internalValues.defaultRootBasis.column2 * Eyes_DefaultDistance;
}
else
{
this.transform.position = _defaultPosition;
}
if (!_defaultLocalBasisIsIdentity)
{
this.transform.rotation = _defaultRotation;
}
else
{
this.transform.localRotation = Quaternion.identity;
}
this.transform.localScale = Vector3.one;
}
_worldPosition = _defaultPosition;
_worldRotation = _defaultRotation;
if (_effectorType == EffectorType.Eyes)
{
_worldPosition += fullBodyIK.internalValues.defaultRootBasis.column2 * Eyes_DefaultDistance;
}
}
public LimbIK( FullBodyIK fullBodyIK, LimbIKLocation limbIKLocation )
{
Assert( fullBodyIK != null );
if( fullBodyIK == null ) {
return;
}
_settings = fullBodyIK.settings;
_internalValues = fullBodyIK.internalValues;
_limbIKLocation = limbIKLocation;
_limbIKType = FullBodyIK.ToLimbIKType( limbIKLocation );
_limbIKSide = FullBodyIK.ToLimbIKSide( limbIKLocation );
if( _limbIKType == LimbIKType.Leg ) {
var legBones = (_limbIKSide == Side.Left) ? fullBodyIK.leftLegBones : fullBodyIK.rightLegBones;
var legEffectors = (_limbIKSide == Side.Left) ? fullBodyIK.leftLegEffectors : fullBodyIK.rightLegEffectors;
_beginBone = legBones.leg;
_bendingBone = legBones.knee;
_endBone = legBones.foot;
_bendingEffector = legEffectors.knee;
_endEffector = legEffectors.foot;
} else if( _limbIKType == LimbIKType.Arm ) {
var armBones = (_limbIKSide == Side.Left) ? fullBodyIK.leftArmBones : fullBodyIK.rightArmBones;
var armEffectors = (_limbIKSide == Side.Left) ? fullBodyIK.leftArmEffectors : fullBodyIK.rightArmEffectors;
_beginBone = armBones.arm;
_bendingBone = armBones.elbow;
_endBone = armBones.wrist;
_bendingEffector = armEffectors.elbow;
_endEffector = armEffectors.wrist;
_PrepareRollBones( ref _armRollBones, armBones.armRoll );
_PrepareRollBones( ref _elbowRollBones, armBones.elbowRoll );
}
_Prepare( fullBodyIK );
}
void _EffectorField( string effectorName, ref FullBodyIK.Effector effector )
{
var fbik = this.target as FullBodyIKBehaviourBase;
var editorSettings = fbik.fullBodyIK.editorSettings;
if( effector == null ) {
return;
}
GUILayout.BeginHorizontal();
GUILayout.Label( effectorName, GUILayout.Width( effectorNameFieldSize ) );
GUILayout.FlexibleSpace();
if( editorSettings.isShowEffectorTransform ) {
EditorUtil.GUI.ObjectField( fbik, "", ref effector.transform, true, GUILayout.Height( 18.0f ) );
} else {
float labelSpace = 24.0f;
EditorGUILayout.LabelField( "Pos", GUILayout.Width( labelSpace ) );
EditorUtil.GUI.ToggleLegacy( fbik, "", ref effector.positionEnabled );
EditorUtil.GUI.PushEnabled( effector.positionEnabled );
EditorUtil.GUI.HorizonalSlider( fbik, ref effector.positionWeight, 0.0f, 1.0f, GUILayout.ExpandWidth( false ), GUILayout.Width( 30.0f ) );
EditorUtil.GUI.PushEnabled( effector.pullContained );
EditorGUILayout.LabelField( "Pull", GUILayout.Width( labelSpace ) );
EditorUtil.GUI.HorizonalSlider( fbik, ref effector.pull, 0.0f, 1.0f, GUILayout.ExpandWidth( false ), GUILayout.MinWidth( 30.0f ) );
EditorUtil.GUI.PopEnabled();
EditorUtil.GUI.PopEnabled();
EditorUtil.GUI.PushEnabled( effector.rotationContained );
EditorGUILayout.LabelField( "Rot", GUILayout.Width( labelSpace ) );
EditorUtil.GUI.ToggleLegacy( fbik, "", ref effector.rotationEnabled );
EditorUtil.GUI.PushEnabled( effector.rotationEnabled );
EditorUtil.GUI.HorizonalSlider( fbik, ref effector.rotationWeight, 0.0f, 1.0f, GUILayout.ExpandWidth( false ), GUILayout.Width( 30.0f ) );
EditorUtil.GUI.PopEnabled();
EditorUtil.GUI.PopEnabled();
}
GUILayout.EndHorizontal();
}
void _ComputeLocalAxis(FullBodyIK fullBodyIK)
{
// Compute _localAxisBasis for each bones.
if (this.transformIsAlive && (_parentBone != null && _parentBone.transformIsAlive))
{
if (_localAxisFrom == _LocalAxisFrom.Parent ||
_parentBone._localAxisFrom == _LocalAxisFrom.Child)
{
Vector3 dir = _defaultLocalDirection;
if (dir.x != 0.0f || dir.y != 0.0f || dir.z != 0.0f)
{
if (_localAxisFrom == _LocalAxisFrom.Parent)
{
SAFBIKComputeBasisFrom(out _localAxisBasis, ref fullBodyIK.internalValues.defaultRootBasis, ref dir, _localDirectionAs);
}
if (_parentBone._localAxisFrom == _LocalAxisFrom.Child)
{
if (_parentBone._boneType == BoneType.Shoulder)
{
Bone shoulderBone = _parentBone;
Bone spineUBone = _parentBone._parentBone;
Bone neckBone = (fullBodyIK.headBones != null) ? fullBodyIK.headBones.neck : null;
if (neckBone != null && !neckBone.transformIsAlive)
{
neckBone = null;
}
if (fullBodyIK.internalValues.shoulderDirYAsNeck == -1)
{
if (fullBodyIK.settings.shoulderDirYAsNeck == AutomaticBool.Auto)
{
if (spineUBone != null && neckBone != null)
{
Vector3 shoulderToSpineU = shoulderBone._defaultLocalDirection;
Vector3 shoulderToNeck = neckBone._defaultPosition - shoulderBone._defaultPosition;
if (SAFBIKVecNormalize(ref shoulderToNeck))
{
float shoulderToSpineUTheta = Mathf.Abs(Vector3.Dot(dir, shoulderToSpineU));
float shoulderToNeckTheta = Mathf.Abs(Vector3.Dot(dir, shoulderToNeck));
if (shoulderToSpineUTheta < shoulderToNeckTheta)
{
fullBodyIK.internalValues.shoulderDirYAsNeck = 0;
}
else
{
fullBodyIK.internalValues.shoulderDirYAsNeck = 1;
}
}
else
{
fullBodyIK.internalValues.shoulderDirYAsNeck = 0;
}
}
else
{
fullBodyIK.internalValues.shoulderDirYAsNeck = 0;
}
}
else
{
fullBodyIK.internalValues.shoulderDirYAsNeck = (fullBodyIK.settings.shoulderDirYAsNeck != AutomaticBool.Disable) ? 1 : 0;
}
}
Vector3 xDir, yDir, zDir;
xDir = (_parentBone._localDirectionAs == _DirectionAs.XMinus) ? -dir : dir;
if (fullBodyIK.internalValues.shoulderDirYAsNeck != 0 && neckBone != null)
{
yDir = neckBone._defaultPosition - shoulderBone._defaultPosition;
}
else
{
yDir = shoulderBone._defaultLocalDirection;
}
zDir = Vector3.Cross(xDir, yDir);
yDir = Vector3.Cross(zDir, xDir);
if (SAFBIKVecNormalize2(ref yDir, ref zDir))
{
_parentBone._localAxisBasis.SetColumn(ref xDir, ref yDir, ref zDir);
}
}
else if (_parentBone._boneType == BoneType.Spine && _boneType != BoneType.Spine && _boneType != BoneType.Neck)
{
// Compute spine/neck only( Exclude shouder / arm ).
}
else if (_parentBone._boneType == BoneType.Hips && _boneType != BoneType.Spine)
{
// Compute spine only( Exclude leg ).
}
else
{
if (_parentBone._boneType == BoneType.Hips)
{
Vector3 baseX = fullBodyIK.internalValues.defaultRootBasis.column0;
SAFBIKComputeBasisFromXYLockY(out _parentBone._localAxisBasis, ref baseX, ref dir);
}
else if (_parentBone._boneType == BoneType.Spine || _parentBone._boneType == BoneType.Neck)
{
// Using parent axis for spine or neck. Preprocess for BodyIK.
if (_parentBone._parentBone != null)
{
Vector3 dirX = _parentBone._parentBone._localAxisBasis.column0;
SAFBIKComputeBasisFromXYLockY(out _parentBone._localAxisBasis, ref dirX, ref dir);
}
}
else
{
if (_localAxisFrom == _LocalAxisFrom.Parent && _localDirectionAs == _parentBone._localDirectionAs)
{
_parentBone._localAxisBasis = _localAxisBasis;
}
else
{
SAFBIKComputeBasisFrom(out _parentBone._localAxisBasis,
ref fullBodyIK.internalValues.defaultRootBasis, ref dir, _parentBone._localDirectionAs);
}
}
}
}
}
}
}
}
public void Prepare( FullBodyIK fullBodyIK )
{
Assert( fullBodyIK != null );
_transformIsAlive = -1;
_localAxisBasis = Matrix3x3.identity;
_isWritebackWorldPosition = false;
_parentBone = null;
// Find transform alive parent bone.
if( this.transformIsAlive ) {
for( Bone temp = _parentBoneLocationBased; temp != null; temp = temp._parentBoneLocationBased ) {
if( temp.transformIsAlive ) {
_parentBone = temp;
break;
}
}
}
#if SAFULLBODYIK_DEBUG
if( _boneType != BoneType.Hips && _boneType != BoneType.Eye ) {
if( this.transformIsAlive && _parentBone == null ) {
DebugLogError( "parentBone is not found. " + _boneLocation + " (" + _boneType + ") parentBoneLocationBased: " + ((_parentBoneLocationBased != null) ? _parentBoneLocationBased.name : "") );
}
}
#endif
if( _boneLocation == BoneLocation.Hips ) {
if( this.transformIsAlive ) {
_isWritebackWorldPosition = true;
}
} else if( _boneLocation == BoneLocation.Spine ) {
if( this.transformIsAlive ) {
if( _parentBone != null && _parentBone.transformIsAlive ) {
if( IsParentOfRecusively( _parentBone.transform, this.transform ) ) {
_isWritebackWorldPosition = true;
}
}
}
}
if( _boneType == BoneType.Eye ) {
if( fullBodyIK._IsHiddenCustomEyes() ) {
_isWritebackWorldPosition = true;
}
}
// Get defaultPosition / defaultRotation
if( this.transformIsAlive ) {
_defaultPosition = this.transform.position;
_defaultRotation = this.transform.rotation;
SAFBIKMatSetRot( out _defaultBasis, ref _defaultRotation );
if( _parentBone != null ) { // Always _parentBone.transformIsAlive == true
_defaultLocalTranslate = _defaultPosition - _parentBone._defaultPosition;
_defaultLocalLength = FastLength.FromVector3( ref _defaultLocalTranslate );
if( _defaultLocalLength.length > FLOAT_EPSILON ) {
float lengthInv = (1.0f / _defaultLocalLength.length);
_defaultLocalDirection.x = _defaultLocalTranslate.x * lengthInv;
_defaultLocalDirection.y = _defaultLocalTranslate.y * lengthInv;
_defaultLocalDirection.z = _defaultLocalTranslate.z * lengthInv;
}
}
SAFBIKMatMultInv0( out _worldToBaseBasis, ref _defaultBasis, ref fullBodyIK.internalValues.defaultRootBasis );
_baseToWorldBasis = _worldToBaseBasis.transpose;
SAFBIKMatGetRot( out _worldToBaseRotation, ref _worldToBaseBasis );
_baseToWorldRotation = Inverse( _worldToBaseRotation );
} else {
_defaultPosition = Vector3.zero;
_defaultRotation = Quaternion.identity;
_defaultBasis = Matrix3x3.identity;
_defaultLocalTranslate = Vector3.zero;
_defaultLocalLength = new FastLength();
_defaultLocalDirection = Vector3.zero;
_worldToBaseBasis = Matrix3x3.identity;
_baseToWorldBasis = Matrix3x3.identity;
_worldToBaseRotation = Quaternion.identity;
_baseToWorldRotation = Quaternion.identity;
}
_ComputeLocalAxis( fullBodyIK ); // Require PostPrepare()
}
void _Prepare( FullBodyIK fullBodyIK )
{
if( _spineBones != null ) {
int spineLength = _spineBones.Length;
_spineDirXRate = new float[spineLength];
if( spineLength > 1 ) {
_spinePrevCenterArmToChildBasis = new Matrix3x3[spineLength - 1];
_spineCenterArmToChildBasis = new Matrix3x3[spineLength - 1];
for( int i = 0; i != spineLength - 1; ++i ) {
_spinePrevCenterArmToChildBasis[i] = Matrix3x3.identity;
_spineCenterArmToChildBasis[i] = Matrix3x3.identity;
}
}
}
}
public void Prepare(FullBodyIK fullBodyIK)
{
Assert(fullBodyIK != null);
_transformIsAlive = -1;
_localAxisBasis = Matrix3x3.identity;
_isWritebackWorldPosition = false;
_parentBone = null;
// Find transform alive parent bone.
if (this.transformIsAlive)
{
for (Bone temp = _parentBoneLocationBased; temp != null; temp = temp._parentBoneLocationBased)
{
if (temp.transformIsAlive)
{
_parentBone = temp;
break;
}
}
}
#if SAFULLBODYIK_DEBUG
if (_boneType != BoneType.Hips && _boneType != BoneType.Eye)
{
if (this.transformIsAlive && _parentBone == null)
{
DebugLogError("parentBone is not found. " + _boneLocation + " (" + _boneType + ") parentBoneLocationBased: " + ((_parentBoneLocationBased != null) ? _parentBoneLocationBased.name : ""));
}
}
#endif
if (_boneLocation == BoneLocation.Hips)
{
if (this.transformIsAlive)
{
_isWritebackWorldPosition = true;
}
}
else if (_boneLocation == BoneLocation.Spine)
{
if (this.transformIsAlive)
{
if (_parentBone != null && _parentBone.transformIsAlive)
{
if (IsParentOfRecusively(_parentBone.transform, this.transform))
{
_isWritebackWorldPosition = true;
}
}
}
}
if (_boneType == BoneType.Eye)
{
if (fullBodyIK.settings.modelTemplate == ModelTemplate.UnityChan)
{
_isWritebackWorldPosition = true;
}
}
// Get defaultPosition / defaultRotation
if (this.transformIsAlive)
{
_defaultPosition = this.transform.position;
_defaultRotation = this.transform.rotation;
SAFBIKMatSetRot(out _defaultBasis, ref _defaultRotation);
if (_parentBone != null) // Always _parentBone.transformIsAlive == true
{
_defaultLocalTranslate = _defaultPosition - _parentBone._defaultPosition;
_defaultLocalLength = FastLength.FromVector3(ref _defaultLocalTranslate);
if (_defaultLocalLength.length > FLOAT_EPSILON)
{
float lengthInv = (1.0f / _defaultLocalLength.length);
_defaultLocalDirection.x = _defaultLocalTranslate.x * lengthInv;
_defaultLocalDirection.y = _defaultLocalTranslate.y * lengthInv;
_defaultLocalDirection.z = _defaultLocalTranslate.z * lengthInv;
}
}
SAFBIKMatMultInv0(out _worldToBaseBasis, ref _defaultBasis, ref fullBodyIK.internalValues.defaultRootBasis);
_baseToWorldBasis = _worldToBaseBasis.transpose;
SAFBIKMatGetRot(out _worldToBaseRotation, ref _worldToBaseBasis);
_baseToWorldRotation = Inverse(_worldToBaseRotation);
}
else
{
_defaultPosition = Vector3.zero;
_defaultRotation = Quaternion.identity;
_defaultBasis = Matrix3x3.identity;
_defaultLocalTranslate = Vector3.zero;
_defaultLocalLength = new FastLength();
_defaultLocalDirection = Vector3.zero;
_worldToBaseBasis = Matrix3x3.identity;
_baseToWorldBasis = Matrix3x3.identity;
_worldToBaseRotation = Quaternion.identity;
_baseToWorldRotation = Quaternion.identity;
}
_ComputeLocalAxis(fullBodyIK); // Require PostPrepare()
}
public bool Solve( FullBodyIK fullBodyIK )
{
if( _neckBone == null || !_neckBone.transformIsAlive ||
_headBone == null || !_headBone.transformIsAlive ||
_headBone.parentBone == null || !_headBone.parentBone.transformIsAlive ) {
return false;
}
_SyncDisplacement( fullBodyIK );
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
if( headPositionWeight <= IKEpsilon && eyesPositionWeight <= IKEpsilon ) {
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Quaternion parentBaseRotation;
SAFBIKQuatMult( out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation );
if( _internalValues.resetTransforms ) {
Quaternion tempRotation;
SAFBIKQuatMult( out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation );
_neckBone.worldRotation = tempRotation;
}
float headRotationWeight = _headEffector.rotationEnabled ? _headEffector.rotationWeight : 0.0f;
if( headRotationWeight > IKEpsilon ) {
Quaternion headEffectorWorldRotation = _headEffector.worldRotation;
Quaternion toRotation;
SAFBIKQuatMult( out toRotation, ref headEffectorWorldRotation, ref _headEffectorToWorldRotation );
if( headRotationWeight < 1.0f - IKEpsilon ) {
Quaternion fromRotation;
if( _internalValues.resetTransforms ) {
SAFBIKQuatMult( out fromRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation );
} else {
fromRotation = _headBone.worldRotation; // This is able to use _headBone.worldRotation directly.
}
_headBone.worldRotation = Quaternion.Lerp( fromRotation, toRotation, headRotationWeight );
} else {
_headBone.worldRotation = toRotation;
}
_HeadRotationLimit();
} else {
if( _internalValues.resetTransforms ) {
Quaternion tempRotation;
SAFBIKQuatMult( out tempRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation );
_headBone.worldRotation = tempRotation;
}
}
if( _internalValues.resetTransforms ) {
if( _isEnabledCustomEyes ) {
fullBodyIK._ResetCustomEyes();
} else {
_ResetEyes();
}
}
return _internalValues.resetTransforms || (headRotationWeight > IKEpsilon);
}
_Solve( fullBodyIK );
return true;
}
public BodyIK( FullBodyIK fullBodyIK, LimbIK[] limbIK )
{
Assert( fullBodyIK != null );
_limbIK = limbIK;
_settings = fullBodyIK.settings;
_internalValues = fullBodyIK.internalValues;
_hipsBone = _PrepareBone( fullBodyIK.bodyBones.hips );
_neckBone = _PrepareBone( fullBodyIK.headBones.neck );
_headBone = _PrepareBone( fullBodyIK.headBones.head );
_hipsEffector = fullBodyIK.bodyEffectors.hips;
_neckEffector = fullBodyIK.headEffectors.neck;
_headEffector = fullBodyIK.headEffectors.head;
_eyesEffector = fullBodyIK.headEffectors.eyes;
_armEffectors[0] = fullBodyIK.leftArmEffectors.arm;
_armEffectors[1] = fullBodyIK.rightArmEffectors.arm;
_elbowEffectors[0] = fullBodyIK.leftArmEffectors.elbow;
_elbowEffectors[1] = fullBodyIK.rightArmEffectors.elbow;
_wristEffectors[0] = fullBodyIK.leftArmEffectors.wrist;
_wristEffectors[1] = fullBodyIK.rightArmEffectors.wrist;
_kneeEffectors[0] = fullBodyIK.leftLegEffectors.knee;
_kneeEffectors[1] = fullBodyIK.rightLegEffectors.knee;
_footEffectors[0] = fullBodyIK.leftLegEffectors.foot;
_footEffectors[1] = fullBodyIK.rightLegEffectors.foot;
_spineBones = _PrepareSpineBones( fullBodyIK.bones );
if( _spineBones != null && _spineBones.Length > 0 ) {
int spineLength = _spineBones.Length;
_spineBone = _spineBones[0];
_spineUBone = _spineBones[spineLength - 1];
_spineEnabled = new bool[spineLength];
}
// Memo: These should be pair bones.(Necessary each side bones.)
_kneeBones = _PrepareBones( fullBodyIK.leftLegBones.knee, fullBodyIK.rightLegBones.knee );
_elbowBones = _PrepareBones( fullBodyIK.leftArmBones.elbow, fullBodyIK.rightArmBones.elbow );
_legBones = _PrepareBones( fullBodyIK.leftLegBones.leg, fullBodyIK.rightLegBones.leg );
_armBones = _PrepareBones( fullBodyIK.leftArmBones.arm, fullBodyIK.rightArmBones.arm );
_shoulderBones = _PrepareBones( fullBodyIK.leftArmBones.shoulder, fullBodyIK.rightArmBones.shoulder );
_nearArmBones = (_shoulderBones != null) ? _shoulderBones : _nearArmBones;
_Prepare( fullBodyIK );
}
void _Solve(FullBodyIK fullBodyIK)
{
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Matrix3x3 parentBasis;
SAFBIKMatSetRotMultInv1(out parentBasis, ref parentWorldRotation, ref _neckBone.parentBone._defaultRotation);
Matrix3x3 parentBaseBasis;
SAFBIKMatMult(out parentBaseBasis, ref parentBasis, ref _internalValues.defaultRootBasis);
Quaternion parentBaseRotation;
SAFBIKQuatMult(out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation);
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
Quaternion neckBonePrevRotation = Quaternion.identity;
Quaternion headBonePrevRotation = Quaternion.identity;
Quaternion leftEyeBonePrevRotation = Quaternion.identity;
Quaternion rightEyeBonePrevRotation = Quaternion.identity;
if (!_internalValues.resetTransforms)
{
neckBonePrevRotation = _neckBone.worldRotation;
headBonePrevRotation = _headBone.worldRotation;
if (_leftEyeBone != null && _leftEyeBone.transformIsAlive)
{
leftEyeBonePrevRotation = _leftEyeBone.worldRotation;
}
if (_rightEyeBone != null && _rightEyeBone.transformIsAlive)
{
rightEyeBonePrevRotation = _rightEyeBone.worldRotation;
}
}
// for Neck
if (headPositionWeight > IKEpsilon)
{
Matrix3x3 neckBoneBasis;
SAFBIKMatMult(out neckBoneBasis, ref parentBasis, ref _neckBone._localAxisBasis);
Vector3 yDir = _headEffector.worldPosition - _neckBone.worldPosition; // Not use _hidden_worldPosition
if (SAFBIKVecNormalize(ref yDir))
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref neckBoneBasis, ref yDir);
if (_LimitXZ_Square(ref localDir,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitPitchDownTheta.sin,
_internalValues.headIK.neckLimitPitchUpTheta.sin))
{
SAFBIKMatMultVec(out yDir, ref neckBoneBasis, ref localDir);
}
Vector3 xDir = parentBaseBasis.column0;
Vector3 zDir = parentBaseBasis.column2;
if (SAFBIKComputeBasisLockY(out neckBoneBasis, ref xDir, ref yDir, ref zDir))
{
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref neckBoneBasis, ref _neckBone._boneToWorldBasis);
if (headPositionWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult(out fromRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
}
else
{
fromRotation = neckBonePrevRotation; // This is able to use _headBone.worldRotation directly.
}
_neckBone.worldRotation = Quaternion.Lerp(fromRotation, worldRotation, headPositionWeight);
}
else
{
_neckBone.worldRotation = worldRotation;
}
}
}
}
else if (_internalValues.resetTransforms)
{
Quaternion tempRotation;
SAFBIKQuatMult(out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
_neckBone.worldRotation = tempRotation;
}
// for Head / Eyes
if (eyesPositionWeight <= IKEpsilon)
{
float headRotationWeight = _headEffector.rotationEnabled ? _headEffector.rotationWeight : 0.0f;
if (headRotationWeight > IKEpsilon)
{
Quaternion headEffectorWorldRotation = _headEffector.worldRotation;
Quaternion toRotation;
SAFBIKQuatMult(out toRotation, ref headEffectorWorldRotation, ref _headEffectorToWorldRotation);
if (headRotationWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult3(out fromRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation);
}
else
{
// Not use _headBone.worldRotation.
SAFBIKQuatMultNorm3Inv1(out fromRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation);
}
_headBone.worldRotation = Quaternion.Lerp(fromRotation, toRotation, headRotationWeight);
}
else
{
_headBone.worldRotation = toRotation;
}
}
else
{
if (_internalValues.resetTransforms)
{
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headBoneWorldRotation;
SAFBIKQuatMult3(out headBoneWorldRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation);
_headBone.worldRotation = headBoneWorldRotation;
}
}
_HeadRotationLimit();
if (_internalValues.resetTransforms)
{
if (_isEnabledCustomEyes)
{
fullBodyIK._ResetCustomEyes();
}
else
{
_ResetEyes();
}
}
return;
}
{
Vector3 eyesPosition, parentBoneWorldPosition = _neckBone.parentBone.worldPosition;
SAFBIKMatMultVecPreSubAdd(out eyesPosition, ref parentBasis, ref _eyesEffector._defaultPosition, ref _neckBone.parentBone._defaultPosition, ref parentBoneWorldPosition);
// Note: Not use _eyesEffector._hidden_worldPosition
Vector3 eyesDir = _eyesEffector.worldPosition - eyesPosition; // Memo: Not normalize yet.
Matrix3x3 neckBaseBasis = parentBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref parentBaseBasis, ref eyesDir);
localDir.y *= _settings.headIK.eyesToNeckPitchRate;
SAFBIKVecNormalize(ref localDir);
if (_ComputeEyesRange(ref localDir, _internalValues.headIK.eyesTraceTheta.cos))
{
if (localDir.y < -_internalValues.headIK.neckLimitPitchDownTheta.sin)
{
localDir.y = -_internalValues.headIK.neckLimitPitchDownTheta.sin;
}
else if (localDir.y > _internalValues.headIK.neckLimitPitchUpTheta.sin)
{
localDir.y = _internalValues.headIK.neckLimitPitchUpTheta.sin;
}
localDir.x = 0.0f;
localDir.z = SAFBIKSqrt(1.0f - localDir.y * localDir.y);
}
SAFBIKMatMultVec(out eyesDir, ref parentBaseBasis, ref localDir);
{
Vector3 xDir = parentBaseBasis.column0;
Vector3 yDir = parentBaseBasis.column1;
Vector3 zDir = eyesDir;
if (!SAFBIKComputeBasisLockZ(out neckBaseBasis, ref xDir, ref yDir, ref zDir))
{
neckBaseBasis = parentBaseBasis; // Failsafe.
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref neckBaseBasis, ref _neckBone._baseToWorldBasis);
if (_eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion neckWorldRotation = Quaternion.Lerp(_neckBone.worldRotation, worldRotation, _eyesEffector.positionWeight); // This is able to use _neckBone.worldRotation directly.
_neckBone.worldRotation = neckWorldRotation;
SAFBIKMatSetRotMult(out neckBaseBasis, ref neckWorldRotation, ref _neckBone._worldToBaseRotation);
}
else
{
_neckBone.worldRotation = worldRotation;
}
}
Matrix3x3 neckBasis;
SAFBIKMatMult(out neckBasis, ref neckBaseBasis, ref _internalValues.defaultRootBasisInv);
Vector3 neckBoneWorldPosition = _neckBone.worldPosition;
SAFBIKMatMultVecPreSubAdd(out eyesPosition, ref neckBasis, ref _eyesEffector._defaultPosition, ref _neckBone._defaultPosition, ref neckBoneWorldPosition);
// Note: Not use _eyesEffector._hidden_worldPosition
eyesDir = _eyesEffector.worldPosition - eyesPosition;
Matrix3x3 headBaseBasis = neckBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv(out localDir, ref neckBaseBasis, ref eyesDir);
localDir.x *= _settings.headIK.eyesToHeadYawRate;
localDir.y *= _settings.headIK.eyesToHeadPitchRate;
SAFBIKVecNormalize(ref localDir);
if (_ComputeEyesRange(ref localDir, _internalValues.headIK.eyesTraceTheta.cos))
{
// Note: Not use _LimitXY() for Stability
_LimitXY_Square(ref localDir,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitPitchDownTheta.sin,
_internalValues.headIK.headLimitPitchUpTheta.sin);
}
SAFBIKMatMultVec(out eyesDir, ref neckBaseBasis, ref localDir);
{
Vector3 xDir = neckBaseBasis.column0;
Vector3 yDir = neckBaseBasis.column1;
Vector3 zDir = eyesDir;
if (!SAFBIKComputeBasisLockZ(out headBaseBasis, ref xDir, ref yDir, ref zDir))
{
headBaseBasis = neckBaseBasis;
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot(out worldRotation, ref headBaseBasis, ref _headBone._baseToWorldBasis);
if (_eyesEffector.positionWeight < 1.0f - IKEpsilon)
{
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headFromWorldRotation;
SAFBIKQuatMultNorm3Inv1(out headFromWorldRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation);
Quaternion headWorldRotation = Quaternion.Lerp(headFromWorldRotation, worldRotation, _eyesEffector.positionWeight);
_headBone.worldRotation = headWorldRotation;
SAFBIKMatSetRotMult(out headBaseBasis, ref headWorldRotation, ref _headBone._worldToBaseRotation);
}
else
{
_headBone.worldRotation = worldRotation;
}
}
Matrix3x3 headBasis;
SAFBIKMatMult(out headBasis, ref headBaseBasis, ref _internalValues.defaultRootBasisInv);
if (_isEnabledCustomEyes)
{
fullBodyIK._SolveCustomEyes(ref neckBasis, ref headBasis, ref headBaseBasis);
}
else
{
_SolveEyes(ref neckBasis, ref headBasis, ref headBaseBasis, ref headBonePrevRotation, ref leftEyeBonePrevRotation, ref rightEyeBonePrevRotation);
}
}
}
void _FingerBoneField( string boneName, ref FullBodyIK.Bone[] bones, bool isOptional )
{
if( bones == null || bones.Length != 4 ) {
bones = new FullBodyIK.Bone[4];
}
for( int i = 0; i < bones.Length; ++i ) {
string name = null;
if( i + 1 == bones.Length ) {
name = boneName + " Tip";
} else {
name = boneName + " " + (i + 1).ToString();
}
_BoneField( name, ref bones[i], isOptional );
}
}
void _ComputeLocalAxis( FullBodyIK fullBodyIK )
{
// Compute _localAxisBasis for each bones.
if( this.transformIsAlive && (_parentBone != null && _parentBone.transformIsAlive) ) {
if( _localAxisFrom == _LocalAxisFrom.Parent ||
_parentBone._localAxisFrom == _LocalAxisFrom.Child ) {
Vector3 dir = _defaultLocalDirection;
if( dir.x != 0.0f || dir.y != 0.0f || dir.z != 0.0f ) {
if( _localAxisFrom == _LocalAxisFrom.Parent ) {
SAFBIKComputeBasisFrom( out _localAxisBasis, ref fullBodyIK.internalValues.defaultRootBasis, ref dir, _localDirectionAs );
}
if( _parentBone._localAxisFrom == _LocalAxisFrom.Child ) {
if( _parentBone._boneType == BoneType.Shoulder ) {
Bone shoulderBone = _parentBone;
Bone spineUBone = _parentBone._parentBone;
Bone neckBone = (fullBodyIK.headBones != null) ? fullBodyIK.headBones.neck : null;
if( neckBone != null && !neckBone.transformIsAlive ) {
neckBone = null;
}
if( fullBodyIK.internalValues.shoulderDirYAsNeck == -1 ) {
if( fullBodyIK.settings.shoulderDirYAsNeck == AutomaticBool.Auto ) {
if( spineUBone != null && neckBone != null ) {
Vector3 shoulderToSpineU = shoulderBone._defaultLocalDirection;
Vector3 shoulderToNeck = neckBone._defaultPosition - shoulderBone._defaultPosition;
if( SAFBIKVecNormalize( ref shoulderToNeck ) ) {
float shoulderToSpineUTheta = Mathf.Abs( Vector3.Dot( dir, shoulderToSpineU ) );
float shoulderToNeckTheta = Mathf.Abs( Vector3.Dot( dir, shoulderToNeck ) );
if( shoulderToSpineUTheta < shoulderToNeckTheta ) {
fullBodyIK.internalValues.shoulderDirYAsNeck = 0;
} else {
fullBodyIK.internalValues.shoulderDirYAsNeck = 1;
}
} else {
fullBodyIK.internalValues.shoulderDirYAsNeck = 0;
}
} else {
fullBodyIK.internalValues.shoulderDirYAsNeck = 0;
}
} else {
fullBodyIK.internalValues.shoulderDirYAsNeck = (fullBodyIK.settings.shoulderDirYAsNeck != AutomaticBool.Disable) ? 1 : 0;
}
}
Vector3 xDir, yDir, zDir;
xDir = (_parentBone._localDirectionAs == _DirectionAs.XMinus) ? -dir : dir;
if( fullBodyIK.internalValues.shoulderDirYAsNeck != 0 && neckBone != null ) {
yDir = neckBone._defaultPosition - shoulderBone._defaultPosition;
} else {
yDir = shoulderBone._defaultLocalDirection;
}
zDir = Vector3.Cross( xDir, yDir );
yDir = Vector3.Cross( zDir, xDir );
if( SAFBIKVecNormalize2( ref yDir, ref zDir ) ) {
_parentBone._localAxisBasis.SetColumn( ref xDir, ref yDir, ref zDir );
}
} else if( _parentBone._boneType == BoneType.Spine && _boneType != BoneType.Spine && _boneType != BoneType.Neck ) {
// Compute spine/neck only( Exclude shouder / arm ).
} else if( _parentBone._boneType == BoneType.Hips && _boneType != BoneType.Spine ) {
// Compute spine only( Exclude leg ).
} else {
if( _parentBone._boneType == BoneType.Hips ) {
Vector3 baseX = fullBodyIK.internalValues.defaultRootBasis.column0;
SAFBIKComputeBasisFromXYLockY( out _parentBone._localAxisBasis, ref baseX, ref dir );
} else if( _parentBone._boneType == BoneType.Spine || _parentBone._boneType == BoneType.Neck ) {
// Using parent axis for spine or neck. Preprocess for BodyIK.
if( _parentBone._parentBone != null ) {
Vector3 dirX = _parentBone._parentBone._localAxisBasis.column0;
SAFBIKComputeBasisFromXYLockY( out _parentBone._localAxisBasis, ref dirX, ref dir );
}
} else {
if( _localAxisFrom == _LocalAxisFrom.Parent && _localDirectionAs == _parentBone._localDirectionAs ) {
_parentBone._localAxisBasis = _localAxisBasis;
} else {
SAFBIKComputeBasisFrom( out _parentBone._localAxisBasis,
ref fullBodyIK.internalValues.defaultRootBasis, ref dir, _parentBone._localDirectionAs );
}
}
}
}
}
}
}
}
void _Prepare( FullBodyIK fullBodyIK )
{
SAFBIKQuatMultInv0( out _endEffectorToWorldRotation, ref _endEffector._defaultRotation, ref _endBone._defaultRotation );
// for _defaultCosTheta, _defaultSinTheta
_beginToBendingLength = _bendingBone._defaultLocalLength.length;
_beginToBendingLengthSq = _bendingBone._defaultLocalLength.lengthSq;
_bendingToEndLength = _endBone._defaultLocalLength.length;
_bendingToEndLengthSq = _endBone._defaultLocalLength.lengthSq;
float beginToEndLength, beginToEndLengthSq;
beginToEndLength = SAFBIKVecLengthAndLengthSq2( out beginToEndLengthSq,
ref _endBone._defaultPosition, ref _beginBone._defaultPosition );
_defaultCosTheta = ComputeCosTheta(
_bendingToEndLengthSq, // lenASq
beginToEndLengthSq, // lenBSq
_beginToBendingLengthSq, // lenCSq
beginToEndLength, // lenB
_beginToBendingLength ); // lenC
_defaultSinTheta = SAFBIKSqrtClamp01( 1.0f - _defaultCosTheta * _defaultCosTheta );
CheckNaN( _defaultSinTheta );
}
public void _ComputeDefaultTransform(FullBodyIK fullBodyIK)
{
if (_parentEffector != null)
{
_defaultRotation = _parentEffector._defaultRotation;
}
if (_effectorType == EffectorType.Root)
{
_defaultPosition = fullBodyIK.internalValues.defaultRootPosition;
_defaultRotation = fullBodyIK.internalValues.defaultRootRotation;
}
else if (_effectorType == EffectorType.HandFinger)
{
Assert(_bone != null);
if (_bone != null)
{
if (_bone.transformIsAlive)
{
_defaultPosition = bone._defaultPosition;
}
else // Failsafe. Simulate finger tips.
// Memo: If transformIsAlive == false, _parentBone is null.
{
Assert(_bone.parentBoneLocationBased != null && _bone.parentBoneLocationBased.parentBoneLocationBased != null);
if (_bone.parentBoneLocationBased != null && _bone.parentBoneLocationBased.parentBoneLocationBased != null)
{
Vector3 tipTranslate = (bone.parentBoneLocationBased._defaultPosition - bone.parentBoneLocationBased.parentBoneLocationBased._defaultPosition);
_defaultPosition = bone.parentBoneLocationBased._defaultPosition + tipTranslate;
_isSimulateFingerTips = true;
}
}
}
}
else if (_effectorType == EffectorType.Eyes)
{
Assert(_bone != null);
_isHiddenEyes = fullBodyIK._IsHiddenCustomEyes();
if (!_isHiddenEyes && _bone != null && _bone.transformIsAlive &&
_leftBone != null && _leftBone.transformIsAlive &&
_rightBone != null && _rightBone.transformIsAlive)
{
// _bone ... Head / _leftBone ... LeftEye / _rightBone ... RightEye
_defaultPosition = (_leftBone._defaultPosition + _rightBone._defaultPosition) * 0.5f;
}
else if (_bone != null && _bone.transformIsAlive)
{
_defaultPosition = _bone._defaultPosition;
// _bone ... Head / _bone.parentBone ... Neck
if (_bone.parentBone != null && _bone.parentBone.transformIsAlive && _bone.parentBone.boneType == BoneType.Neck)
{
Vector3 neckToHead = _bone._defaultPosition - _bone.parentBone._defaultPosition;
float neckToHeadY = (neckToHead.y > 0.0f) ? neckToHead.y : 0.0f;
_defaultPosition += fullBodyIK.internalValues.defaultRootBasis.column1 * neckToHeadY;
_defaultPosition += fullBodyIK.internalValues.defaultRootBasis.column2 * neckToHeadY;
}
}
}
else if (_effectorType == EffectorType.Hips)
{
Assert(_bone != null && _leftBone != null && _rightBone != null);
if (_bone != null && _leftBone != null && _rightBone != null)
{
// _bone ... Hips / _leftBone ... LeftLeg / _rightBone ... RightLeg
_defaultPosition = (_leftBone._defaultPosition + _rightBone._defaultPosition) * 0.5f;
}
}
else // Normally case.
{
Assert(_bone != null);
if (_bone != null)
{
_defaultPosition = bone._defaultPosition;
if (!_defaultLocalBasisIsIdentity) // For wrist & foot.
{
_defaultRotation = bone._localAxisRotation;
}
}
}
}
public void _ComputeDefaultTransform( FullBodyIK fullBodyIK )
{
if( _parentEffector != null ) {
_defaultRotation = _parentEffector._defaultRotation;
}
if( _effectorType == EffectorType.Root ) {
_defaultPosition = fullBodyIK.internalValues.defaultRootPosition;
_defaultRotation = fullBodyIK.internalValues.defaultRootRotation;
} else if( _effectorType == EffectorType.HandFinger ) {
Assert( _bone != null );
if( _bone != null ) {
if( _bone.transformIsAlive ) {
_defaultPosition = bone._defaultPosition;
} else { // Failsafe. Simulate finger tips.
// Memo: If transformIsAlive == false, _parentBone is null.
Assert( _bone.parentBoneLocationBased != null && _bone.parentBoneLocationBased.parentBoneLocationBased != null );
if( _bone.parentBoneLocationBased != null && _bone.parentBoneLocationBased.parentBoneLocationBased != null ) {
Vector3 tipTranslate = (bone.parentBoneLocationBased._defaultPosition - bone.parentBoneLocationBased.parentBoneLocationBased._defaultPosition);
_defaultPosition = bone.parentBoneLocationBased._defaultPosition + tipTranslate;
_isSimulateFingerTips = true;
}
}
}
} else if( _effectorType == EffectorType.Eyes ) {
Assert( _bone != null );
_isHiddenEyes = fullBodyIK._IsHiddenCustomEyes();
if( !_isHiddenEyes && _bone != null && _bone.transformIsAlive &&
_leftBone != null && _leftBone.transformIsAlive &&
_rightBone != null && _rightBone.transformIsAlive ) {
// _bone ... Head / _leftBone ... LeftEye / _rightBone ... RightEye
_defaultPosition = (_leftBone._defaultPosition + _rightBone._defaultPosition) * 0.5f;
} else if( _bone != null && _bone.transformIsAlive ) {
_defaultPosition = _bone._defaultPosition;
// _bone ... Head / _bone.parentBone ... Neck
if( _bone.parentBone != null && _bone.parentBone.transformIsAlive && _bone.parentBone.boneType == BoneType.Neck ) {
Vector3 neckToHead = _bone._defaultPosition - _bone.parentBone._defaultPosition;
float neckToHeadY = (neckToHead.y > 0.0f) ? neckToHead.y : 0.0f;
_defaultPosition += fullBodyIK.internalValues.defaultRootBasis.column1 * neckToHeadY;
_defaultPosition += fullBodyIK.internalValues.defaultRootBasis.column2 * neckToHeadY;
}
}
} else if( _effectorType == EffectorType.Hips ) {
Assert( _bone != null && _leftBone != null && _rightBone != null );
if( _bone != null && _leftBone != null && _rightBone != null ) {
// _bone ... Hips / _leftBone ... LeftLeg / _rightBone ... RightLeg
_defaultPosition = (_leftBone._defaultPosition + _rightBone._defaultPosition) * 0.5f;
}
} else { // Normally case.
Assert( _bone != null );
if( _bone != null ) {
_defaultPosition = bone._defaultPosition;
if( !_defaultLocalBasisIsIdentity ) { // For wrist & foot.
_defaultRotation = bone._localAxisRotation;
}
}
}
}
public void Prepare( FullBodyIK fullBodyIK )
{
Assert( fullBodyIK != null );
_ClearInternal();
_ComputeDefaultTransform( fullBodyIK );
// Reset transform.
if( this.transformIsAlive ) {
if( _effectorType == EffectorType.Eyes ) {
this.transform.position = _defaultPosition + fullBodyIK.internalValues.defaultRootBasis.column2 * Eyes_DefaultDistance;
} else {
this.transform.position = _defaultPosition;
}
if( !_defaultLocalBasisIsIdentity ) {
this.transform.rotation = _defaultRotation;
} else {
this.transform.localRotation = Quaternion.identity;
}
this.transform.localScale = Vector3.one;
}
_worldPosition = _defaultPosition;
_worldRotation = _defaultRotation;
if( _effectorType == EffectorType.Eyes ) {
_worldPosition += fullBodyIK.internalValues.defaultRootBasis.column2 * Eyes_DefaultDistance;
}
}
public FingerIK( FullBodyIK fullBodyIK, FingerIKType fingerIKType )
{
_fingerIKType = fingerIKType;
_settings = fullBodyIK.settings;
_internalValues = fullBodyIK.internalValues;
FingersBones fingerBones = null;
FingersEffectors fingerEffectors = null;
switch( fingerIKType ) {
case FingerIKType.LeftWrist:
_parentBone = fullBodyIK.leftArmBones.wrist;
fingerBones = fullBodyIK.leftHandFingersBones;
fingerEffectors = fullBodyIK.leftHandFingersEffectors;
break;
case FingerIKType.RightWrist:
_parentBone = fullBodyIK.rightArmBones.wrist;
fingerBones = fullBodyIK.rightHandFingersBones;
fingerEffectors = fullBodyIK.rightHandFingersEffectors;
break;
}
_notThumb1PitchUTraceSmooth = new FastAngle( _notThumb1PitchUTrace.angle + _notThumb1PitchUSmooth.angle );
if( fingerBones != null && fingerEffectors != null ) {
for( int fingerType = 0; fingerType < (int)FingerType.Max; ++fingerType ) {
Bone[] bones = null;
Effector effector = null;
switch( fingerType ) {
case (int)FingerType.Thumb:
bones = fingerBones.thumb;
effector = fingerEffectors.thumb;
break;
case (int)FingerType.Index:
bones = fingerBones.index;
effector = fingerEffectors.index;
break;
case (int)FingerType.Middle:
bones = fingerBones.middle;
effector = fingerEffectors.middle;
break;
case (int)FingerType.Ring:
bones = fingerBones.ring;
effector = fingerEffectors.ring;
break;
case (int)FingerType.Little:
bones = fingerBones.little;
effector = fingerEffectors.little;
break;
}
if( bones != null && effector != null ) {
_PrepareBranch( fingerType, bones, effector );
}
}
}
}
public void _SyncDisplacement( FullBodyIK fullBodyIK )
{
Assert( fullBodyIK != null );
Vector3 hipsOffset0 = _GetHipsOffset( 0, fullBodyIK.leftLegBones.leg, fullBodyIK.leftLegBones.knee, fullBodyIK.leftLegBones.foot );
Vector3 hipsOffset1 = _GetHipsOffset( 1, fullBodyIK.rightLegBones.leg, fullBodyIK.rightLegBones.knee, fullBodyIK.rightLegBones.foot );
hipsOffset = (hipsOffset0 + hipsOffset1) * 0.5f;
}
public bool Solve(FullBodyIK fullBodyIK)
{
if (_neckBone == null || !_neckBone.transformIsAlive ||
_headBone == null || !_headBone.transformIsAlive ||
_headBone.parentBone == null || !_headBone.parentBone.transformIsAlive)
{
return(false);
}
_SyncDisplacement(fullBodyIK);
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
if (headPositionWeight <= IKEpsilon && eyesPositionWeight <= IKEpsilon)
{
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Quaternion parentBaseRotation;
SAFBIKQuatMult(out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation);
if (_internalValues.resetTransforms)
{
Quaternion tempRotation;
SAFBIKQuatMult(out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation);
_neckBone.worldRotation = tempRotation;
}
float headRotationWeight = _headEffector.rotationEnabled ? _headEffector.rotationWeight : 0.0f;
if (headRotationWeight > IKEpsilon)
{
Quaternion headEffectorWorldRotation = _headEffector.worldRotation;
Quaternion toRotation;
SAFBIKQuatMult(out toRotation, ref headEffectorWorldRotation, ref _headEffectorToWorldRotation);
if (headRotationWeight < 1.0f - IKEpsilon)
{
Quaternion fromRotation;
if (_internalValues.resetTransforms)
{
SAFBIKQuatMult(out fromRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation);
}
else
{
fromRotation = _headBone.worldRotation; // This is able to use _headBone.worldRotation directly.
}
_headBone.worldRotation = Quaternion.Lerp(fromRotation, toRotation, headRotationWeight);
}
else
{
_headBone.worldRotation = toRotation;
}
_HeadRotationLimit();
}
else
{
if (_internalValues.resetTransforms)
{
Quaternion tempRotation;
SAFBIKQuatMult(out tempRotation, ref parentBaseRotation, ref _headBone._baseToWorldRotation);
_headBone.worldRotation = tempRotation;
}
}
if (_internalValues.resetTransforms)
{
if (_isEnabledCustomEyes)
{
fullBodyIK._ResetCustomEyes();
}
else
{
_ResetEyes();
}
}
return(_internalValues.resetTransforms || (headRotationWeight > IKEpsilon));
}
_Solve(fullBodyIK);
return(true);
}
public void PostSyncDisplacement( FullBodyIK fullBodyIK )
{
if( _boneLocation == BoneLocation.Hips ) {
_defaultPosition = fullBodyIK.boneCaches.defaultHipsPosition + fullBodyIK.boneCaches.hipsOffset;
} else if( _parentBone != null ) {
_defaultPosition = _parentBone._defaultPosition + _defaultLocalTranslate;
}
_ComputeLocalAxis( fullBodyIK ); // Require PostPrepare()
}
public void Prepare( FullBodyIK fullBodyIK )
{
_PrepareHipsToFootLength( 0, fullBodyIK.leftLegBones.leg, fullBodyIK.leftLegBones.knee, fullBodyIK.leftLegBones.foot, fullBodyIK.internalValues );
_PrepareHipsToFootLength( 1, fullBodyIK.rightLegBones.leg, fullBodyIK.rightLegBones.knee, fullBodyIK.rightLegBones.foot, fullBodyIK.internalValues );
if( fullBodyIK.bodyBones.hips != null ) {
defaultHipsPosition = fullBodyIK.bodyBones.hips._defaultPosition;
}
}
void _Solve( FullBodyIK fullBodyIK )
{
Quaternion parentWorldRotation = _neckBone.parentBone.worldRotation;
Matrix3x3 parentBasis;
SAFBIKMatSetRotMultInv1( out parentBasis, ref parentWorldRotation, ref _neckBone.parentBone._defaultRotation );
Matrix3x3 parentBaseBasis;
SAFBIKMatMult( out parentBaseBasis, ref parentBasis, ref _internalValues.defaultRootBasis );
Quaternion parentBaseRotation;
SAFBIKQuatMult( out parentBaseRotation, ref parentWorldRotation, ref _neckBone.parentBone._worldToBaseRotation );
float headPositionWeight = _headEffector.positionEnabled ? _headEffector.positionWeight : 0.0f;
float eyesPositionWeight = _eyesEffector.positionEnabled ? _eyesEffector.positionWeight : 0.0f;
Quaternion neckBonePrevRotation = Quaternion.identity;
Quaternion headBonePrevRotation = Quaternion.identity;
Quaternion leftEyeBonePrevRotation = Quaternion.identity;
Quaternion rightEyeBonePrevRotation = Quaternion.identity;
if( !_internalValues.resetTransforms ) {
neckBonePrevRotation = _neckBone.worldRotation;
headBonePrevRotation = _headBone.worldRotation;
if( _leftEyeBone != null && _leftEyeBone.transformIsAlive ) {
leftEyeBonePrevRotation = _leftEyeBone.worldRotation;
}
if( _rightEyeBone != null && _rightEyeBone.transformIsAlive ) {
rightEyeBonePrevRotation = _rightEyeBone.worldRotation;
}
}
// for Neck
if( headPositionWeight > IKEpsilon ) {
Matrix3x3 neckBoneBasis;
SAFBIKMatMult( out neckBoneBasis, ref parentBasis, ref _neckBone._localAxisBasis );
Vector3 yDir = _headEffector.worldPosition - _neckBone.worldPosition; // Not use _hidden_worldPosition
if( SAFBIKVecNormalize( ref yDir ) ) {
Vector3 localDir;
SAFBIKMatMultVecInv( out localDir, ref neckBoneBasis, ref yDir );
if( _LimitXZ_Square( ref localDir,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitRollTheta.sin,
_internalValues.headIK.neckLimitPitchDownTheta.sin,
_internalValues.headIK.neckLimitPitchUpTheta.sin ) ) {
SAFBIKMatMultVec( out yDir, ref neckBoneBasis, ref localDir );
}
Vector3 xDir = parentBaseBasis.column0;
Vector3 zDir = parentBaseBasis.column2;
if( SAFBIKComputeBasisLockY( out neckBoneBasis, ref xDir, ref yDir, ref zDir ) ) {
Quaternion worldRotation;
SAFBIKMatMultGetRot( out worldRotation, ref neckBoneBasis, ref _neckBone._boneToWorldBasis );
if( headPositionWeight < 1.0f - IKEpsilon ) {
Quaternion fromRotation;
if( _internalValues.resetTransforms ) {
SAFBIKQuatMult( out fromRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation );
} else {
fromRotation = neckBonePrevRotation; // This is able to use _headBone.worldRotation directly.
}
_neckBone.worldRotation = Quaternion.Lerp( fromRotation, worldRotation, headPositionWeight );
} else {
_neckBone.worldRotation = worldRotation;
}
}
}
} else if( _internalValues.resetTransforms ) {
Quaternion tempRotation;
SAFBIKQuatMult( out tempRotation, ref parentBaseRotation, ref _neckBone._baseToWorldRotation );
_neckBone.worldRotation = tempRotation;
}
// for Head / Eyes
if( eyesPositionWeight <= IKEpsilon ) {
float headRotationWeight = _headEffector.rotationEnabled ? _headEffector.rotationWeight : 0.0f;
if( headRotationWeight > IKEpsilon ) {
Quaternion headEffectorWorldRotation = _headEffector.worldRotation;
Quaternion toRotation;
SAFBIKQuatMult( out toRotation, ref headEffectorWorldRotation, ref _headEffectorToWorldRotation );
if( headRotationWeight < 1.0f - IKEpsilon ) {
Quaternion fromRotation;
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
if( _internalValues.resetTransforms ) {
SAFBIKQuatMult3( out fromRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation );
} else {
// Not use _headBone.worldRotation.
SAFBIKQuatMultNorm3Inv1( out fromRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation );
}
_headBone.worldRotation = Quaternion.Lerp( fromRotation, toRotation, headRotationWeight );
} else {
_headBone.worldRotation = toRotation;
}
} else {
if( _internalValues.resetTransforms ) {
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headBoneWorldRotation;
SAFBIKQuatMult3( out headBoneWorldRotation, ref neckBoneWorldRotation, ref _neckBone._worldToBaseRotation, ref _headBone._baseToWorldRotation );
_headBone.worldRotation = headBoneWorldRotation;
}
}
_HeadRotationLimit();
if( _internalValues.resetTransforms ) {
if( _isEnabledCustomEyes ) {
fullBodyIK._ResetCustomEyes();
} else {
_ResetEyes();
}
}
return;
}
{
Vector3 eyesPosition, parentBoneWorldPosition = _neckBone.parentBone.worldPosition;
SAFBIKMatMultVecPreSubAdd( out eyesPosition, ref parentBasis, ref _eyesEffector._defaultPosition, ref _neckBone.parentBone._defaultPosition, ref parentBoneWorldPosition );
// Note: Not use _eyesEffector._hidden_worldPosition
Vector3 eyesDir = _eyesEffector.worldPosition - eyesPosition; // Memo: Not normalize yet.
Matrix3x3 neckBaseBasis = parentBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv( out localDir, ref parentBaseBasis, ref eyesDir );
localDir.y *= _settings.headIK.eyesToNeckPitchRate;
SAFBIKVecNormalize( ref localDir );
if( _ComputeEyesRange( ref localDir, _internalValues.headIK.eyesTraceTheta.cos ) ) {
if( localDir.y < -_internalValues.headIK.neckLimitPitchDownTheta.sin ) {
localDir.y = -_internalValues.headIK.neckLimitPitchDownTheta.sin;
} else if( localDir.y > _internalValues.headIK.neckLimitPitchUpTheta.sin ) {
localDir.y = _internalValues.headIK.neckLimitPitchUpTheta.sin;
}
localDir.x = 0.0f;
localDir.z = SAFBIKSqrt( 1.0f - localDir.y * localDir.y );
}
SAFBIKMatMultVec( out eyesDir, ref parentBaseBasis, ref localDir );
{
Vector3 xDir = parentBaseBasis.column0;
Vector3 yDir = parentBaseBasis.column1;
Vector3 zDir = eyesDir;
if( !SAFBIKComputeBasisLockZ( out neckBaseBasis, ref xDir, ref yDir, ref zDir ) ) {
neckBaseBasis = parentBaseBasis; // Failsafe.
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot( out worldRotation, ref neckBaseBasis, ref _neckBone._baseToWorldBasis );
if( _eyesEffector.positionWeight < 1.0f - IKEpsilon ) {
Quaternion neckWorldRotation = Quaternion.Lerp( _neckBone.worldRotation, worldRotation, _eyesEffector.positionWeight ); // This is able to use _neckBone.worldRotation directly.
_neckBone.worldRotation = neckWorldRotation;
SAFBIKMatSetRotMult( out neckBaseBasis, ref neckWorldRotation, ref _neckBone._worldToBaseRotation );
} else {
_neckBone.worldRotation = worldRotation;
}
}
Matrix3x3 neckBasis;
SAFBIKMatMult( out neckBasis, ref neckBaseBasis, ref _internalValues.defaultRootBasisInv );
Vector3 neckBoneWorldPosition = _neckBone.worldPosition;
SAFBIKMatMultVecPreSubAdd( out eyesPosition, ref neckBasis, ref _eyesEffector._defaultPosition, ref _neckBone._defaultPosition, ref neckBoneWorldPosition );
// Note: Not use _eyesEffector._hidden_worldPosition
eyesDir = _eyesEffector.worldPosition - eyesPosition;
Matrix3x3 headBaseBasis = neckBaseBasis;
{
Vector3 localDir;
SAFBIKMatMultVecInv( out localDir, ref neckBaseBasis, ref eyesDir );
localDir.x *= _settings.headIK.eyesToHeadYawRate;
localDir.y *= _settings.headIK.eyesToHeadPitchRate;
SAFBIKVecNormalize( ref localDir );
if( _ComputeEyesRange( ref localDir, _internalValues.headIK.eyesTraceTheta.cos ) ) {
// Note: Not use _LimitXY() for Stability
_LimitXY_Square( ref localDir,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitYawTheta.sin,
_internalValues.headIK.headLimitPitchDownTheta.sin,
_internalValues.headIK.headLimitPitchUpTheta.sin );
}
SAFBIKMatMultVec( out eyesDir, ref neckBaseBasis, ref localDir );
{
Vector3 xDir = neckBaseBasis.column0;
Vector3 yDir = neckBaseBasis.column1;
Vector3 zDir = eyesDir;
if( !SAFBIKComputeBasisLockZ( out headBaseBasis, ref xDir, ref yDir, ref zDir ) ) {
headBaseBasis = neckBaseBasis;
}
}
Quaternion worldRotation;
SAFBIKMatMultGetRot( out worldRotation, ref headBaseBasis, ref _headBone._baseToWorldBasis );
if( _eyesEffector.positionWeight < 1.0f - IKEpsilon ) {
Quaternion neckBoneWorldRotation = _neckBone.worldRotation;
Quaternion headFromWorldRotation;
SAFBIKQuatMultNorm3Inv1( out headFromWorldRotation, ref neckBoneWorldRotation, ref neckBonePrevRotation, ref headBonePrevRotation );
Quaternion headWorldRotation = Quaternion.Lerp( headFromWorldRotation, worldRotation, _eyesEffector.positionWeight );
_headBone.worldRotation = headWorldRotation;
SAFBIKMatSetRotMult( out headBaseBasis, ref headWorldRotation, ref _headBone._worldToBaseRotation );
} else {
_headBone.worldRotation = worldRotation;
}
}
Matrix3x3 headBasis;
SAFBIKMatMult( out headBasis, ref headBaseBasis, ref _internalValues.defaultRootBasisInv );
if( _isEnabledCustomEyes ) {
fullBodyIK._SolveCustomEyes( ref neckBasis, ref headBasis, ref headBaseBasis );
} else {
_SolveEyes( ref neckBasis, ref headBasis, ref headBaseBasis, ref headBonePrevRotation, ref leftEyeBonePrevRotation, ref rightEyeBonePrevRotation );
}
}
}
