/// <summary>
/// 两个向量的球形插值
/// </summary>
/// <param name="a">向量a</param>
/// <param name="b">向量b</param>
/// <param name="t">t的值在[0..1]</param>
/// <returns></returns>
public static Vector3RightHand Slerp(Vector3RightHand a, Vector3RightHand b, float t)
{
// Quaternion fromQua = Quaternion.LookRotation(from, Vector3.up);
// Quaternion toQua = Quaternion.LookRotation(to, Vector3.up);
// Quaternion endQua = Quaternion.Slerp(fromQua, toQua, t);
//
// Vector3 endDir = endQua * Vector3.forward;
// endDir.Normalize();
//
// float length = Mathf3d.Lerp(from.magnitude, to.magnitude, t);
// return endDir.normalized * length;
if (t <= 0)
{
return(a);
}
else if (t >= 1)
{
return(b);
}
Vector3RightHand v = RotateTo(a, b, Vector3RightHand.Angle(a, b) * t);
//向量的长度，跟线性插值一样计算
float length = b.magnitude * t + a.magnitude * (1 - t);
return(v.normalized * length);
}
// 欧拉角转四元数
static QuaternionRightHand FromEulerRad(Vector3RightHand euler)
{
var yaw = euler.z;
var pitch = euler.x;
var roll = euler.y;
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)Mathf.Sin((float)yawOver2);
float cosYawOver2 = (float)Mathf.Cos((float)yawOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)Mathf.Sin((float)pitchOver2);
float cosPitchOver2 = (float)Mathf.Cos((float)pitchOver2);
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)Mathf.Sin((float)rollOver2);
float cosRollOver2 = (float)Mathf.Cos((float)rollOver2);
QuaternionRightHand result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
result.z = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
return(result);
}
static Vector3RightHand NormalizeAngles(Vector3RightHand angles)
{
angles.x = NormalizeAngle(angles.x);
angles.y = NormalizeAngle(angles.y);
angles.z = NormalizeAngle(angles.z);
return(angles);
}
/// <summary>
/// Construct a new MyQuaternion from vector and w components
/// </summary>
/// <param name="v">The vector part</param>
/// <param name="w">The w part</param>
QuaternionRightHand(Vector3RightHand v, float w)
{
this.x = v.x;
this.y = v.y;
this.z = v.z;
this.w = w;
}
/// <summary>
/// 可以用来计算ojbtoworld矩阵
/// </summary>
/// <param name="pos"></param>
/// <param name="q"></param>
/// <param name="s"></param>
/// <returns></returns>
public static Matrix4x4RightHand TRS(Vector3RightHand pos, QuaternionRightHand q, Vector3RightHand s)
{
Matrix4x4RightHand posMatrix = Matrix4x4RightHand.identity;
posMatrix.m03 = pos.x;
posMatrix.m13 = pos.y;
posMatrix.m23 = pos.z;
Matrix4x4RightHand rotateMatrix = Matrix4x4RightHand.identity;
rotateMatrix.m00 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
rotateMatrix.m10 = 2 * q.x * q.y + 2 * q.w * q.z;
rotateMatrix.m20 = 2 * q.x * q.z - 2 * q.w * q.y;
rotateMatrix.m01 = 2 * q.x * q.y - 2 * q.w * q.z;
rotateMatrix.m11 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
rotateMatrix.m21 = 2 * q.y * q.z + 2 * q.w * q.x;
rotateMatrix.m02 = 2 * q.x * q.z + 2 * q.w * q.y;
rotateMatrix.m12 = 2 * q.y * q.z - 2 * q.w * q.x;
rotateMatrix.m22 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
Matrix4x4RightHand scaleMatrix = Scale(s);
Matrix4x4RightHand ret = posMatrix * rotateMatrix * scaleMatrix;
return(ret);
}
public static QuaternionRightHand LookRotation(Vector3RightHand forward, Vector3RightHand up)
{
forward = Vector3RightHand.Normalize(forward);
Vector3RightHand right = Vector3RightHand.Normalize(Vector3RightHand.Cross(up, forward));
up = Vector3RightHand.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 QuaternionRightHand();
if (num8 > 0f)
{
var num = (float)Mathf.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)Mathf.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)Mathf.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)Mathf.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 static Vector3RightHand ClampMagnitude(Vector3RightHand vector, float maxLength)
{
if (vector.sqrMagnitude > maxLength * maxLength)
{
return(vector.normalized * maxLength);
}
return(vector);
}
public static Vector3RightHand Cross(Vector3RightHand lhs, Vector3RightHand rhs)
{
//return new Vector3RightHand(lhs.y * rhs.z - lhs.z * rhs.y, lhs.z * rhs.x - lhs.x * rhs.z, lhs.x * rhs.y - lhs.y * rhs.x);
return(new Vector3RightHand(
lhs.y * rhs.z - rhs.y * lhs.z,
lhs.z * rhs.x - rhs.z * lhs.x,
lhs.x * rhs.y - rhs.x * lhs.y));
}
public static Vector3RightHand operator -(Vector3RightHand a)
{
Vector3RightHand ret = new Vector3RightHand();
ret.x = -a.x;
ret.y = -a.y;
ret.z = -a.z;
return(ret);
}
public static void Test()
{
Vector3RightHand src = new Vector3RightHand(1, 2, 3);
Vector3RightHand des = new Vector3RightHand(40, 50, 60);
Debug.Log("测试vecotr3.slerp" + Vector3RightHand.Slerp(src, des, 0.7f));
//Console.ReadLine();
}
public Vector3RightHand MultiplyVector(Vector3RightHand v)
{
Vector3RightHand result;
result.x = this.m00 * v.x + this.m01 * v.y + this.m02 * v.z;
result.y = this.m10 * v.x + this.m11 * v.y + this.m12 * v.z;
result.z = this.m20 * v.x + this.m21 * v.y + this.m22 * v.z;
return(result);
}
public static Vector3RightHand operator +(Vector3RightHand a, Vector3RightHand b)
{
Vector3RightHand ret = new Vector3RightHand();
ret.x = a.x + b.x;
ret.y = a.y + b.y;
ret.z = a.z + b.z;
return(ret);
}
public static Vector3RightHand operator /(Vector3RightHand a, float d)
{
Vector3RightHand ret = new Vector3RightHand();
ret.x = a.x / d;
ret.y = a.y / d;
ret.z = a.z / d;
return(ret);
}
public static Vector3RightHand operator *(float d, Vector3RightHand a)
{
Vector3RightHand ret = new Vector3RightHand();
ret.x = a.x * d;
ret.y = a.y * d;
ret.z = a.z * d;
return(ret);
}
public static Vector3RightHand Normalize(Vector3RightHand value)
{
float num = Vector3RightHand.Magnitude(value);
if (num > 1E-05f)
{
return(value / num);
}
return(Vector3RightHand.zero);
}
public static Vector3RightHand Project(Vector3RightHand vector, Vector3RightHand onNormal)
{
float num = Vector3RightHand.Dot(onNormal, onNormal);
if (num < 1.401298E-45f)
{
return(Vector3RightHand.zero);
}
return(onNormal * Vector3RightHand.Dot(vector, onNormal) / num);
}
public override bool Equals(object other)
{
if (!(other is Vector3RightHand))
{
return(false);
}
Vector3RightHand vector = (Vector3RightHand)other;
return(this.x.Equals(vector.x) && this.y.Equals(vector.y) && this.z.Equals(vector.z));
}
public static Vector3RightHand MoveTowards(Vector3RightHand current, Vector3RightHand target, float maxDistanceDelta)
{
Vector3RightHand a = target - current;
float magnitude = a.magnitude;
if (magnitude <= maxDistanceDelta || magnitude == 0f)
{
return(target);
}
return(current + a / magnitude * maxDistanceDelta);
}
public void Normalize()
{
float num = Vector3RightHand.Magnitude(this);
if (num > 1E-05f)
{
this /= num;
}
else
{
this = Vector3RightHand.zero;
}
}
public Vector3RightHand MultiplyPoint(Vector3RightHand v)
{
Vector3RightHand result;
result.x = this.m00 * v.x + this.m01 * v.y + this.m02 * v.z + this.m03;
result.y = this.m10 * v.x + this.m11 * v.y + this.m12 * v.z + this.m13;
result.z = this.m20 * v.x + this.m21 * v.y + this.m22 * v.z + this.m23;
float num = this.m30 * v.x + this.m31 * v.y + this.m32 * v.z + this.m33;
num = 1f / num;
result.x *= num;
result.y *= num;
result.z *= num;
return(result);
}
//static Quaternion FromToRotation(Vector3 fromDiection, Vector3 toDirection)
//根据两个向量计算出旋转量，计算出来的旋转量为从fromDiection,旋转到toDirection的旋转量。
//这句话意思很明显了。就是计算旋转量。
//那么LookRotation(Vector3 forward)计算的是，Z轴旋转到forward的旋转量。
//推出：Quaternion.LookRotation(new Vector3(1,0,0)) == Quaternion.FromToRotation(Vector3.forward, new Vector3(1,0,0));
//因为前者就是计算向前向量到当前向量（1,0,0）的旋转量的，其实现过程就是后者喽。
public static QuaternionRightHand FromToRotation(Vector3RightHand v1, Vector3RightHand v2)
{
// fromDirection.Normalize();
// toDirection.Normalize();
// Quaternion fromQua = LookRotation(fromDirection);
// Quaternion toQua = LookRotation(toDirection);
// fromQua = Normalize(fromQua);
// toQua = Normalize(toQua);
//Vector3 fromDir = fromQua * Vector3.forward;
//Vector3 toDir = toQua * Vector3.forward;
//Quaternion ret = RotateTowards(fromQua, toQua, float.MaxValue);
//return ret;
return(QuaternionRightHand.AngleAxis(Vector3RightHand.Angle(v1, v2), Vector3RightHand.Cross(v1, v2)));
}
static void ToAxisAngleRad(QuaternionRightHand q, out Vector3RightHand axis, out float angle)
{
if (Mathf.Abs(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * (float)Mathf.Acos(q.w); // angle
float den = (float)Mathf.Sqrt(1.0f - 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 Vector3RightHand(1, 0, 0);
}
}
public static QuaternionRightHand AngleAxis(float degress, Vector3RightHand axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
QuaternionRightHand result = identity;
var radians = degress * Mathf.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * (float)Mathf.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = (float)Mathf.Cos(radians);
return(Normalize(result));
}
/// <summary>
/// 将向量from向向量to旋转角度angle
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="angle"></param>
/// <returns></returns>
static Vector3RightHand RotateTo(Vector3RightHand from, Vector3RightHand to, float angle)
{
//如果两向量角度为0
if (Vector3RightHand.Angle(from, to) == 0)
{
return(from);
}
//旋转轴
Vector3RightHand n = Vector3RightHand.Cross(from, to);
//旋转轴规范化
n.Normalize();
//旋转矩阵
Matrix4x4RightHand rotateMatrix = new Matrix4x4RightHand();
//旋转的弧度
double radian = angle * Mathf.PI / 180;
float cosAngle = (float)Mathf.Cos((float)radian);
float sinAngle = (float)Mathf.Sin((float)radian);
//矩阵的数据
//这里看不懂的自行科普矩阵知识
rotateMatrix.SetRow(0, new Vector4(n.x * n.x * (1 - cosAngle) + cosAngle, n.x * n.y * (1 - cosAngle) + n.z * sinAngle, n.x * n.z * (1 - cosAngle) - n.y * sinAngle, 0));
rotateMatrix.SetRow(1, new Vector4(n.x * n.y * (1 - cosAngle) - n.z * sinAngle, n.y * n.y * (1 - cosAngle) + cosAngle, n.y * n.z * (1 - cosAngle) + n.x * sinAngle, 0));
rotateMatrix.SetRow(2, new Vector4(n.x * n.z * (1 - cosAngle) + n.y * sinAngle, n.y * n.z * (1 - cosAngle) - n.x * sinAngle, n.z * n.z * (1 - cosAngle) + cosAngle, 0));
rotateMatrix.SetRow(3, new Vector4(0, 0, 0, 1));
Vector4 v = Vector3RightHand.ToVector4(from);
Vector3RightHand vector = new Vector3RightHand();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; j++)
{
vector[i] += v[j] * rotateMatrix[j, i];
}
}
return(vector);
}
public static Matrix4x4RightHand Scale(Vector3RightHand v)
{
return(new Matrix4x4RightHand
{
m00 = v.x,
m01 = 0f,
m02 = 0f,
m03 = 0f,
m10 = 0f,
m11 = v.y,
m12 = 0f,
m13 = 0f,
m20 = 0f,
m21 = 0f,
m22 = v.z,
m23 = 0f,
m30 = 0f,
m31 = 0f,
m32 = 0f,
m33 = 1f
});
}
public static void Test()
{
Vector3RightHand pos = new Vector3RightHand(100, 200, 300);
QuaternionRightHand rotate = QuaternionRightHand.identity;
rotate.eulerAngles = new Vector3RightHand(40, 50, 60);
//Console.WriteLine("测试Quaternion " + rotate);
Vector3RightHand scale = new Vector3RightHand(7, 8, 9);
Matrix4x4RightHand trs = TRS(pos, rotate, scale);
Console.WriteLine("测试trs\n" + trs.ToString());
Matrix4x4RightHand transpose = Transpose(trs);
Console.WriteLine("测试转置矩阵\n" + transpose.ToString());
Matrix4x4RightHand inverse = Inverse(trs);
Console.WriteLine("测试逆矩阵\n" + inverse.ToString());
Matrix4x4RightHand posMatrix = Matrix4x4RightHand.identity;
posMatrix.m03 = pos.x;
posMatrix.m13 = pos.y;
posMatrix.m23 = pos.z;
Vector4 point = new Vector4(0, 0, 0, 1);
Vector4 newPoint = posMatrix * point;
float fov = 60;
float aspect = (float)16 / 9;
float zNear = 1;
float zFar = 1000;
Matrix4x4RightHand perspectiveMat = Matrix4x4RightHand.Perspective(fov, aspect, zNear, zFar);
Console.WriteLine("测试透视矩阵\n" + perspectiveMat.ToString());
//Console.ReadLine();
}
public void SetLookRotation(Vector3RightHand view, Vector3RightHand up)
{
this = LookRotation(view, up);
}
public void ToAngleAxis(out float angle, out Vector3RightHand axis)
{
QuaternionRightHand.ToAxisAngleRad(this, out axis, out angle);
angle *= Mathf.Rad2Deg;
}
public void SetFromToRotation(Vector3RightHand fromDirection, Vector3RightHand toDirection)
{
this = FromToRotation(fromDirection, toDirection);
}
private static QuaternionRightHand SlerpUnclamped(QuaternionRightHand a, QuaternionRightHand 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 + Vector3RightHand.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)Mathf.Acos(cosHalfAngle);
float sinHalfAngle = (float)Mathf.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Mathf.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
blendB = (float)Mathf.Sin(halfAngle * t) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - t;
blendB = t;
}
QuaternionRightHand result = new QuaternionRightHand(blendA * a.xyz + blendB * b.xyz, blendA * a.w + blendB * b.w);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(identity);
}
}
