public void SyncDisplacement()
{
if (_parentBone != null && _parentBone.transformIsAlive && transformIsAlive)
{
Vector3 translate = this.worldPosition - _parentBone.worldPosition;
_defaultLocalLength = FastLength.FromVector3(ref translate);
if (_parentBone.transform == this.transform.parent)
{
Vector3 localPosition = this.transform.localPosition;
if (SAFBIKVecNormalize(ref localPosition))
{
Vector3 tempDirection;
SAFBIKMatMultVec(out tempDirection, ref _parentBone._defaultBasis, ref localPosition);
_defaultLocalDirection = tempDirection;
_defaultLocalTranslate = tempDirection * _defaultLocalLength.length;
}
else
{
_defaultLocalDirection = Vector3.zero;
_defaultLocalTranslate = Vector3.zero;
}
}
else
{
_defaultLocalTranslate = _defaultLocalDirection * _defaultLocalLength.length;
}
}
}
static bool _KeepMaxLength( ref Vector3 posTo, ref Vector3 posFrom, ref FastLength keepLength )
{
Vector3 v = posTo - posFrom;
float len = SAFBIKVecLength( ref v );
if( len > IKEpsilon && len > keepLength.length ) {
v = v * (keepLength.length / len);
posTo = posFrom + v;
return true;
}
return false;
}
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()
}
static bool _SolveTargetBeginPos( ref Vector3 targetBeginPos, ref Vector3 targetEndPos, ref FastLength targetBeginToEndLength, float endPull )
{
Vector3 beginToEnd = (targetEndPos - targetBeginPos);
float beginToEndLengthSq = beginToEnd.sqrMagnitude;
if( beginToEndLengthSq > targetBeginToEndLength.lengthSq + FLOAT_EPSILON ) {
float beginToEndLength = SAFBIKSqrt( beginToEndLengthSq );
if( beginToEndLength > IKEpsilon ) {
float tempLength = beginToEndLength - targetBeginToEndLength.length;
tempLength = tempLength / beginToEndLength;
if( tempLength > IKEpsilon ) {
if( endPull < 1.0f - IKEpsilon ) {
tempLength *= endPull;
}
targetBeginPos += beginToEnd * tempLength;
return true;
}
}
}
return false;
}
public void SyncDisplacement()
{
if( _parentBone != null && _parentBone.transformIsAlive && transformIsAlive ) {
Vector3 translate = this.worldPosition - _parentBone.worldPosition;
_defaultLocalLength = FastLength.FromVector3( ref translate );
if( _parentBone.transform == this.transform.parent ) {
Vector3 localPosition = this.transform.localPosition;
if( SAFBIKVecNormalize( ref localPosition ) ) {
Vector3 tempDirection;
SAFBIKMatMultVec( out tempDirection, ref _parentBone._defaultBasis, ref localPosition );
_defaultLocalDirection = tempDirection;
_defaultLocalTranslate = tempDirection * _defaultLocalLength.length;
} else {
_defaultLocalDirection = Vector3.zero;
_defaultLocalTranslate = Vector3.zero;
}
} else {
_defaultLocalTranslate = _defaultLocalDirection * _defaultLocalLength.length;
}
}
}
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()
}
// Trigonometry to A
public static float ComputeSinTheta(
FastLength lenA,
FastLength lenB,
FastLength lenC )
{
float bc2 = lenB.length * lenC.length * 2.0f;
if( bc2 > IKEpsilon ) {
float cs = (lenB.lengthSq + lenC.lengthSq - lenA.lengthSq) / bc2;
return SAFBIKSqrtClamp01( 1.0f - cs * cs );
}
return 0.0f;
}
// Trigonometry to A
public static float ComputeCosTheta(
FastLength lenA,
FastLength lenB,
FastLength lenC )
{
float bc2 = lenB.length * lenC.length * 2.0f;
if( bc2 > IKEpsilon ) {
return (lenB.lengthSq + lenC.lengthSq - lenA.lengthSq) / bc2;
}
return 0.0f;
}
