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);
_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()
}
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 );
_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()
}