/// <summary>
/// Lerp.
/// </summary>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
/// <param name="blend">Blend.</param>
public static UVector3 Lerp(UVector3 a, UVector3 b, float blend)
{
a.x = blend * (b.x - a.x) + a.x;
a.y = blend * (b.y - a.y) + a.y;
a.z = blend * (b.z - a.z) + a.z;
return(a);
}
/// <summary>
/// Cross
/// </summary>
/// <param name="left">Left.</param>
/// <param name="right">Right.</param>
public static UVector3 Cross(UVector3 left, UVector3 right)
{
var result = new UVector3(left.y * right.z - left.z * right.y,
left.z * right.x - left.x * right.z,
left.x * right.y - left.y * right.x);
return(result);
}
/// <summary>
/// Creates the scale.
/// </summary>
/// <returns>The scale.</returns>
/// <param name="scale">Scale.</param>
public static UMatrix4x4 CreateScale(UVector3 scale)
{
var mscale = identity;
mscale.m11 = scale.x;
mscale.m22 = scale.y;
mscale.m33 = scale.z;
return(mscale);
}
/// <summary>
/// Creates the translate.
/// </summary>
/// <returns>The translate.</returns>
/// <param name="trans">Trans.</param>
public static UMatrix4x4 CreateTranslate(UVector3 trans)
{
var mTrasn = identity;
mTrasn.m14 = trans.x;
mTrasn.m24 = trans.y;
mTrasn.m34 = trans.z;
return(mTrasn);
}
/// <summary>
/// Normalize the specified vector.
/// </summary>
/// <param name="vec">Vec.</param>
public static UVector3 Normalize(UVector3 vec)
{
if (vec.sqrMagnitude < MathHelper.Epsilon)
{
return(zero);
}
float scale = 1.0f / vec.magnitude;
vec.x *= scale;
vec.y *= scale;
vec.z *= scale;
return(vec);
}
/// <summary>
/// Cals the angle with axis y.
/// </summary>
/// <returns>The angle with axis y.</returns>
/// <param name="forward">Forward.</param>
public static float CalAngleWithAxisY(UVector3 forward)
{
forward.y = 0;
forward.Normalized();
var acos = Math.Acos(forward.z) * MathHelper.Rad2Deg;
if (forward.x > 0)
{
return((float)acos);
}
else
{
return(360f - (float)(acos));
}
}
/// <summary>
/// Angles the axis.
/// </summary>
/// <returns>The axis.</returns>
/// <param name="angle">Angle.</param>
/// <param name="axis">Axis.</param>
public static UQuaternion AngleAxis(float angle, UVector3 axis)
{
if (Math.Abs(axis.sqrMagnitude) < MathHelper.Epsilon)
{
return(identity);
}
axis.Normalized();
var sin = (float)Math.Sin(angle * MathHelper.Deg2Rad * 0.5f);
var cos = (float)Math.Cos(angle * MathHelper.Deg2Rad * 0.5f);
float w = cos;
float x = axis.x * sin;
float y = axis.y * sin;
float z = axis.z * sin;
return(new UQuaternion(x, y, z, w));
}
/// <summary>
/// Clears the scale.
/// </summary>
public void ClearScale()
{
var x = new UVector3(m11, m21, m31).normalized;
var y = new UVector3(m12, m22, m32).normalized;
var z = new UVector3(m13, m23, m33).normalized;
m11 = x[1];
m21 = x[2];
m31 = x[3];
m12 = y[1];
m22 = y[2];
m32 = y[3];
m13 = z[1];
m23 = z[2];
m33 = z[3];
}
/// <summary>
/// Looks at position, target and up.
/// </summary>
/// <returns>The <see cref="UMath.UMatrix4x4"/>.</returns>
/// <param name="eye">Position.</param>
/// <param name="target">Target.</param>
/// <param name="up">Up.</param>
public static UMatrix4x4 LookAt(UVector3 eye, UVector3 target, UVector3 up)
{
var w = (target - eye);
w.Normalized();
var u = UVector3.Cross(up, w);
u.Normalized();
var v = UVector3.Cross(w, u);
v.Normalized();
var trans = CreateTranslate(-eye);
var m = new UMatrix4x4(
u.x, v.x, w.x, 0,
u.y, v.y, w.y, 0,
u.z, v.z, w.z, 0,
0, 0, 0, 1);
return(trans * m);
}
/// <summary>
/// angle of two vector
/// </summary>
/// <param name="first">First.</param>
/// <param name="second">Second.</param>
public static float Angle(UVector3 first, UVector3 second)
{
return((float)System.Math.Acos((UVector3.Dot(first, second)) / (first.magnitude * second.magnitude))
* MathHelper.Rad2Deg);
}
public UVector4(UVector3 v, float w) : this(v.x, v.y, v.z, w)
{
}
/// <summary>
/// Decompose the specified m, trans, rotation and scale.
/// </summary>
/// <param name="m">M.</param>
/// <param name="trans">Trans.</param>
/// <param name="rotation">Rotation.</param>
/// <param name="scale">Scale.</param>
public static void Decompose(UMatrix4x4 m, out UVector3 trans, out UQuaternion rotation, out UVector3 scale)
{
trans = m.Translate;
rotation = m.Rotation;
scale = m.Scale;
}
/// <summary>
/// Looks the rotation.
/// </summary>
/// <returns>The rotation.</returns>
/// <param name="forward">Forward.</param>
/// <param name="up">Up.</param>
public static UQuaternion LookRotation(UVector3 forward, UVector3 up)
{
return(UMatrix4x4.LookAt(UVector3.zero, forward, up).Rotation);
}
/// <summary>
/// Looks the rotation.
/// </summary>
/// <returns>The rotation.</returns>
/// <param name="forward">Forward.</param>
public static UQuaternion LookRotation(UVector3 forward)
{
return(LookRotation(forward, UVector3.up));
}
/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first quaternion</param>
/// <param name="q2">The second quaternion</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static UQuaternion Slerp(UQuaternion q1, UQuaternion q2, float blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == 0.0f)
{
if (q2.LengthSquared == 0.0f)
{
return(identity);
}
return(q2);
}
else if (q2.LengthSquared == 0.0f)
{
return(q1);
}
float cosHalfAngle = q1.w * q2.w + UVector3.Dot(q1.Xyz, q2.Xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < 0.0f)
{
q2.Xyz = -q2.Xyz;
q2.w = -q2.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 - blend)) * oneOverSinHalfAngle;
blendB = (float)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
var result = new UQuaternion(
blendA * q1.Xyz + blendB * q2.Xyz,
blendA * q1.w + blendB * q2.w);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(identity);
}
}
/// <summary>
/// Creates the TRS.
/// </summary>
/// <returns>The TR.</returns>
/// <param name="trans">Trans.</param>
/// <param name="rot">Rot.</param>
/// <param name="scale">Scale.</param>
public static UMatrix4x4 TRS(UVector3 trans, UQuaternion rot, UVector3 scale)
{
return(CreateTranslate(trans) * CreateRotation(rot) * CreateScale(scale));
}
/// <summary>
/// Transforms the direction.
/// </summary>
/// <returns>The direction.</returns>
/// <param name="dir">Dir.</param>
public UVector3 TransformDirection(UVector3 dir)
{
return((localToWorldMatrix * Matrix * new UVector4(dir, 0)).Xyz);
}
/// <summary>
/// Transforms the point.
/// </summary>
/// <returns>The point.</returns>
/// <param name="point">Point.</param>
public UVector3 TransformPoint(UVector3 point)
{
return((localToWorldMatrix * Matrix * new UVector4(point, 1)).Xyz);
}
/// <summary>
/// Tos the angle axis.
/// </summary>
/// <param name="angle">Angle.</param>
/// <param name="axis">Axis.</param>
public void ToAngleAxis(out float angle, out UVector3 axis)
{
axis = this.Xyz.normalized;
angle = (float)Math.Acos(w) * 2.0f * MathHelper.Rad2Deg;
}
/// <summary>
/// Dot
/// </summary>
/// <param name="left">Left.</param>
/// <param name="right">Right.</param>
public static float Dot(UVector3 left, UVector3 right)
{
return(left.x * right.x + left.y * right.y + left.z * right.z);
}
public UQuaternion(UVector3 xyz, float w) : this(xyz.x, xyz.y, xyz.z, w)
{
}
/// <summary>
/// Distance the specified l and r.
/// </summary>
/// <param name="l">L.</param>
/// <param name="r">The red component.</param>
public static float Distance(UVector3 l, UVector3 r)
{
return((l - r).magnitude);
}
/// <summary>
/// Euler the specified eulerAngles.
/// </summary>
/// <param name="eulerAngles">Euler angles.</param>
public static UQuaternion Euler(UVector3 eulerAngles)
{
return(Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z));
}
