/// <summary>
/// IK를 시뮬레이션한다.
/// 요청한 Bone을 Tail로 하여 Head까지 처리해야하나, SimulateIK()와 달리, lastCheckUnitf를 실질적인 Head로 보고 처리한다.
/// ChainUnit보다 짧은 제한적인 요청시 호출되는 함수.
/// lastCheckUnit이 없거나 Chain 내에 없으면 SimulateIK와 동일한 결과를 만든다.
/// 결과값은 Delta Angle로 나오며, 이 값을 참조하여 결정한다. (Matrix 중 어디에 쓸지는 외부에서 결정)
/// </summary>
/// <param name="targetPosW"></param>
public bool SimulateIK_Limited(Vector2 targetPosW, bool isContinuous, apBoneIKChainUnit lastCheckUnit)
{
if (!_bone._isIKTail)
{
return(false);
}
if (_chainUnits.Count == 0)
{
return(false);
}
if (!_chainUnits.Contains(lastCheckUnit))
{
return(SimulateIK(targetPosW, isContinuous));
}
//주석을 대부분 날릴테니 SimulateIK 함수를 참고하자
apBoneIKChainUnit chainUnit = null;
//[Tail] .....[] .... [Head]
float lengthTotal = 0.0f;
int iLastUnit = -1;
//1. Simulate 준비
for (int i = 0; i < _chainUnits.Count; i++)
{
chainUnit = _chainUnits[i];
chainUnit.ReadyToSimulate();
if (iLastUnit < 0)
{
//아직 제한 범위가 검색 되기 전이라면 계산에 포함
lengthTotal += chainUnit._lengthBoneToTarget;
}
//여기까지만 처리하도록 하자
if (chainUnit == lastCheckUnit)
{
iLastUnit = i;
}
}
int nLimitedChain = iLastUnit + 1;
if (nLimitedChain == 0)
{
return(false);
}
//<Head 대신 LastChainUnit을 사용한다>
//전체 루프 횟수도 변경 (기존에는 _nLoop)
int nLimitedLoop = MAX_CALCULATE_LOOP_EDITOR;
if (nLimitedChain * nLimitedLoop > MAX_TOTAL_UNIT_CALCULATE)
{
//전체 계산 횟수 (Unit * Loop)가 제한을 넘겼을 때
nLimitedLoop = MAX_TOTAL_UNIT_CALCULATE / nLimitedChain;
if (nLimitedLoop < 2)
{
nLimitedLoop = 2;
}
}
//1. 길이 확인 후 압축을 해야하는지 적용
//float length2Target = (targetPosW - _headChainUnit._bonePosW).magnitude;
float length2Target = (targetPosW - lastCheckUnit._bonePosW).magnitude;
//float length2Tail = (_tailChainUnit._targetPosW - _headChainUnit._bonePosW).magnitude;
float length2Tail = (_tailChainUnit._targetPosW - lastCheckUnit._bonePosW).magnitude;
if (length2Tail == 0.0f)
{
return(false);
}
float beforSqrDist = (targetPosW - _tailChainUnit._bonePosW).sqrMagnitude;
apBoneIKChainUnit curBoneUnit = null;
if (length2Target < lengthTotal)
{
//압축을 해야한다.
float compressRatio = Mathf.Clamp01(length2Target / lengthTotal);
float compressAngle = Mathf.Acos(compressRatio) * Mathf.Rad2Deg;
Vector2 dirHeadToTarget = targetPosW - lastCheckUnit._bonePosW;
//for (int i = 0; i < _chainUnits.Count; i++)
for (int i = 0; i < nLimitedChain; i++) //<<전체 개수가 아닌 제한적인 개수만 돌린다.
{
curBoneUnit = _chainUnits[i];
//if(curBoneUnit._isAngleContraint && !curBoneUnit._isPreferredAngleAdapted)
if (curBoneUnit._isAngleContraint)
{
curBoneUnit._angleLocal_Next = curBoneUnit._angleDir_Preferred * (1.0f - compressRatio) + curBoneUnit._angleLocal_Next + compressRatio;
//Preferred를 적용했다는 것을 알려주자
curBoneUnit._isPreferredAngleAdapted = true;
}
}
lastCheckUnit.CalculateWorldRecursive();
}
else if (length2Target > lengthTotal + 1.0f) //Bias 추가해서 플래그 리셋
{
//for (int i = 0; i < _chainUnits.Count; i++)
for (int i = 0; i < nLimitedChain; i++) //제한된 개수만 돌린다.
{
_chainUnits[i]._isPreferredAngleAdapted = false;
}
}
curBoneUnit = null;
int nCalculate = 1;
//for (int i = 0; i < _nLoop; i++)
for (int i = 0; i < nLimitedLoop; i++) //_nLoop대신 제한된 Loop횟수로 변경
{
curBoneUnit = _tailChainUnit;
//totalDeltaAngle = 0.0f;
while (true)
{
//루프를 돕시다.
//curBoneUnit.RequestIK(targetPosW, i, _nLoop);
curBoneUnit.RequestIK(targetPosW, isContinuous);
curBoneUnit.CalculateWorldRecursive();
//추가
//현재 Bone이 Last였으면 break;
if (curBoneUnit == lastCheckUnit)
{
break;
}
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("다시 멀어졌다");
return(false);
}
}
_requestedTargetPosW = _tailChainUnit._targetPosW;
_requestedBonePosW = _tailChainUnit._bonePosW;
//Debug.Log("IK Result [" + nCalculate + "] : Dist : " + dist + " (Tail : " + _tailChainUnit._targetPosW + " > Target : " + targetPosW + ")");
return(true);
}
/// <summary>
/// IK를 시뮬레이션한다.
/// 요청한 Bone을 Tail로 하여 Head까지 처리한다.
/// 결과값은 Delta Angle로 나오며, 이 값을 참조하여 결정한다. (Matrix 중 어디에 쓸지는 외부에서 결정)
/// </summary>
/// <param name="targetPosW"></param>
public bool SimulateIK(Vector2 targetPosW, bool isContinuous)
{
if (!_bone._isIKTail)
{
return(false);
}
if (_chainUnits.Count == 0)
{
return(false);
}
apBoneIKChainUnit chainUnit = null;
//[Tail] .....[] .... [Head]
//Tail에 가까운(인덱스가 가장 작은) Constraint가 적용된 Bone을 구한다.
//Head에 가까운(인덱스가 가장 큰) Constraint가 적용된 Bone을 구한다.
float lengthTotal = 0.0f;
//1. Simulate 준비
for (int i = 0; i < _chainUnits.Count; i++)
{
chainUnit = _chainUnits[i];
chainUnit.ReadyToSimulate();
lengthTotal += chainUnit._lengthBoneToTarget;
}
// 전체 Loop 연산 순서
// 1. 바로 CCD를 결정할 것인지, 아니면 각을 비튼 후에 CCD를 시작할 것인지 결정
// 체크 방법은,
// - Head Pos -> Target Pos의 Dir를 구하고
// - Request IK가 Dir 내부에 위치하는지(길이와 각도 범위) 계산
// 1-2. 만약 Dir 내부에 Request IK가 위치한다면)
// - Head 를 기준으로
// "Head의 + Angle (90 또는 Upper)과 나머지 Unit의 - Anlge (-90 또는 Lower)합이 비슷한지, 또는 그 반대인지 결정
// (이건 Ready To Simulate에서 미리 연산한다)
// (그외의 - 각도) >= (Head 각도) * 0.5 이면 된다.
// - 둘다 가능하면) 현재 Head의 Local 각도가 +인지 -인지에 따라서 미리 상한/하한치까지 구부리고 시작
// - 가능한게 하나라면) 그 각도로 비틀고 시작하자
// - 가능한게 없다면) (그외의 - 각도)
// 2. (계산된) Target Pos W의 위치가 Request IK Pos와 오차 범위 내에 위치한다 -> 종료
// 2. Loop 연산 횟수가 끝났다 -> 종료
// 3. Tail -> Head 순서로 Loop를 진행한다.
// 4. 마지막으로 Tail에서 한번 더 수행하고 끝
// 5. 2)로 돌아가서 더 처리할지 체크하고 반복한다.
//1. CCD 전에 각도를 왜곡해야하는지 판별
//수정) 이 작업은 각 마디에서 해야한다.
//다시 수정
//(Unit이 2개 이상인 경우)
//"현재 Bone 좌표를 기준"으로
//Head -> Target 길이가 Head -> Tail 길이보다 짧은 경우
//CCD는 내부로 들어오지 못하여 에러가 발생한다.
//압축 처리를 해야한다.
//Angle Constraint를 이용하자
//압축된 거리 비례를 dLenRatio라고 할때 (30% 줄어들면 0.7)
//Constraint가 없을때)
//압축되어야 하는 각도는 Cos(X) = 0.7
//X = ACos(0.7)이다.
//Head와 Tail은 X에서 X의 0.8~1.5 이내의 값을 가진다.
//그 외의 유닛은 제한을 두지 않는다. (초기값만 줄 뿐)
//Head와 Tail의 지정 각도는 X와 -X (나머지는 0으로 지정한다.
//Constraint가 있을 때)
//압축 권장 각도는 X = ACos(0.7)
//Pref가 지정된 Bone에서는 Pref가 +일때, -일때를 구분한다.
//지정 각도는 Pref이며, X가 포함되도록 영역 크기를 잡는다.
//지정 각도는 Pref이다.
//Head와 Tail의 지정각도는 X이며, +인지, -인지는 가장 가까운 Pref Bone의 값을 이용한다.
//주의) Pref가 0인 경우는 Limit 중에서 더 넓은 쪽의 부호값을 사용하며, 이때는 X를 초기값으로 한다.
//초기값으로 회전한 후에, 동적 Angle Constraint를 적용한다.
//1. 길이 확인 후 압축을 해야하는지 적용
float length2Target = (targetPosW - _headChainUnit._bonePosW).magnitude;
float length2Tail = (_tailChainUnit._targetPosW - _headChainUnit._bonePosW).magnitude;
if (length2Tail == 0.0f)
{
return(false);
}
float beforSqrDist = (targetPosW - _tailChainUnit._bonePosW).sqrMagnitude;
apBoneIKChainUnit curBoneUnit = null;
if (length2Target < lengthTotal)
{
//압축을 해야한다.
float compressRatio = Mathf.Clamp01(length2Target / lengthTotal);
float compressAngle = Mathf.Acos(compressRatio) * Mathf.Rad2Deg;
//알고리즘 다시 수정
//"직선 내에 있으면 탄력적으로 Pref 방향으로 움직인다라는 것으로 변경
//Pref 값과 그 방향을 지정해준다.
//기본적으로 +compressAngle 값을 지정
//_tailChainUnit이 있다면 처음엔 반대, 그 이후엔 Constraint의 부호를 따른다.
//bool curPlusX = true;
//if(_tailChainUnit != null)
//{
// curPlusX = !_tailChainUnit._isAngleDir_Plus;
//}
Vector2 dirHeadToTarget = targetPosW - _headChainUnit._bonePosW;
for (int i = 0; i < _chainUnits.Count; i++)
{
curBoneUnit = _chainUnits[i];
//if(curBoneUnit._isAngleContraint && !curBoneUnit._isPreferredAngleAdapted)
if (curBoneUnit._isAngleContraint)
{
//부호값 갱신
//Vector2 dirBoneToTarget = targetPosW - curBoneUnit._bonePosW;
//if (Vector2.Angle(dirHeadToTarget, dirBoneToTarget) < 5.0f)
{
//curBoneUnit._angleLocal_Next = curBoneUnit._angleDir_Preferred;
curBoneUnit._angleLocal_Next = curBoneUnit._angleDir_Preferred * (1.0f - compressRatio) + curBoneUnit._angleLocal_Next + compressRatio;
//Preferred를 적용했다는 것을 알려주자
curBoneUnit._isPreferredAngleAdapted = true;
//Debug.Log("Adapt Preferred Angle : " + curBoneUnit._baseBone._name);
}
//curPlusX = curBoneUnit._isAngleDir_Plus;
}
//else
//{
// if (curPlusX)
// {
// curBoneUnit._angleDir_Preferred = compressAngle;
// }
// else
// {
// curBoneUnit._angleDir_Preferred = -compressAngle;
// }
//}
}
_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++)
{
// 2. (계산된) Target Pos W의 위치가 Request IK Pos와 오차 범위 내에 위치한다 -> 종료
// 2. Loop 연산 횟수가 끝났다 -> 종료
// 3. Tail -> Head 순서로 Loop를 진행한다.
// 4. 마지막으로 Tail에서 한번 더 수행하고 끝
// 5. 2)로 돌아가서 더 처리할지 체크하고 반복한다.
//if ((_tailChainUnit._targetPosW - targetPosW).sqrMagnitude < BIAS_TARGET_POS_MATCH)
//{
// break;
//}
curBoneUnit = _tailChainUnit;
//totalDeltaAngle = 0.0f;
while (true)
{
//루프를 돕시다.
//curBoneUnit.RequestIK(targetPosW, i, _nLoop);
curBoneUnit.RequestIK(targetPosW, isContinuous);
curBoneUnit.CalculateWorldRecursive();
//매번 위치체크
//if((_tailChainUnit._targetPosW - targetPosW).sqrMagnitude < BIAS_TARGET_POS_MATCH)
//{
// break;
//}
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("다시 멀어졌다");
return(false);
}
}
_requestedTargetPosW = _tailChainUnit._targetPosW;
_requestedBonePosW = _tailChainUnit._bonePosW;
//Debug.Log("IK Result [" + nCalculate + "] : Dist : " + dist + " (Tail : " + _tailChainUnit._targetPosW + " > Target : " + targetPosW + ")");
return(true);
}
