mirror of
https://github.com/tengge1/ShadowEditor.git
synced 2026-01-25 15:08:11 +00:00
568 lines
16 KiB
C#
568 lines
16 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using _Math = System.Math;
|
|
|
|
namespace THREE
|
|
{
|
|
/// <summary>
|
|
/// @author mikael emtinger / http://gomo.se/
|
|
/// @author alteredq / http://alteredqualia.com/
|
|
/// @author WestLangley / http://github.com/WestLangley
|
|
/// @author bhouston / http://clara.io
|
|
/// @author tengge / https://github.com/tengge1
|
|
/// </summary>
|
|
public class Quaternion
|
|
{
|
|
public double _x;
|
|
public double _y;
|
|
public double _z;
|
|
public double _w;
|
|
|
|
public Quaternion(double x = 0.0, double y = 0.0, double z = 0.0, double w = 1.0)
|
|
{
|
|
this._x = x;
|
|
this._y = y;
|
|
this._z = z;
|
|
this._w = w;
|
|
|
|
this.OnChangeCallback = _OnChangeCallback;
|
|
}
|
|
|
|
public Quaternion Slerp(Quaternion qa, Quaternion qb, Quaternion qm, double t)
|
|
{
|
|
return qm.Copy(qa).Slerp(qb, t);
|
|
}
|
|
|
|
public void SlerpFlat(double[] dst, int dstOffset, double[] src0, int srcOffset0, double[] src1, int srcOffset1, double t)
|
|
{
|
|
// fuzz-free, array-based Quaternion SLERP operation
|
|
|
|
double x0 = src0[srcOffset0 + 0],
|
|
y0 = src0[srcOffset0 + 1],
|
|
z0 = src0[srcOffset0 + 2],
|
|
w0 = src0[srcOffset0 + 3],
|
|
|
|
x1 = src1[srcOffset1 + 0],
|
|
y1 = src1[srcOffset1 + 1],
|
|
z1 = src1[srcOffset1 + 2],
|
|
w1 = src1[srcOffset1 + 3];
|
|
|
|
if (w0 != w1 || x0 != x1 || y0 != y1 || z0 != z1)
|
|
{
|
|
double s = 1 - t,
|
|
cos = x0 * x1 + y0 * y1 + z0 * z1 + w0 * w1,
|
|
|
|
dir = (cos >= 0 ? 1 : -1),
|
|
sqrSin = 1 - cos * cos;
|
|
|
|
// Skip the Slerp for tiny steps to avoid numeric problems:
|
|
if (sqrSin > double.Epsilon)
|
|
{
|
|
double sin = _Math.Sqrt(sqrSin),
|
|
len = _Math.Atan2(sin, cos * dir);
|
|
|
|
s = _Math.Sin(s * len) / sin;
|
|
t = _Math.Sin(t * len) / sin;
|
|
}
|
|
|
|
var tDir = t * dir;
|
|
|
|
x0 = x0 * s + x1 * tDir;
|
|
y0 = y0 * s + y1 * tDir;
|
|
z0 = z0 * s + z1 * tDir;
|
|
w0 = w0 * s + w1 * tDir;
|
|
|
|
// Normalize in case we just did a lerp:
|
|
if (s == 1 - t)
|
|
{
|
|
var f = 1 / _Math.Sqrt(x0 * x0 + y0 * y0 + z0 * z0 + w0 * w0);
|
|
|
|
x0 *= f;
|
|
y0 *= f;
|
|
z0 *= f;
|
|
w0 *= f;
|
|
}
|
|
}
|
|
|
|
dst[dstOffset] = x0;
|
|
dst[dstOffset + 1] = y0;
|
|
dst[dstOffset + 2] = z0;
|
|
dst[dstOffset + 3] = w0;
|
|
}
|
|
|
|
public double X
|
|
{
|
|
get { return this._x; }
|
|
|
|
set
|
|
{
|
|
this._x = value;
|
|
this.OnChangeCallback();
|
|
}
|
|
}
|
|
|
|
public double Y
|
|
{
|
|
get { return this._y; }
|
|
|
|
set
|
|
{
|
|
|
|
this._y = value;
|
|
this.OnChangeCallback();
|
|
|
|
}
|
|
}
|
|
|
|
public double Z
|
|
{
|
|
get { return this._z; }
|
|
|
|
set
|
|
{
|
|
this._z = value;
|
|
this.OnChangeCallback();
|
|
}
|
|
}
|
|
|
|
public double W
|
|
{
|
|
get { return this._w; }
|
|
|
|
set
|
|
{
|
|
this._w = value;
|
|
this.OnChangeCallback();
|
|
}
|
|
}
|
|
|
|
public Quaternion Set(double x, double y, double z, double w)
|
|
{
|
|
this._x = x;
|
|
this._y = y;
|
|
this._z = z;
|
|
this._w = w;
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion Clone()
|
|
{
|
|
return new Quaternion(this._x, this._y, this._z, this._w);
|
|
}
|
|
|
|
public Quaternion Copy(Quaternion quaternion)
|
|
{
|
|
this._x = quaternion._x;
|
|
this._y = quaternion._y;
|
|
this._z = quaternion._z;
|
|
this._w = quaternion._w;
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion SetFromEuler(Euler euler, bool update = false)
|
|
{
|
|
double x = euler._x, y = euler._y, z = euler._z;
|
|
string order = euler._order;
|
|
|
|
// http://www.mathworks.com/matlabcentral/fileexchange/
|
|
// 20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
|
|
// content/SpinCalc.m
|
|
|
|
var c1 = _Math.Cos(x / 2);
|
|
var c2 = _Math.Cos(y / 2);
|
|
var c3 = _Math.Cos(z / 2);
|
|
|
|
var s1 = _Math.Sin(x / 2);
|
|
var s2 = _Math.Sin(y / 2);
|
|
var s3 = _Math.Sin(z / 2);
|
|
|
|
if (order == "XYZ")
|
|
{
|
|
this._x = s1 * c2 * c3 + c1 * s2 * s3;
|
|
this._y = c1 * s2 * c3 - s1 * c2 * s3;
|
|
this._z = c1 * c2 * s3 + s1 * s2 * c3;
|
|
this._w = c1 * c2 * c3 - s1 * s2 * s3;
|
|
}
|
|
else if (order == "YXZ")
|
|
{
|
|
this._x = s1 * c2 * c3 + c1 * s2 * s3;
|
|
this._y = c1 * s2 * c3 - s1 * c2 * s3;
|
|
this._z = c1 * c2 * s3 - s1 * s2 * c3;
|
|
this._w = c1 * c2 * c3 + s1 * s2 * s3;
|
|
}
|
|
else if (order == "ZXY")
|
|
{
|
|
this._x = s1 * c2 * c3 - c1 * s2 * s3;
|
|
this._y = c1 * s2 * c3 + s1 * c2 * s3;
|
|
this._z = c1 * c2 * s3 + s1 * s2 * c3;
|
|
this._w = c1 * c2 * c3 - s1 * s2 * s3;
|
|
}
|
|
else if (order == "ZYX")
|
|
{
|
|
this._x = s1 * c2 * c3 - c1 * s2 * s3;
|
|
this._y = c1 * s2 * c3 + s1 * c2 * s3;
|
|
this._z = c1 * c2 * s3 - s1 * s2 * c3;
|
|
this._w = c1 * c2 * c3 + s1 * s2 * s3;
|
|
}
|
|
else if (order == "YZX")
|
|
{
|
|
this._x = s1 * c2 * c3 + c1 * s2 * s3;
|
|
this._y = c1 * s2 * c3 + s1 * c2 * s3;
|
|
this._z = c1 * c2 * s3 - s1 * s2 * c3;
|
|
this._w = c1 * c2 * c3 - s1 * s2 * s3;
|
|
}
|
|
else if (order == "XZY")
|
|
{
|
|
this._x = s1 * c2 * c3 - c1 * s2 * s3;
|
|
this._y = c1 * s2 * c3 - s1 * c2 * s3;
|
|
this._z = c1 * c2 * s3 + s1 * s2 * c3;
|
|
this._w = c1 * c2 * c3 + s1 * s2 * s3;
|
|
}
|
|
|
|
if (update != false) this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion SetFromAxisAngle(Vector3 axis, double angle)
|
|
{
|
|
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
|
|
|
|
// assumes axis is normalized
|
|
|
|
double halfAngle = angle / 2, s = _Math.Sin(halfAngle);
|
|
|
|
this._x = axis.x * s;
|
|
this._y = axis.y * s;
|
|
this._z = axis.z * s;
|
|
this._w = _Math.Cos(halfAngle);
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion SetFromRotationMatrix(Matrix4 m)
|
|
{
|
|
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
|
|
|
|
// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
|
|
|
|
double[] te = m.elements;
|
|
|
|
double m11 = te[0], m12 = te[4], m13 = te[8],
|
|
m21 = te[1], m22 = te[5], m23 = te[9],
|
|
m31 = te[2], m32 = te[6], m33 = te[10],
|
|
|
|
trace = m11 + m22 + m33,
|
|
s;
|
|
|
|
if (trace > 0)
|
|
{
|
|
s = 0.5 / _Math.Sqrt(trace + 1.0);
|
|
|
|
this._w = 0.25 / s;
|
|
this._x = (m32 - m23) * s;
|
|
this._y = (m13 - m31) * s;
|
|
this._z = (m21 - m12) * s;
|
|
}
|
|
else if (m11 > m22 && m11 > m33)
|
|
{
|
|
s = 2.0 * _Math.Sqrt(1.0 + m11 - m22 - m33);
|
|
|
|
this._w = (m32 - m23) / s;
|
|
this._x = 0.25 * s;
|
|
this._y = (m12 + m21) / s;
|
|
this._z = (m13 + m31) / s;
|
|
}
|
|
else if (m22 > m33)
|
|
{
|
|
s = 2.0 * _Math.Sqrt(1.0 + m22 - m11 - m33);
|
|
|
|
this._w = (m13 - m31) / s;
|
|
this._x = (m12 + m21) / s;
|
|
this._y = 0.25 * s;
|
|
this._z = (m23 + m32) / s;
|
|
|
|
}
|
|
else
|
|
{
|
|
s = 2.0 * _Math.Sqrt(1.0 + m33 - m11 - m22);
|
|
|
|
this._w = (m21 - m12) / s;
|
|
this._x = (m13 + m31) / s;
|
|
this._y = (m23 + m32) / s;
|
|
this._z = 0.25 * s;
|
|
}
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion SetFromUnitVectors(Vector3 vFrom, Vector3 vTo)
|
|
{
|
|
// assumes direction vectors vFrom and vTo are normalized
|
|
var v1 = new Vector3();
|
|
var EPS = 0.000001;
|
|
|
|
double r = vFrom.Dot(vTo) + 1;
|
|
|
|
if (r < EPS)
|
|
{
|
|
r = 0;
|
|
|
|
if (_Math.Abs(vFrom.x) > _Math.Abs(vFrom.z))
|
|
{
|
|
v1.Set(-vFrom.y, vFrom.x, 0);
|
|
}
|
|
else
|
|
{
|
|
v1.Set(0, -vFrom.z, vFrom.y);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
v1.CrossVectors(vFrom, vTo);
|
|
}
|
|
|
|
this._x = v1.x;
|
|
this._y = v1.y;
|
|
this._z = v1.z;
|
|
this._w = r;
|
|
|
|
return this.Normalize();
|
|
}
|
|
|
|
public double AngleTo(Quaternion q)
|
|
{
|
|
return 2 * _Math.Acos(_Math.Abs(Math.Clamp(this.Dot(q), -1, 1)));
|
|
}
|
|
|
|
public Quaternion RotateTowards(Quaternion q, double step)
|
|
{
|
|
var angle = this.AngleTo(q);
|
|
|
|
if (angle == 0) return this;
|
|
|
|
var t = _Math.Min(1, step / angle);
|
|
|
|
this.Slerp(q, t);
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion Inverse()
|
|
{
|
|
// quaternion is assumed to have unit length
|
|
return this.Conjugate();
|
|
}
|
|
|
|
public Quaternion Conjugate()
|
|
{
|
|
this._x *= -1;
|
|
this._y *= -1;
|
|
this._z *= -1;
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public double Dot(Quaternion v)
|
|
{
|
|
return this._x * v._x + this._y * v._y + this._z * v._z + this._w * v._w;
|
|
}
|
|
|
|
public double LengthSq()
|
|
{
|
|
return this._x * this._x + this._y * this._y + this._z * this._z + this._w * this._w;
|
|
}
|
|
|
|
public double Length()
|
|
{
|
|
return _Math.Sqrt(this._x * this._x + this._y * this._y + this._z * this._z + this._w * this._w);
|
|
}
|
|
|
|
public Quaternion Normalize()
|
|
{
|
|
var l = this.Length();
|
|
|
|
if (l == 0)
|
|
{
|
|
|
|
this._x = 0;
|
|
this._y = 0;
|
|
this._z = 0;
|
|
this._w = 1;
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
l = 1 / l;
|
|
|
|
this._x = this._x * l;
|
|
this._y = this._y * l;
|
|
this._z = this._z * l;
|
|
this._w = this._w * l;
|
|
|
|
}
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion Multiply(Quaternion q, Quaternion p = null)
|
|
{
|
|
if (p != null)
|
|
{
|
|
Console.WriteLine("THREE.Quaternion: .multiply() now only accepts one argument. Use .multiplyQuaternions( a, b ) instead.");
|
|
return this.MultiplyQuaternions(q, p);
|
|
}
|
|
|
|
return this.MultiplyQuaternions(this, q);
|
|
}
|
|
|
|
public Quaternion Premultiply(Quaternion q)
|
|
{
|
|
return this.MultiplyQuaternions(q, this);
|
|
}
|
|
|
|
public Quaternion MultiplyQuaternions(Quaternion a, Quaternion b)
|
|
{
|
|
// from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
|
|
|
|
double qax = a._x, qay = a._y, qaz = a._z, qaw = a._w;
|
|
double qbx = b._x, qby = b._y, qbz = b._z, qbw = b._w;
|
|
|
|
this._x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
|
|
this._y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
|
|
this._z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
|
|
this._w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public Quaternion Slerp(Quaternion qb, double t)
|
|
{
|
|
if (t == 0) return this;
|
|
if (t == 1) return this.Copy(qb);
|
|
|
|
double x = this._x, y = this._y, z = this._z, w = this._w;
|
|
|
|
// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
|
|
|
|
var cosHalfTheta = w * qb._w + x * qb._x + y * qb._y + z * qb._z;
|
|
|
|
if (cosHalfTheta < 0)
|
|
{
|
|
this._w = -qb._w;
|
|
this._x = -qb._x;
|
|
this._y = -qb._y;
|
|
this._z = -qb._z;
|
|
|
|
cosHalfTheta = -cosHalfTheta;
|
|
}
|
|
else
|
|
{
|
|
this.Copy(qb);
|
|
}
|
|
|
|
if (cosHalfTheta >= 1.0)
|
|
{
|
|
this._w = w;
|
|
this._x = x;
|
|
this._y = y;
|
|
this._z = z;
|
|
|
|
return this;
|
|
}
|
|
|
|
var sqrSinHalfTheta = 1.0 - cosHalfTheta * cosHalfTheta;
|
|
|
|
if (sqrSinHalfTheta <= double.Epsilon)
|
|
{
|
|
var s = 1 - t;
|
|
this._w = s * w + t * this._w;
|
|
this._x = s * x + t * this._x;
|
|
this._y = s * y + t * this._y;
|
|
this._z = s * z + t * this._z;
|
|
|
|
return this.Normalize();
|
|
}
|
|
|
|
var sinHalfTheta = _Math.Sqrt(sqrSinHalfTheta);
|
|
var halfTheta = _Math.Atan2(sinHalfTheta, cosHalfTheta);
|
|
double ratioA = _Math.Sin((1 - t) * halfTheta) / sinHalfTheta,
|
|
ratioB = _Math.Sin(t * halfTheta) / sinHalfTheta;
|
|
|
|
this._w = (w * ratioA + this._w * ratioB);
|
|
this._x = (x * ratioA + this._x * ratioB);
|
|
this._y = (y * ratioA + this._y * ratioB);
|
|
this._z = (z * ratioA + this._z * ratioB);
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public bool Equals(Quaternion quaternion)
|
|
{
|
|
return quaternion._x == this._x && quaternion._y == this._y && quaternion._z == this._z && quaternion._w == this._w;
|
|
}
|
|
|
|
public Quaternion FromArray(double[] array, int offset = 0)
|
|
{
|
|
this._x = array[offset];
|
|
this._y = array[offset + 1];
|
|
this._z = array[offset + 2];
|
|
this._w = array[offset + 3];
|
|
|
|
this.OnChangeCallback();
|
|
|
|
return this;
|
|
}
|
|
|
|
public double[] ToArray(double[] array = null, int offset = 0)
|
|
{
|
|
if (array == null) array = new double[4];
|
|
|
|
array[offset] = this._x;
|
|
array[offset + 1] = this._y;
|
|
array[offset + 2] = this._z;
|
|
array[offset + 3] = this._w;
|
|
|
|
return array;
|
|
}
|
|
|
|
public Quaternion OnChange(OnChangeCallbackFunc callback)
|
|
{
|
|
this.OnChangeCallback = callback;
|
|
|
|
return this;
|
|
}
|
|
|
|
public delegate void OnChangeCallbackFunc();
|
|
|
|
private void _OnChangeCallback()
|
|
{
|
|
|
|
}
|
|
|
|
public OnChangeCallbackFunc OnChangeCallback;
|
|
}
|
|
}
|