// 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); }