// Unity's order of rotations is ZXY
// A "blocked axis" can be both an axis that was not available to begin with or an axis that reached its limit
private void HandleNotNullRotation(ref Vector3 rotation, ref Vector3 dir)
{
if (Mathf.Approximately(rotation.x, 360.0f))
{
rotation.x = 0.0f;
}
if (Mathf.Approximately(rotation.y, 360.0f))
{
rotation.y = 0.0f;
}
if (Mathf.Approximately(rotation.z, 360.0f))
{
rotation.z = 0.0f;
}
if (Axon_Utils.ApproxVec3(rotation, new Vector3(0, 0, 0), 0.001f) == false)
{
Vector3 currDir = (Quaternion.Euler(rotation) * _origFwd).normalized;
Vector3 intermedDir = dir - currDir;
if (intermedDir.sqrMagnitude > 0.01f)
{
dir = (Quaternion.LookRotation(dir - currDir) * _origFwd).normalized;
}
}
}
private void RotateEndBone()
{
// End bone rotation calculation is the same for both cases: orient towards target
Vector3 midPos = _endBone.transform.position;
Vector3 endPos = _endBone.EndPoint.position;
Vector3 targetPos = _target.position;
float deltaAngle = Vector3.Angle(_endBone.OrigRootToEnd, targetPos - midPos);
Quaternion newEndRot = Quaternion.FromToRotation(_endBone.OrigRootToEnd, targetPos - midPos);
Vector3 newEndFwd = newEndRot * _endBone.OrigFwd;
newEndFwd = Quaternion.Inverse(_baseBone.transform.rotation) * newEndFwd;
// End bone fwd twist angle calculation
// _endBone.EulerLookDirection(newEndFwd, 0);
// _endBone.SetIdealRot();
// Vector3 tempEndPos = _endBone.EndPoint.position;
// midPos = _endBone.transform.position;
// endPos = _endBone.EndPoint.position;
// float endFwdTwistAngle = Vector3.SignedAngle(tempEndPos - midPos, targetPos - midPos, newEndFwd);
Vector3 rootToMid = _endBone.EndPoint.position - _endBone.transform.position;
float twistAngle = Vector3.SignedAngle(rootToMid, targetPos - _endBone.transform.position, newEndFwd);
_endBone.EulerLookDirection(newEndFwd, twistAngle);
Axon_Utils.DetailedLogVec(_endBone.IdealEulerAngles);
}
private void TryTwistBaseBoneForEndBone()
{
// Does endbone require a twist of basebone to get to target?
if (_baseBone.GetDegree(FreedomDegree.FreedomAxis.twist).HasValue)
{
_baseBone.SetIdealRot(false);
_endBone.SetIdealRot();
Vector3 midPos = _endBone.transform.position;
Vector3 endPos = _endBone.EndPoint.position;
Vector3 rootPos = _baseBone.transform.position;
Vector3 targetPos = _target.position;
List <FreedomDegree.FreedomAxis> degrees = _endBone.GetFreeAxes();
if (degrees.Contains(FreedomDegree.FreedomAxis.twist))
{
degrees.Remove(FreedomDegree.FreedomAxis.twist);
}
float boneTwistAngle = 0.0f;
if (degrees.Count == 1)
{
switch (degrees[0])
{
case FreedomDegree.FreedomAxis.rotX:
boneTwistAngle = Axon_Utils.AngleBetweenPlanes(_baseBone.OrigFwd, _baseBone.OrigUp, _baseBone.BoneTwistAxis, targetPos - rootPos);
break;
case FreedomDegree.FreedomAxis.rotY:
boneTwistAngle = Axon_Utils.AngleBetweenPlanes(_baseBone.OrigRight, _baseBone.OrigFwd, _baseBone.BoneTwistAxis, targetPos - rootPos);
break;
case FreedomDegree.FreedomAxis.rotZ:
boneTwistAngle = Axon_Utils.AngleBetweenPlanes(_baseBone.OrigUp, _baseBone.OrigRight, _baseBone.BoneTwistAxis, targetPos - rootPos);
break;
}
if (boneTwistAngle >= 180.0f)
{
boneTwistAngle -= 180.0f;
}
if (boneTwistAngle <= -180.0f)
{
boneTwistAngle += 180.0f;
}
// Debug.Log(boneTwistAngle);
_baseBone.EulerTwist(boneTwistAngle);
}
else
{
// ????
}
}
}
private bool RegularMoveToTarget()
{
Vector3 targetPos = _target.position;
Vector3 endPos = _endBone.EndPoint.position;
if (Vector3.Distance(targetPos, endPos) < _minTargetRange)
{
return(false);
}
Vector3 midPos = _endBone.transform.position;
Vector3 rootPos = _baseBone.transform.position;
Vector3 swivPos = _swivelTransform.position;
float distRootToTarget = Vector3.Distance(rootPos, targetPos);
float rootBoneLength = Vector3.Distance(rootPos, midPos);
float totalSystemLength = rootBoneLength + Vector3.Distance(midPos, endPos);
if (totalSystemLength > distRootToTarget)
{
Vector3 elbowPos = GetElbowPos();
RotateBaseBoneInRange(elbowPos);
}
else
{
Quaternion newRot = Quaternion.FromToRotation(_baseBone.OrigRootToEnd, targetPos - rootPos);
Vector3 newFwd = newRot * _baseBone.OrigFwd;
_baseBone.EulerLookDirection(newFwd, 0);
_baseBone.SetIdealRot(false);
float twistAngle = Axon_Utils.AngleAroundAxis(_baseBone.EndPoint.position - rootPos, targetPos - rootPos, newFwd);
_baseBone.EulerLookDirection(newFwd, twistAngle);
}
_baseBone.SetIdealRot();
RotateEndBone();
_endBone.SetIdealRot();
TryTwistBaseBoneForEndBone();
return(true);
}
