/* Copyright (C) Itseez3D, Inc. - All Rights Reserved
* You may not use this file except in compliance with an authorized license
* Unauthorized copying of this file, via any medium is strictly prohibited
* Proprietary and confidential
* UNLESS REQUIRED BY APPLICABLE LAW OR AGREED BY ITSEEZ3D, INC. IN WRITING, SOFTWARE DISTRIBUTED UNDER THE LICENSE IS DISTRIBUTED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OR
* CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED
* See the License for the specific language governing permissions and limitations under the License.
* Written by Itseez3D, Inc. <support@avatarsdk.com>, August 2026
*/

using UnityEngine;

namespace AvatarSDKMove
{
	public static class QuaternionExt
	{
		private const float FloatMin = 1e-10f;

		public static Quaternion FromToRotation(Vector3 from, Vector3 to)
		{
			float theta = Vector3.Dot(from.normalized, to.normalized);
			if (theta >= 1.0f)
				return Quaternion.identity;

			if (theta <= -1.0f)
			{
				Vector3 axis = Vector3.Cross(from, Vector3.right);
				if (axis.sqrMagnitude == 0.0f)
					axis = Vector3.Cross(from, Vector3.up);

				return Quaternion.AngleAxis(180.0f, axis);
			}

			return Quaternion.AngleAxis(Mathf.Acos(theta) * Mathf.Rad2Deg, Vector3.Cross(from, to).normalized);
		}

		public static Quaternion NormalizeSafe(Quaternion q)
		{
			float dot = Quaternion.Dot(q, q);
			if (dot <= FloatMin)
				return Quaternion.identity;

			float rsqrt = 1.0f / Mathf.Sqrt(dot);
			return new Quaternion(q.x * rsqrt, q.y * rsqrt, q.z * rsqrt, q.w * rsqrt);
		}
	}
}
