664 lines
23 KiB
C#
664 lines
23 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using System.Numerics;
|
|
|
|
|
|
|
|
namespace ServerCore;
|
|
|
|
|
|
// HANDOVER: 3차원 기반 회전 함수를 제공 한다.
|
|
|
|
|
|
[Serializable]
|
|
public struct QuaternionHelper
|
|
{
|
|
const float radToDeg = (float)(180.0 / Math.PI);
|
|
const float degToRad = (float)(Math.PI / 180.0);
|
|
|
|
public const float kEpsilon = 1E-06f; // should probably be used in the 0 tests in LookRotation or Slerp
|
|
|
|
|
|
public Vector3 xyz
|
|
{
|
|
set
|
|
{
|
|
x = value.X;
|
|
y = value.Y;
|
|
z = value.Z;
|
|
}
|
|
get
|
|
{
|
|
return new Vector3(x, y, z);
|
|
}
|
|
}
|
|
|
|
public float x;
|
|
public float y;
|
|
public float z;
|
|
public float w;
|
|
|
|
public float this[Int32 index]
|
|
{
|
|
get
|
|
{
|
|
switch (index)
|
|
{
|
|
case 0:
|
|
return this.x;
|
|
case 1:
|
|
return this.y;
|
|
case 2:
|
|
return this.z;
|
|
case 3:
|
|
return this.w;
|
|
default:
|
|
throw new IndexOutOfRangeException("Invalid Quaternion index: " + index + ", can use only 0,1,2,3");
|
|
}
|
|
}
|
|
set
|
|
{
|
|
switch (index)
|
|
{
|
|
case 0:
|
|
this.x = value;
|
|
break;
|
|
case 1:
|
|
this.y = value;
|
|
break;
|
|
case 2:
|
|
this.z = value;
|
|
break;
|
|
case 3:
|
|
this.w = value;
|
|
break;
|
|
default:
|
|
throw new IndexOutOfRangeException("Invalid Quaternion index: " + index + ", can use only 0,1,2,3");
|
|
}
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// <para>The identity rotation (RO).</para>
|
|
/// </summary>
|
|
public static QuaternionHelper identity
|
|
{
|
|
get
|
|
{
|
|
return new QuaternionHelper(0f, 0f, 0f, 1f);
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// <para>Returns the euler angle representation of the rotation.</para>
|
|
/// </summary>
|
|
|
|
public Vector3 eulerAngles
|
|
{
|
|
get
|
|
{
|
|
return Vector3Helper.multiple(QuaternionHelper.toEulerRad(this), degToRad);
|
|
}
|
|
set
|
|
{
|
|
this = QuaternionHelper.fromEulerRad(Vector3Helper.multiple(value, degToRad));
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// Gets the length (magnitude) of the quaternion.
|
|
/// </summary>
|
|
/// <seealso cref="LengthSquared"/>
|
|
|
|
public float Length
|
|
{
|
|
get
|
|
{
|
|
return (float)System.Math.Sqrt(x * x + y * y + z * z + w * w);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the square of the quaternion length (magnitude).
|
|
/// </summary>
|
|
|
|
public float LengthSquared
|
|
{
|
|
get
|
|
{
|
|
return x * x + y * y + z * z + w * w;
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// <para>Constructs new Quaternion with given x,y,z,w components.</para>
|
|
/// </summary>
|
|
/// <param name="x"></param>
|
|
/// <param name="y"></param>
|
|
/// <param name="z"></param>
|
|
/// <param name="w"></param>
|
|
public QuaternionHelper(float x, float y, float z, float w)
|
|
{
|
|
this.x = x;
|
|
this.y = y;
|
|
this.x = z;
|
|
this.w = w;
|
|
}
|
|
/// <summary>
|
|
/// Construct a new Quaternion from vector and w components
|
|
/// </summary>
|
|
/// <param name="v">The vector part</param>
|
|
/// <param name="w">The w part</param>
|
|
public QuaternionHelper(Vector3 v, float w)
|
|
{
|
|
this.x = v.X;
|
|
this.y = v.Y;
|
|
this.z = v.Z;
|
|
this.w = w;
|
|
}
|
|
/// <summary>
|
|
/// <para>Set x, y, z and w components of an existing Quaternion.</para>
|
|
/// </summary>
|
|
/// <param name="new_x"></param>
|
|
/// <param name="new_y"></param>
|
|
/// <param name="new_z"></param>
|
|
/// <param name="new_w"></param>
|
|
public void set(float new_x, float new_y, float new_z, float new_w)
|
|
{
|
|
this.x = new_x;
|
|
this.y = new_y;
|
|
this.z = new_z;
|
|
this.w = new_w;
|
|
}
|
|
/// <summary>
|
|
/// Scales the Quaternion to unit length.
|
|
/// </summary>
|
|
public void normalize()
|
|
{
|
|
float scale = 1.0f / this.Length;
|
|
xyz = Vector3Helper.multiple(xyz, scale);
|
|
w = w * scale;
|
|
}
|
|
/// <summary>
|
|
/// Scale the given quaternion to unit length
|
|
/// </summary>
|
|
/// <param name="q">The quaternion to normalize</param>
|
|
/// <returns>The normalized quaternion</returns>
|
|
public static QuaternionHelper normalize(QuaternionHelper q)
|
|
{
|
|
QuaternionHelper result;
|
|
normalize(ref q, out result);
|
|
|
|
return result;
|
|
}
|
|
/// <summary>
|
|
/// Scale the given quaternion to unit length
|
|
/// </summary>
|
|
/// <param name="q">The quaternion to normalize</param>
|
|
/// <param name="result">The normalized quaternion</param>
|
|
public static void normalize(ref QuaternionHelper q, out QuaternionHelper result)
|
|
{
|
|
float scale = 1.0f / q.Length;
|
|
result = new QuaternionHelper(Vector3Helper.multiple(q.xyz, scale), q.w * scale);
|
|
}
|
|
/// <summary>
|
|
/// <para>The dot product between two rotations.</para>
|
|
/// </summary>
|
|
/// <param name="a"></param>
|
|
/// <param name="b"></param>
|
|
public static float dot(QuaternionHelper a, QuaternionHelper b)
|
|
{
|
|
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
|
}
|
|
/// <summary>
|
|
/// <para>Creates a rotation which rotates /angle/ degrees around /axis/.</para>
|
|
/// </summary>
|
|
/// <param name="angle"></param>
|
|
/// <param name="axis"></param>
|
|
public static QuaternionHelper angleAxis(float angle, Vector3 axis)
|
|
{
|
|
return QuaternionHelper.angleAxis(angle, ref axis);
|
|
}
|
|
private static QuaternionHelper angleAxis(float degress, ref Vector3 axis)
|
|
{
|
|
if (Vector3Helper.sqrMagnitude(axis) == 0.0f)
|
|
{
|
|
return identity;
|
|
}
|
|
|
|
QuaternionHelper result = identity;
|
|
var radians = degress * degToRad;
|
|
radians *= 0.5f;
|
|
Vector3Helper.normalize(axis);
|
|
axis = Vector3Helper.multiple(axis, (float)System.Math.Sin(radians));
|
|
result.x = axis.X;
|
|
result.y = axis.Y;
|
|
result.z = axis.Z;
|
|
result.w = (float)System.Math.Cos(radians);
|
|
|
|
return normalize(result);
|
|
}
|
|
public void toAngleAxis(out float angle, out Vector3 axis)
|
|
{
|
|
QuaternionHelper.toAxisAngleRad(this, out axis, out angle);
|
|
angle *= radToDeg;
|
|
}
|
|
/// <summary>
|
|
/// <para>Creates a rotation which rotates from /fromDirection/ to /toDirection/.</para>
|
|
/// </summary>
|
|
/// <param name="fromDirection"></param>
|
|
/// <param name="toDirection"></param>
|
|
public static QuaternionHelper fromToRotation(Vector3 fromDirection, Vector3 toDirection)
|
|
{
|
|
return rotateTowards(lookRotation(fromDirection), lookRotation(toDirection), float.MaxValue);
|
|
}
|
|
/// <summary>
|
|
/// <para>Creates a rotation which rotates from /fromDirection/ to /toDirection/.</para>
|
|
/// </summary>
|
|
/// <param name="fromDirection"></param>
|
|
/// <param name="toDirection"></param>
|
|
public void setFromToRotation(Vector3 fromDirection, Vector3 toDirection)
|
|
{
|
|
this = QuaternionHelper.fromToRotation(fromDirection, toDirection);
|
|
}
|
|
/// <summary>
|
|
/// <para>Creates a rotation with the specified /forward/ and /upwards/ directions.</para>
|
|
/// </summary>
|
|
/// <param name="forward">The direction to look in.</param>
|
|
/// <param name="upwards">The vector that defines in which direction up is.</param>
|
|
public static QuaternionHelper lookRotation(Vector3 forward, Vector3 upwards)
|
|
{
|
|
return QuaternionHelper.lookRotation(ref forward, ref upwards);
|
|
}
|
|
public static QuaternionHelper lookRotation(Vector3 forward)
|
|
{
|
|
Vector3 up = Vector3Helper.up;
|
|
return QuaternionHelper.lookRotation(ref forward, ref up);
|
|
}
|
|
|
|
// 유니티와 결과물이 다르므로 다시 구현 해야 한다.
|
|
// from http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html
|
|
private static QuaternionHelper lookRotation(ref Vector3 forward, ref Vector3 up)
|
|
{
|
|
Vector3Helper.normalize(forward);
|
|
|
|
Vector3 vector = forward.normalize();
|
|
Vector3 vector2 = Vector3Helper.cross(up, vector).normalize();
|
|
Vector3 vector3 = Vector3Helper.cross(vector, vector2);
|
|
|
|
var m00 = vector2.X;
|
|
var m01 = vector2.Y;
|
|
var m02 = vector2.Z;
|
|
var m10 = vector3.X;
|
|
var m11 = vector3.Y;
|
|
var m12 = vector3.Z;
|
|
var m20 = vector.X;
|
|
var m21 = vector.Y;
|
|
var m22 = vector.Z;
|
|
|
|
float num8 = (m00 + m11) + m22;
|
|
var quaternion = new QuaternionHelper();
|
|
if (num8 > 0f)
|
|
{
|
|
var num = MathHelper.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 = MathHelper.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 = MathHelper.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 = MathHelper.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;
|
|
}
|
|
public void setLookRotation(Vector3 view)
|
|
{
|
|
Vector3 up = Vector3Helper.up;
|
|
this.setLookRotation(view, up);
|
|
}
|
|
/// <summary>
|
|
/// <para>Creates a rotation with the specified /forward/ and /upwards/ directions.</para>
|
|
/// </summary>
|
|
/// <param name="view">The direction to look in.</param>
|
|
/// <param name="up">The vector that defines in which direction up is.</param>
|
|
public void setLookRotation(Vector3 view, Vector3 up)
|
|
{
|
|
this = QuaternionHelper.lookRotation(view, up);
|
|
}
|
|
/// <summary>
|
|
/// <para>Spherically interpolates between /a/ and /b/ by t. The parameter /t/ is clamped to the range [0, 1].</para>
|
|
/// </summary>
|
|
/// <param name="a"></param>
|
|
/// <param name="b"></param>
|
|
/// <param name="t"></param>
|
|
public static QuaternionHelper slerp(QuaternionHelper a, QuaternionHelper b, float t)
|
|
{
|
|
return QuaternionHelper.slerp(ref a, ref b, t);
|
|
}
|
|
private static QuaternionHelper slerp(ref QuaternionHelper a, ref QuaternionHelper b, float t)
|
|
{
|
|
if (t > 1) t = 1;
|
|
if (t < 0) t = 0;
|
|
return slerpUnclamped(ref a, ref b, t);
|
|
}
|
|
/// <summary>
|
|
/// <para>Spherically interpolates between /a/ and /b/ by t. The parameter /t/ is not clamped.</para>
|
|
/// </summary>
|
|
/// <param name="a"></param>
|
|
/// <param name="b"></param>
|
|
/// <param name="t"></param>
|
|
public static QuaternionHelper slerpUnclamped(QuaternionHelper a, QuaternionHelper b, float t)
|
|
{
|
|
return QuaternionHelper.slerpUnclamped(ref a, ref b, t);
|
|
}
|
|
private static QuaternionHelper slerpUnclamped(ref QuaternionHelper a, ref QuaternionHelper 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 + Vector3Helper.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 = Vector3Helper.multiple(b.xyz, -1);
|
|
b.w = -b.w;
|
|
cosHalfAngle = -cosHalfAngle;
|
|
}
|
|
|
|
float blendA;
|
|
float blendB;
|
|
if (cosHalfAngle < 0.99f)
|
|
{
|
|
// do proper slerp for big angles
|
|
float halfAngle = (float)System.Math.Acos(cosHalfAngle);
|
|
float sinHalfAngle = (float)System.Math.Sin(halfAngle);
|
|
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
|
|
blendA = (float)System.Math.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
|
|
blendB = (float)System.Math.Sin(halfAngle * t) * oneOverSinHalfAngle;
|
|
}
|
|
else
|
|
{
|
|
// do lerp if angle is really small.
|
|
blendA = 1.0f - t;
|
|
blendB = t;
|
|
}
|
|
|
|
QuaternionHelper result = new QuaternionHelper(Vector3Helper.multiple(a.xyz, blendA) + Vector3Helper.multiple(b.xyz, blendB), blendA * a.w + blendB * b.w);
|
|
if (result.LengthSquared > 0.0f)
|
|
return normalize(result);
|
|
else
|
|
return identity;
|
|
}
|
|
/// <summary>
|
|
/// <para>Interpolates between /a/ and /b/ by /t/ and normalizes the result afterwards. The parameter /t/ is clamped to the range [0, 1].</para>
|
|
/// </summary>
|
|
/// <param name="a"></param>
|
|
/// <param name="b"></param>
|
|
/// <param name="t"></param>
|
|
public static QuaternionHelper lerp(QuaternionHelper a, QuaternionHelper b, float t)
|
|
{
|
|
if (t > 1) t = 1;
|
|
if (t < 0) t = 0;
|
|
return slerp(ref a, ref b, t); // TODO: use lerp not slerp, "Because quaternion works in 4D. Rotation in 4D are linear" ???
|
|
}
|
|
/// <summary>
|
|
/// <para>Interpolates between /a/ and /b/ by /t/ and normalizes the result afterwards. The parameter /t/ is not clamped.</para>
|
|
/// </summary>
|
|
/// <param name="a"></param>
|
|
/// <param name="b"></param>
|
|
/// <param name="t"></param>
|
|
public static QuaternionHelper lerpUnclamped(QuaternionHelper a, QuaternionHelper b, float t)
|
|
{
|
|
return slerp(ref a, ref b, t);
|
|
}
|
|
/// <summary>
|
|
/// <para>Rotates a rotation /from/ towards /to/.</para>
|
|
/// </summary>
|
|
/// <param name="from"></param>
|
|
/// <param name="to"></param>
|
|
/// <param name="maxDegreesDelta"></param>
|
|
public static QuaternionHelper rotateTowards(QuaternionHelper from, QuaternionHelper to, float maxDegreesDelta)
|
|
{
|
|
float angled = angle(from, to);
|
|
if (angled == 0.0f) return to;
|
|
return slerpUnclamped(from, to, MathHelper.min(1.0f, maxDegreesDelta / angled));
|
|
}
|
|
/// <summary>
|
|
/// <para>Returns the Inverse of /rotation/.</para>
|
|
/// </summary>
|
|
/// <param name="rotation"></param>
|
|
public static QuaternionHelper inverse(QuaternionHelper rotation)
|
|
{
|
|
float lengthSq = rotation.LengthSquared;
|
|
if (lengthSq != 0.0)
|
|
{
|
|
float i = 1.0f / lengthSq;
|
|
return new QuaternionHelper(Vector3Helper.multiple(rotation.xyz, -i), rotation.w * i);
|
|
}
|
|
return rotation;
|
|
}
|
|
/// <summary>
|
|
/// <para>Returns a nicely formatted string of the Quaternion.</para>
|
|
/// </summary>
|
|
/// <param name="format"></param>
|
|
public override string ToString()
|
|
{
|
|
return string.Format("({0:F1}, {1:F1}, {2:F1}, {3:F1})", this.x, this.y, this.z, this.w);
|
|
}
|
|
/// <summary>
|
|
/// <para>Returns a nicely formatted string of the Quaternion.</para>
|
|
/// </summary>
|
|
/// <param name="format"></param>
|
|
public string toString(string format)
|
|
{
|
|
return string.Format("({0}, {1}, {2}, {3})", this.x.ToString(format), this.y.ToString(format), this.z.ToString(format), this.w.ToString(format));
|
|
}
|
|
/// <summary>
|
|
/// <para>Returns the angle in degrees between two rotations /a/ and /b/.</para>
|
|
/// </summary>
|
|
/// <param name="a"></param>
|
|
/// <param name="b"></param>
|
|
public static float angle(QuaternionHelper a, QuaternionHelper b)
|
|
{
|
|
float f = QuaternionHelper.dot(a, b);
|
|
return MathHelper.acos(MathHelper.min(MathHelper.abs(f), 1f)) * 2f * radToDeg;
|
|
}
|
|
/// <summary>
|
|
/// <para>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).</para>
|
|
/// </summary>
|
|
/// <param name="x"></param>
|
|
/// <param name="y"></param>
|
|
/// <param name="z"></param>
|
|
public static QuaternionHelper euler(float x, float y, float z)
|
|
{
|
|
return QuaternionHelper.fromEulerRad(Vector3Helper.multiple(new Vector3((float)x, (float)y, (float)z), degToRad));
|
|
}
|
|
/// <summary>
|
|
/// <para>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).</para>
|
|
/// </summary>
|
|
/// <param name="euler"></param>
|
|
public static QuaternionHelper euler(Vector3 euler)
|
|
{
|
|
return QuaternionHelper.fromEulerRad(Vector3Helper.multiple(euler, degToRad));
|
|
}
|
|
|
|
// 유니티와 결과물이 다르므로 다시 구현 해야 한다.
|
|
// from http://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
|
|
private static Vector3 toEulerRad(QuaternionHelper q1)
|
|
{
|
|
ArgumentNullReferenceCheckHelper.throwIfNull(q1, () => $"q1 is null !!!");
|
|
|
|
float sqw = q1.w * q1.w;
|
|
float sqx = q1.x * q1.x;
|
|
float sqy = q1.y * q1.y;
|
|
float sqz = q1.z * q1.z;
|
|
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
|
|
float test = q1.x * q1.w - q1.y * q1.z;
|
|
Vector3 v;
|
|
|
|
if (test > 0.4995f * unit)
|
|
{ // singularity at north pole
|
|
v.Y = 2f * MathHelper.atan2(q1.y, q1.x);
|
|
v.X = MathHelper.PI / 2;
|
|
v.Z = 0;
|
|
return normalizeAngles(Vector3Helper.multiple(v, MathHelper.Rad2Deg));
|
|
}
|
|
if (test < -0.4995f * unit)
|
|
{ // singularity at south pole
|
|
v.Y = -2f * MathHelper.atan2(q1.y, q1.x);
|
|
v.X = -MathHelper.PI / 2;
|
|
v.Z = 0;
|
|
return normalizeAngles(Vector3Helper.multiple(v, MathHelper.Rad2Deg));
|
|
}
|
|
QuaternionHelper q = new QuaternionHelper(q1.w, q1.z, q1.x, q1.y);
|
|
v.Y = MathHelper.atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
|
|
v.X = MathHelper.asin(2f * (q.x * q.z - q.w * q.y)); // Pitch
|
|
v.Z = MathHelper.atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll
|
|
return normalizeAngles(Vector3Helper.multiple(v, MathHelper.Rad2Deg));
|
|
}
|
|
private static Vector3 normalizeAngles(Vector3 angles)
|
|
{
|
|
angles.X = normalizeAngle(angles.Y);
|
|
angles.Y = normalizeAngle(angles.Y);
|
|
angles.Z = normalizeAngle(angles.Z);
|
|
return angles;
|
|
}
|
|
private static float normalizeAngle(float angle)
|
|
{
|
|
while (angle > 360)
|
|
angle -= 360;
|
|
while (angle < 0)
|
|
angle += 360;
|
|
return angle;
|
|
}
|
|
// from http://stackoverflow.com/questions/11492299/quaternion-to-euler-angles-algorithm-how-to-convert-to-y-up-and-between-ha
|
|
private static QuaternionHelper fromEulerRad(Vector3 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);
|
|
QuaternionHelper result;
|
|
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2;
|
|
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
|
|
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
|
|
return result;
|
|
}
|
|
private static void toAxisAngleRad(QuaternionHelper q, out Vector3 axis, out float angle)
|
|
{
|
|
if (System.Math.Abs(q.w) > 1.0f)
|
|
q.normalize();
|
|
angle = 2.0f * (float)System.Math.Acos(q.w); // angle
|
|
float den = (float)System.Math.Sqrt(1.0 - q.w * q.w);
|
|
if (den > 0.0001f)
|
|
{
|
|
axis = Vector3Helper.division(q.xyz, den);
|
|
}
|
|
else
|
|
{
|
|
// This occurs when the angle is zero.
|
|
// Not a problem: just set an arbitrary normalized axis.
|
|
axis = new Vector3(1, 0, 0);
|
|
}
|
|
}
|
|
public override Int32 GetHashCode()
|
|
{
|
|
return this.x.GetHashCode() ^ this.y.GetHashCode() << 2 ^ this.z.GetHashCode() >> 2 ^ this.w.GetHashCode() >> 1;
|
|
}
|
|
public override bool Equals(object? other)
|
|
{
|
|
if (!(other is QuaternionHelper))
|
|
{
|
|
return false;
|
|
}
|
|
QuaternionHelper quaternion = (QuaternionHelper)other;
|
|
return this.x.Equals(quaternion.x) && this.y.Equals(quaternion.y) && this.z.Equals(quaternion.z) && this.w.Equals(quaternion.w);
|
|
}
|
|
public bool Equals(QuaternionHelper other)
|
|
{
|
|
return this.x.Equals(other.x) && this.y.Equals(other.y) && this.z.Equals(other.z) && this.w.Equals(other.w);
|
|
}
|
|
public static QuaternionHelper operator *(QuaternionHelper lhs, QuaternionHelper rhs)
|
|
{
|
|
return new QuaternionHelper(lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y, lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z, lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x, lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z);
|
|
}
|
|
public static Vector3 operator *(QuaternionHelper rotation, Vector3 point)
|
|
{
|
|
float num = rotation.x * 2f;
|
|
float num2 = rotation.y * 2f;
|
|
float num3 = rotation.z * 2f;
|
|
float num4 = rotation.x * num;
|
|
float num5 = rotation.y * num2;
|
|
float num6 = rotation.z * num3;
|
|
float num7 = rotation.x * num2;
|
|
float num8 = rotation.x * num3;
|
|
float num9 = rotation.y * num3;
|
|
float num10 = rotation.w * num;
|
|
float num11 = rotation.w * num2;
|
|
float num12 = rotation.w * num3;
|
|
Vector3 result;
|
|
result.X = (1f - (num5 + num6)) * point.X + (num7 - num12) * point.Y + (num8 + num11) * point.Z;
|
|
result.Y = (num7 + num12) * point.X + (1f - (num4 + num6)) * point.Y + (num9 - num10) * point.Z;
|
|
result.Z = (num8 - num11) * point.X + (num9 + num10) * point.Y + (1f - (num4 + num5)) * point.Z;
|
|
return result;
|
|
}
|
|
public static bool operator ==(QuaternionHelper lhs, QuaternionHelper rhs)
|
|
{
|
|
return QuaternionHelper.dot(lhs, rhs) > 0.999999f;
|
|
}
|
|
public static bool operator !=(QuaternionHelper lhs, QuaternionHelper rhs)
|
|
{
|
|
return QuaternionHelper.dot(lhs, rhs) <= 0.999999f;
|
|
}
|
|
}
|