public THQuaternion(THVector3 xyz, float w)
{
this.x = xyz.x;
this.y = xyz.y;
this.z = xyz.z;
this.w = w;
}
public THVector4(THVector3 xyz, float w)
{
this.x = xyz.x;
this.y = xyz.y;
this.z = xyz.z;
this.w = w;
}
private static THQuaternion SlerpUnclamped(ref THQuaternion a, ref THQuaternion b, float t)
{
// if either input is zero, return the other.
if (a.lengthSquared == 0.0f)
{
if (b.lengthSquared == 0.0f)
{
return(identity);
}
return(b);
}
else if (b.lengthSquared == 0.0f)
{
return(a);
}
float cosHalfAngle = a.w * b.w + THVector3.Dot(a.xyz, b.xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(a);
}
else if (cosHalfAngle < 0.0f)
{
b.xyz = -b.xyz;
b.w = -b.w;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
float halfAngle = (float)Math.Acos(cosHalfAngle);
float sinHalfAngle = (float)Math.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Math.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
blendB = (float)Math.Sin(halfAngle * t) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - t;
blendB = t;
}
THQuaternion result = new THQuaternion(a.xyz * blendA + b.xyz * blendB, blendA * a.w + blendB * b.w);
if (result.lengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(identity);
}
}
/// <summary>
/// Normalize a vector
/// </summary>
/// <param name="vector3">Vector to normalize</param>
/// <returns>Normalized vector</returns>
public static THVector3 Normalize(THVector3 vector3)
{
var scale = 1f / vector3.magnitude;
vector3 *= scale;
return(vector3);
}
/// <summary>
/// Set Quaternion values<para />
/// XYZ must be in degs
/// </summary>
/// <param name="xyz">XYZ Angles</param>
public static THQuaternion CreateAndSetFromVec(THVector3 xyz)
{
var q = new THQuaternion();
q.Set(xyz);
return(q);
}
private static THVector3 NormalizeAngles(THVector3 angles)
{
angles.x = NormalizeAngle(angles.x);
angles.y = NormalizeAngle(angles.y);
angles.z = NormalizeAngle(angles.z);
return(angles);
}
public static bool operator !=(THVector3 v1, object v2)
{
if (v2.GetType() != typeof(THVector3))
{
return(true);
}
THVector3 v3 = (THVector3)v2;
return(v1.GetHashCode() != v3.GetHashCode());
}
private static void ToAxisAngleRad(THQuaternion q, out THVector3 axis, out float angle)
{
if (Math.Abs(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * (float)Math.Acos(q.w); // angle
float den = (float)Sqrt(1.0 - q.w * q.w);
if (den > 0.0001f)
{
axis = q.xyz / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = new THVector3(1, 0, 0);
}
}
/// <summary>
/// Causes a rotation <paramref name="angle"/> degs around <paramref name="axis"/>
/// </summary>
/// <param name="angle">Angle to rotate</param>
/// <param name="axis">Rotation axis</param>
/// <returns>A rotated Quaternion</returns>
public static THQuaternion AngleAxis(float angle, ref THVector3 axis)
{
if ((axis.magnitude * axis.magnitude) == 0)
{
return(identity);
}
var result = identity;
var rads = angle * degToRad;
rads *= 0.5f;
axis.Normalize();
axis = axis * (float)Sin(rads);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = (float)Cos(rads);
return(Normalize(result));
}
// from http://stackoverflow.com/questions/11492299/quaternion-to-euler-angles-algorithm-how-to-convert-to-y-up-and-between-ha
private static THQuaternion FromEulerRad(THVector3 euler)
{
var yaw = euler.x;
var pitch = euler.y;
var roll = euler.z;
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)System.Math.Sin((float)rollOver2);
float cosRollOver2 = (float)System.Math.Cos((float)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)System.Math.Sin((float)pitchOver2);
float cosPitchOver2 = (float)System.Math.Cos((float)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)System.Math.Sin((float)yawOver2);
float cosYawOver2 = (float)System.Math.Cos((float)yawOver2);
THQuaternion result;
result.x = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
result.z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.w = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
return(result);
}
/// <summary>
/// Returns this Quaternion as an Angle and Axis
/// </summary>
/// <param name="angle">Rotation angle</param>
/// <param name="axis">Rotation axis</param>
public void ToAngleAxis(out float angle, out THVector3 axis)
{
ToAxisAngleRad(this, out axis, out angle);
angle *= radToDeg;
}
/// <summary>
/// Returns a rotation that rotates z degrees around the z axis, x degrees around the x axis, and y degrees around the y axis (in that order)
/// </summary>
/// <param name="euler">Rotation</param>
public static THQuaternion Euler(THVector3 euler)
{
return(FromEulerRad(euler * degToRad));
}
/// <summary>
/// Corss product of 2 vectors
/// </summary>
/// <param name="a">First vector</param>
/// <param name="b">Second vector</param>
/// <returns>Cross product</returns>
public static THVector3 Cross(THVector3 a, THVector3 b)
{
return(UnityEngine.Vector3.Cross(a, b));
}
/// <summary>
/// Creates a rotation with <paramref name="view"/>(forward) and <paramref name="up"/> directions
/// </summary>
/// <param name="view">View direction</param>
/// <param name="up">Up direction</param>
public void SetLookRotation(THVector3 view, [System.ComponentModel.DefaultValue("THVector3.up")] THVector3 up)
{
this = LookRotation(view, up);
}
/// <summary>
/// Sets the look(forward) rotation
/// </summary>
/// <param name="view">Look rotation</param>
public void SetLookRotation(THVector3 view)
{
THVector3 up = THVector3.up;
SetLookRotation(view, up);
}
// from http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html
/// <summary>
/// Creates a rotation with the specified <paramref name="forward"/> and <paramref name="up"/> directions
/// </summary>
/// <param name="forward">Forward direction</param>
/// <param name="up">Upward direction</param>
/// <returns></returns>
private static THQuaternion LookRotation(ref THVector3 forward, ref THVector3 up)
{
// Magic
forward = THVector3.Normalize(forward);
THVector3 right = THVector3.Normalize(THVector3.Cross(up, forward));
up = THVector3.Cross(forward, right);
var m00 = right.x;
var m01 = right.y;
var m02 = right.z;
var m10 = up.x;
var m11 = up.y;
var m12 = up.z;
var m20 = forward.x;
var m21 = forward.y;
var m22 = forward.z;
float num8 = (m00 + m11) + m22;
var quaternion = new THQuaternion();
if (num8 > 0f)
{
var num = (float)System.Math.Sqrt(num8 + 1f);
quaternion.w = num * 0.5f;
num = 0.5f / num;
quaternion.x = (m12 - m21) * num;
quaternion.y = (m20 - m02) * num;
quaternion.z = (m01 - m10) * num;
return(quaternion);
}
if ((m00 >= m11) && (m00 >= m22))
{
var num7 = (float)System.Math.Sqrt(((1f + m00) - m11) - m22);
var num4 = 0.5f / num7;
quaternion.x = 0.5f * num7;
quaternion.y = (m01 + m10) * num4;
quaternion.z = (m02 + m20) * num4;
quaternion.w = (m12 - m21) * num4;
return(quaternion);
}
if (m11 > m22)
{
var num6 = (float)System.Math.Sqrt(((1f + m11) - m00) - m22);
var num3 = 0.5f / num6;
quaternion.x = (m10 + m01) * num3;
quaternion.y = 0.5f * num6;
quaternion.z = (m21 + m12) * num3;
quaternion.w = (m20 - m02) * num3;
return(quaternion);
}
var num5 = (float)System.Math.Sqrt(((1f + m22) - m00) - m11);
var num2 = 0.5f / num5;
quaternion.x = (m20 + m02) * num2;
quaternion.y = (m21 + m12) * num2;
quaternion.z = 0.5f * num5;
quaternion.w = (m01 - m10) * num2;
return(quaternion);
}
/// <summary>
/// Creates a rotation with the specified <paramref name="forward"/> direction
/// </summary>
/// <param name="forward">Forward direction</param>
/// <returns>Rotated Quaternion</returns>
public static THQuaternion LookRotation(THVector3 forward)
{
var up = THVector3.up;
return(LookRotation(ref forward, ref up));
}
/// <summary>
/// Creates a rotation with the specified <paramref name="forward"/> and <paramref name="upwards"/> directions
/// </summary>
/// <param name="forward">Forward direction</param>
/// <param name="upwards">Upward direction</param>
/// <returns>rotated Quaternion</returns>
public static THQuaternion LookRotation(THVector3 forward, [System.ComponentModel.DefaultValue("THVector3.up")] THVector3 upwards)
{
return(LookRotation(ref forward, ref upwards));
}
/// <summary>
/// Rotates this Quaternion from <paramref name="a"/> to <paramref name="b"/>
/// </summary>
/// <param name="a">Start rotation</param>
/// <param name="b">End rotation</param>
public void SetFromToRotation(THVector3 a, THVector3 b)
{
this = FromToRotation(a, b);
}
/// <summary>
/// Set Quaternion values<para />
/// XYZ must be in degs
/// </summary>
/// <param name="xyz">XYZ Angles</param>
public void Set(THVector3 xyz)
{
eulerAngles = xyz;
}
/// <summary>
/// Causes a rotation <paramref name="angle"/> degs around <paramref name="axis"/>
/// </summary>
/// <param name="angle">Angle to rotate</param>
/// <param name="axis">Axis to rotate around</param>
/// <returns>A Rotated Quaternion</returns>
public static THQuaternion AngleAxis(float angle, THVector3 axis)
{
return(AngleAxis(angle, ref axis));
}
/// <summary>
/// Creates a rotation which rotates from <paramref name="a"/> to <paramref name="b"/>
/// </summary>
/// <param name="a">Start rotation</param>
/// <param name="b">End rotation</param>
public static THQuaternion FromToRotation(THVector3 a, THVector3 b)
{
return(RotateTowards(LookRotation(a), LookRotation(b), float.MaxValue));
}
/// <summary>
/// Dot product of 2 vectors
/// </summary>
/// <param name="a">First vector</param>
/// <param name="b">Second vector</param>
/// <returns>Dot product</returns>
public static float Dot(THVector3 a, THVector3 b)
{
return(UnityEngine.Vector3.Dot(a, b));
}
