// Excecutable in Inspector.
		public virtual void Prefix()
		{
			if( _cache_fullBodyIK == null ) {
				_cache_fullBodyIK = fullBodyIK;
			}
			if( _cache_fullBodyIK != null ) {
				_cache_fullBodyIK.Prefix( this.transform );
			}
		}
		void OnDrawGizmos()
		{
			if( _cache_fullBodyIK == null ) {
				_cache_fullBodyIK = fullBodyIK;
			}
			if( _cache_fullBodyIK != null ) {
				_cache_fullBodyIK.DrawGizmos();
			}
		}
		protected virtual void OnDestroy()
		{
			if( _cache_fullBodyIK == null ) {
				_cache_fullBodyIK = fullBodyIK;
			}
			if( _cache_fullBodyIK != null ) {
				_cache_fullBodyIK.Destroy();
			}
		}
Example #4
0
			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;
            }
		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();
		}
Example #8
0
			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 );
                }
			}
Example #9
0
			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 );
			}
Example #10
0
			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;
						}
					}
				}
			}
Example #11
0
			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;
				}
			}
Example #12
0
			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 );
						}
					}
				}
			}
Example #13
0
			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;
			}
Example #14
0
			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;
				}
			}
Example #15
0
			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 );
			}
Example #16
0
			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;
			}
		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 );
			}
		}
Example #18
0
			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 _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();
		}
Example #20
0
			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()
			}
Example #21
0
			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 );
										}
									}
								}
							}

						}
					}
				}
			}
Example #22
0
			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()
			}
Example #23
0
			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;
						}
					}
				}
			}
Example #24
0
			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 );
					}
				}
			}