/// <summary>
/// 현재 호출한 Bone Unit을 시작으로 Tail 방향으로 World를 갱신한다.
/// Parent의 PosW, AngleWorld가 갱신되었어야 한다.
/// IK의 핵심이 되는 _angleLocal_Next가 계산된 상태여야 한다.
/// </summary>
public void CalculateWorldRecursive()
{
if (_parentChainUnit != null)
{
//Parent 기준으로 Pos를 갱신한다.
_parentPosW = _parentChainUnit._bonePosW;
_angleWorld_Parent = _parentChainUnit._angleWorld_Next;
_bonePosW.x = _parentPosW.x + _parentChainUnit._lengthBoneToTarget * Mathf.Cos(_angleWorld_Parent * Mathf.Deg2Rad);
_bonePosW.y = _parentPosW.y + _parentChainUnit._lengthBoneToTarget * Mathf.Sin(_angleWorld_Parent * Mathf.Deg2Rad);
}
//Local Angle에 따라 World Angle을 갱신한다.
_angleWorld_Next = _angleLocal_Next + _angleWorld_Parent;
//Child Unit도 같이 갱신해주자
if (_childChainUnit != null)
{
_childChainUnit.CalculateWorldRecursive();
}
else
{
//엥 여기가 Tail인가염
_targetPosW.x = _bonePosW.x + _lengthBoneToTarget * Mathf.Cos(_angleWorld_Next * Mathf.Deg2Rad);
_targetPosW.y = _bonePosW.y + _lengthBoneToTarget * Mathf.Sin(_angleWorld_Next * Mathf.Deg2Rad);
}
}
/// <summary>
/// IK를 시뮬레이션한다.
/// 요청한 Bone을 Tail로 하여 Head까지 처리한다.
/// 결과값은 Delta Angle로 나오며, 이 값을 참조하여 결정한다. (Matrix 중 어디에 쓸지는 외부에서 결정)
/// </summary>
/// <param name="targetPosW"></param>
public bool SimulateIK(Vector2 targetPosW, bool isContinuous)
{
if (!_bone._isIKTail)
{
//Debug.LogError("Failed 1 - _bone._isIKTail : " + _bone._isIKTail);
return(false);
}
if (_chainUnits.Count == 0)
{
//Debug.LogError("Failed 2 - _chainUnits.Count : " + _chainUnits.Count);
return(false);
}
apOptBoneIKChainUnit chainUnit = null;
//[Tail] .....[] .... [Head]
//Tail에 가까운(인덱스가 가장 작은) Constraint가 적용된 Bone을 구한다.
//Head에 가까운(인덱스가 가장 큰) Constraint가 적용된 Bone을 구한다.
float lengthTotal = 0.0f;
//1. Simulate 준비
//Debug.Log("ReadyToSimulate : " + _chainUnits.Count);
for (int i = 0; i < _chainUnits.Count; i++)
{
chainUnit = _chainUnits[i];
chainUnit.ReadyToSimulate();
lengthTotal += chainUnit._lengthBoneToTarget;
}
if (_tailChainUnit == null)
{
_tailChainUnit = _chainUnits[0];
}
if (_headChainUnit == null)
{
_headChainUnit = _chainUnits[_chainUnits.Count - 1];
}
//1. 길이 확인 후 압축을 해야하는지 적용
float length2Target = (targetPosW - _headChainUnit._bonePosW).magnitude;
//Debug.Log("1) Length to Target : " + length2Target);
//Debug.Log("Target PosW : " + targetPosW + " <- Head : " + _headChainUnit._bonePosW + " [" + _headChainUnit._baseBone._name + "]");
float length2Tail = (_tailChainUnit._targetPosW - _headChainUnit._bonePosW).magnitude;
//Debug.Log("2) Length to Tail : " + length2Tail);
//Debug.Log("Tail : " + _tailChainUnit._targetPosW + " [" + _tailChainUnit._baseBone._name + "] <- Head : " + _headChainUnit._bonePosW + " [" + _headChainUnit._baseBone._name + "]");
if (length2Tail == 0.0f)
{
//Debug.LogError("Failed 3 - length2Tail : " + length2Tail);
//Debug.LogError("_tailChainUnit._targetPosW : " + _tailChainUnit._targetPosW);
//Debug.LogError("_headChainUnit._bonePosW : " + _headChainUnit._bonePosW);
return(false);
}
float beforSqrDist = (targetPosW - _tailChainUnit._bonePosW).sqrMagnitude;
apOptBoneIKChainUnit curBoneUnit = null;
if (length2Target < lengthTotal)
{
//압축을 해야한다.
float compressRatio = Mathf.Clamp01(length2Target / lengthTotal);
//float compressAngle = Mathf.Acos(compressRatio) * Mathf.Rad2Deg;
//Vector2 dirHeadToTarget = targetPosW - _headChainUnit._bonePosW;
for (int i = 0; i < _chainUnits.Count; i++)
{
curBoneUnit = _chainUnits[i];
if (curBoneUnit._isAngleContraint)
{
curBoneUnit._angleLocal_Next = curBoneUnit._angleDir_Preferred * (1.0f - compressRatio) + curBoneUnit._angleLocal_Next + compressRatio;
//Preferred를 적용했다는 것을 알려주자
curBoneUnit._isPreferredAngleAdapted = true;
}
}
_headChainUnit.CalculateWorldRecursive();
}
else if (length2Target > lengthTotal + 1.0f) //Bias 추가해서 플래그 리셋
{
for (int i = 0; i < _chainUnits.Count; i++)
{
_chainUnits[i]._isPreferredAngleAdapted = false;
}
}
curBoneUnit = null;
int nCalculate = 1;
for (int i = 0; i < _nLoop; i++)
{
curBoneUnit = _tailChainUnit;
while (true)
{
//루프를 돕시다.
curBoneUnit.RequestIK(targetPosW, isContinuous);
curBoneUnit.CalculateWorldRecursive();
//Debug.Log(" curBoneUnit._angleWorld_Next
if (curBoneUnit._parentChainUnit != null)
{
curBoneUnit = curBoneUnit._parentChainUnit;
}
else
{
break;
}
}
//마지막으로 Tail에서 처리 한번더
curBoneUnit = _tailChainUnit;
//curBoneUnit.RequestIK(targetPosW, i, _nLoop);
curBoneUnit.RequestIK(targetPosW, isContinuous);
curBoneUnit.CalculateWorldRecursive();
nCalculate++;
}
//만약 Continuous 모드에서 각도가 너무 많이 차이가 나면 실패한 처리다.
//이전 요청 좌표와 거리가 적은 경우 유효
if (isContinuous)
{
if (_isContinuousPrevPos)
{
float distTargetDelta = Vector2.Distance(_prevTargetPosW, targetPosW);
if (distTargetDelta < CONTINUOUS_TARGET_POS_BIAS)
{
//연속된 위치 입력인 경우
//전체의 각도 크기를 구하자
float totalDeltaAngle = 0.0f;
for (int i = 0; i < _chainUnits.Count; i++)
{
totalDeltaAngle += Mathf.Abs(_chainUnits[i]._angleLocal_Delta);
}
//Debug.Log("Cont Move : " + distTargetDelta + " / Delta Angle : " + totalDeltaAngle);
if (totalDeltaAngle > CONTINUOUS_ANGLE_JUMP_LIMIT)
{
//너무 많이 움직였다.
_isContinuousPrevPos = true;
_prevTargetPosW = targetPosW;
//Debug.LogError("Angle Jump Error : Total Angle : " + totalDeltaAngle + " / Delta Target : " + distTargetDelta);
return(false);
}
}
}
_isContinuousPrevPos = true;
_prevTargetPosW = targetPosW;
}
else
{
_isContinuousPrevPos = false;
}
if (isContinuous && length2Target < lengthTotal)
{
float afterSqrdist = (_tailChainUnit._targetPosW - targetPosW).sqrMagnitude;
if (beforSqrDist * 1.2f < afterSqrdist)
{
//오히려 더 멀어졌다.
//Debug.LogError("다시 멀어졌다");
//Debug.LogError("Failed 4 - length2Target < lengthTotal : " + length2Target + " < " + lengthTotal);
return(false);
}
}
_requestedTargetPosW = _tailChainUnit._targetPosW;
_requestedBonePosW = _tailChainUnit._bonePosW;
return(true);
}
