public float DotProduct(MadeQuaternion otherQuat)
{
float product = 0;
product = quat.w * otherQuat.quat.w + quat.x * otherQuat.quat.x + quat.y * otherQuat.quat.y + quat.z * otherQuat.quat.z;
return(product);
}
public MadeQuaternion EulerToQuat(Vector3 vec3Euler)
{
MadeQuaternion newQuat = new MadeQuaternion();
/*
* (yaw, pitch, roll) = (r[0], r[1], r[2])
* qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
* qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
* qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
* qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
* return [qx, qy, qz, qw]
*/
float xTheta = vec3Euler.x / 2 * Mathf.Deg2Rad;
float yTheta = vec3Euler.y / 2 * Mathf.Deg2Rad;
float zTheta = vec3Euler.z / 2 * Mathf.Deg2Rad;
float xSinTheta = Mathf.Sin(xTheta);
float xCosTheta = Mathf.Cos(xTheta);
float ySinTheta = Mathf.Sin(yTheta);
float yCosTheta = Mathf.Cos(yTheta);
float zSinTheta = Mathf.Sin(zTheta);
float zCosTheta = Mathf.Cos(zTheta);
newQuat.quat.x = xSinTheta * yCosTheta * zCosTheta - xCosTheta * ySinTheta * zSinTheta;
newQuat.quat.y = xCosTheta * ySinTheta * zCosTheta + xSinTheta * yCosTheta * zSinTheta;
newQuat.quat.z = xCosTheta * yCosTheta * zSinTheta - xSinTheta * ySinTheta * zCosTheta;
newQuat.quat.w = xCosTheta * yCosTheta * zCosTheta + xSinTheta * ySinTheta * zSinTheta;
return(newQuat);
}
public static MadeQuaternion operator*(MadeQuaternion myQuat, float scalar)
{
MadeQuaternion newQuat = new MadeQuaternion();
newQuat.quat = scalar * myQuat.quat;
return(newQuat);
}
public static MadeQuaternion operator*(MadeQuaternion myQuat, MadeQuaternion otherQuat)
{
MadeQuaternion newQuat = new MadeQuaternion();
float myQuatW = myQuat.quat.w;
float otherQuatW = otherQuat.quat.w;
Vector3 myQuatXYZ = new Vector3(myQuat.quat.x, myQuat.quat.y, myQuat.quat.z);
Vector3 otherQuatXYZ = new Vector3(otherQuat.quat.x, otherQuat.quat.y, otherQuat.quat.z);
float newW = myQuatW * otherQuatW - Vector3.Dot(myQuatXYZ, otherQuatXYZ);
Vector3 newQuatXYZ = myQuatW * otherQuatXYZ + otherQuatW * myQuatXYZ + Vector3.Cross(myQuatXYZ, otherQuatXYZ);
newQuat.quat = new Vector4(newQuatXYZ.x, newQuatXYZ.y, newQuatXYZ.z, newW);
return(newQuat);
}
