//http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
public MyQuaternion ConverFromAxisAngle(MyQuaternion _A)
{
MyQuaternion Result = _A;
if (Result.W > 1)
{
_A.Normalize(Result);
}
float angle = 2 * Mathf.Acos(Result.W);
float s = Mathf.Acos(1 - Result.W * Result.W);
if (s < 0.01f)
{
Result.X = _A.X;
Result.Y = _A.Y;
Result.Z = _A.Z;
}
else
{
Result.X = _A.X / s;
Result.Y = _A.Y / s;
Result.Z = _A.Z / s;
}
return(Result);
}
public MyQuaternion Multiply(MyQuaternion _A, MyQuaternion _B)
{
MyQuaternion result = _A;
result.W = _A.W * _B.X - _A.X * _B.X - _A.Y * _B.Y - _A.Z * _B.Z;
result.X = _A.W * _B.X + _A.X * _B.W - _A.Y * _B.Z + _A.Z * _B.Y;
result.Y = _A.W * _B.Y + _A.X * _B.Z + _A.Y * _B.W - _A.Z * _B.X;
result.Z = _A.W * _B.Z - _A.X * _B.Y + _A.Y * _B.X + _A.Z * _B.W;
return(result);
}
//https://www.mathworks.com/help/aeroblks/quaternioninverse.html
public MyQuaternion InvertQuat(MyQuaternion _A)
{
MyQuaternion Inverted = _A;
float totaldiv = Mathf.Pow(Inverted.W, 2) + Mathf.Pow(Inverted.X, 2) + Mathf.Pow(Inverted.Y, 2) + Mathf.Pow(Inverted.Z, 2);
Inverted.W = Inverted.W / totaldiv;
Inverted.X = -(Inverted.X / totaldiv);
Inverted.Y = -(Inverted.Y / totaldiv);
Inverted.Z = -(Inverted.Y / totaldiv);
return(Inverted);
}
//https://www.mathworks.com/help/aeroblks/quaternionnormalize.html
public MyQuaternion Normalize(MyQuaternion _A)
{
MyQuaternion Result = _A;
float total = Mathf.Pow(Result.W, 2) + Mathf.Pow(Result.X, 2) + Mathf.Pow(Result.Y, 2) + Mathf.Pow(Result.Z, 2);
Result.W /= Mathf.Sqrt(total);
Result.X /= Mathf.Sqrt(total);
Result.Y /= Mathf.Sqrt(total);
Result.Z /= Mathf.Sqrt(total);
return(Result);
}
//http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
public MyQuaternion ConvertToAxisAngle(MyQuaternion _A, float _ALFA)
{
MyQuaternion Result = _A;
Result.W = Mathf.Cos(_ALFA / 2);//definicion del angulo teta
Result.X = Result.X * Mathf.Sin(_ALFA / 2);
Result.Y = Result.Y * Mathf.Sin(_ALFA / 2);
Result.Z = Result.Z * Mathf.Sin(_ALFA / 2);
Result.Normalize(Result);
return(Result);
}
