// Based on:
// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
// http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
private static FQuat FindBetween_Helper(FVector a, FVector b, float normAB)
{
float w = normAB + FVector.DotProduct(a, b);
FQuat result;
if (w >= 1e-6f * normAB)
{
//Axis = FVector::CrossProduct(A, B);
result = new FQuat(
a.Y * b.Z - a.Z * b.Y,
a.Z * b.X - a.X * b.Z,
a.X * b.Y - a.Y * b.X,
w);
}
else
{
// A and B point in opposite directions
w = 0.0f;
result = FMath.Abs(a.X) > FMath.Abs(a.Y)
? new FQuat(-a.Z, 0.0f, a.X, w)
: new FQuat(0.0f, -a.Z, a.Y, w);
}
result.Normalize();
return(result);
}
/// <summary>
/// Check against another vector for equality, within specified error limits.
/// </summary>
/// <param name="v">The vector to check against.</param>
/// <param name="tolerance">Error tolerance. (default to FMath.KindaSmallNumber)</param>
/// <returns>true if the vectors are equal within tolerance limits, false otherwise.</returns>
public bool Equals(FVector2D v, float tolerance)
{
// Change this function name so that we can make use of the default param?
return
(FMath.Abs(X - v.X) <= tolerance &&
FMath.Abs(Y - v.Y) <= tolerance);
}
/// <summary>
/// Checks whether two rotators are equal within specified tolerance, when treated as an orientation.
/// This means that FRotator(0, 0, 360).Equals(FRotator(0,0,0)) is true, because they represent the same final orientation.
/// </summary>
/// <param name="other">The other rotator.</param>
/// <param name="tolerance">Error Tolerance.</param>
/// <returns>true if two rotators are equal, within specified tolerance, otherwise false.</returns>
public bool Equals(FRotator other, float tolerance = FMath.KindaSmallNumber)
{
return
((FMath.Abs(NormalizeAxis(Pitch - other.Pitch)) <= tolerance) &&
(FMath.Abs(NormalizeAxis(Yaw - other.Yaw)) <= tolerance) &&
(FMath.Abs(NormalizeAxis(Roll - other.Roll)) <= tolerance));
}
/// <summary>
/// @note Exp should really only be used after Log.
/// Assumes a quaternion with W=0 and V=theta*v (where |v| = 1).
/// Exp(q) = (sin(theta)*v, cos(theta))
/// </summary>
public FQuat Exp()
{
float angle = FMath.Sqrt(X * X + Y * Y + Z * Z);
float sinAngle = FMath.Sin(angle);
FQuat result;
result.W = FMath.Cos(angle);
if (FMath.Abs(sinAngle) >= FMath.SmallNumber)
{
float scale = sinAngle / angle;
result.X = scale * X;
result.Y = scale * Y;
result.Z = scale * Z;
}
else
{
result.X = X;
result.Y = Y;
result.Z = Z;
}
return(result);
}
/// <summary>
/// Checks whether rotator is nearly zero within specified tolerance, when treated as an orientation.
/// This means that FRotator(0, 0, 360) is "zero", because it is the same final orientation as the zero rotator.
/// </summary>
/// <param name="tolerance">Error Tolerance.</param>
/// <returns>true if rotator is nearly zero, within specified tolerance, otherwise false.</returns>
public bool IsNearlyZero(float tolerance = FMath.KindaSmallNumber)
{
return
(FMath.Abs(NormalizeAxis(Pitch)) <= tolerance &&
FMath.Abs(NormalizeAxis(Yaw)) <= tolerance &&
FMath.Abs(NormalizeAxis(Roll)) <= tolerance);
}
/// <summary>
/// Returns quaternion with W=0 and V=theta*v.
/// </summary>
public FQuat Log()
{
FQuat result;
result.W = 0.0f;
if (FMath.Abs(W) < 1.0f)
{
float angle = FMath.Acos(W);
float sinAngle = FMath.Sin(angle);
if (FMath.Abs(sinAngle) >= FMath.SmallNumber)
{
float scale = angle / sinAngle;
result.X = scale * X;
result.Y = scale * Y;
result.Z = scale * Z;
return(result);
}
}
result.X = X;
result.Y = Y;
result.Z = Z;
return(result);
}
/// <summary>
/// Error tolerant comparison.
/// </summary>
/// <param name="v">Vector to compare against.</param>
/// <param name="tolerance">Error Tolerance.</param>
/// <returns>true if the two vectors are equal within specified tolerance, otherwise false.</returns>
public bool Equals(FVector4 v, float tolerance = FMath.KindaSmallNumber)
{
return
(FMath.Abs(X - v.X) <= tolerance &&
FMath.Abs(Y - v.Y) <= tolerance &&
FMath.Abs(Z - v.Z) <= tolerance &&
FMath.Abs(W - v.W) <= tolerance);
}
/// <summary>
/// Checks whether two planes are equal within specified tolerance.
/// </summary>
/// <param name="other">The other plane.</param>
/// <param name="tolerance">Error Tolerance.</param>
/// <returns>true if the two planes are equal within specified tolerance, otherwise false.</returns>
public bool Equals(FPlane other, float tolerance = FMath.KindaSmallNumber)
{
return
((FMath.Abs(X - other.X) < tolerance) &&
(FMath.Abs(Y - other.Y) < tolerance) &&
(FMath.Abs(Z - other.Z) < tolerance) &&
(FMath.Abs(W - other.W) < tolerance));
}
/// <summary>
/// Find good arbitrary axis vectors to represent U and V axes of a plane,
/// given just the normal.
/// </summary>
public void FindBestAxisVectors3(FVector4 axis1, FVector4 axis2)
{
float nx = FMath.Abs(X);
float ny = FMath.Abs(Y);
float nz = FMath.Abs(Z);
// Find best basis vectors.
if (nz > nx && nz > ny)
{
axis1 = new FVector4(1, 0, 0);
}
else
{
axis1 = new FVector4(0, 0, 1);
}
axis1 = (axis1 - this * Dot3(axis1, this)).GetSafeNormal();
axis2 = axis1 ^ this;
}
/// <summary>
/// Simpler Slerp that doesn't do any checks for 'shortest distance' etc.
/// We need this for the cubic interpolation stuff so that the multiple Slerps dont go in different directions.
/// Result is NOT normalized.
/// </summary>
public static FQuat SlerpFullPath_NotNormalized(FQuat quat1, FQuat quat2, float alpha)
{
float cosAngle = FMath.Clamp(quat1 | quat2, -1.0f, 1.0f);
float angle = FMath.Acos(cosAngle);
//UE_LOG(LogUnrealMath, Log, TEXT("CosAngle: %f Angle: %f"), CosAngle, Angle );
if (FMath.Abs(angle) < FMath.KindaSmallNumber)
{
return(quat1);
}
float sinAngle = FMath.Sin(angle);
float invSinAngle = 1.0f / sinAngle;
float scale0 = FMath.Sin((1.0f - alpha) * angle) * invSinAngle;
float scale1 = FMath.Sin(alpha * angle) * invSinAngle;
return(quat1 * scale0 + quat2 * scale1);
}
// Special-case interpolation
/// <summary>
/// Interpolate a normal vector Current to Target, by interpolating the angle between those vectors with constant step.
/// </summary>
public static FVector VInterpNormalRotationTo(FVector current, FVector target, float deltaTime, float rotationSpeedDegrees)
{
// Find delta rotation between both normals.
FQuat deltaQuat = FQuat.FindBetween(current, target);
// Decompose into an axis and angle for rotation
FVector deltaAxis;
float deltaAngle;
deltaQuat.ToAxisAndAngle(out deltaAxis, out deltaAngle);
// Find rotation step for this frame
float rotationStepRadians = rotationSpeedDegrees * (PI / 180) * deltaTime;
if (FMath.Abs(deltaAngle) > rotationStepRadians)
{
deltaAngle = FMath.Clamp(deltaAngle, -rotationStepRadians, rotationStepRadians);
deltaQuat = new FQuat(deltaAxis, deltaAngle);
return(deltaQuat.RotateVector(current));
}
return(target);
}
/// <summary>
/// Checks whether another Quaternion is equal to this, within specified tolerance.
/// </summary>
/// <param name="other">The other Quaternion.</param>
/// <param name="tolerance">Error tolerance for comparison with other Quaternion.</param>
/// <returns>true if two Quaternions are equal, within specified tolerance, otherwise false.</returns>
public bool Equals(FQuat other, float tolerance = FMath.KindaSmallNumber)
{
return((FMath.Abs(X - other.X) <= tolerance && FMath.Abs(Y - other.Y) <= tolerance && FMath.Abs(Z - other.Z) <= tolerance && FMath.Abs(W - other.W) <= tolerance) ||
(FMath.Abs(X + other.X) <= tolerance && FMath.Abs(Y + other.Y) <= tolerance && FMath.Abs(Z + other.Z) <= tolerance && FMath.Abs(W + other.W) <= tolerance));
}
/// <summary>
/// Creates and initializes a new quaternion from the given matrix.
/// </summary>
/// <param name="m">The rotation matrix to initialize from.</param>
public FQuat(FMatrix m)
{
// If Matrix is NULL, return Identity quaternion. If any of them is 0, you won't be able to construct rotation
// if you have two plane at least, we can reconstruct the frame using cross product, but that's a bit expensive op to do here
// for now, if you convert to matrix from 0 scale and convert back, you'll lose rotation. Don't do that.
if (m.GetScaledAxis(EAxis.X).IsNearlyZero() || m.GetScaledAxis(EAxis.Y).IsNearlyZero() || m.GetScaledAxis(EAxis.Z).IsNearlyZero())
{
this = FQuat.Identity;
return;
}
//#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
// Make sure the Rotation part of the Matrix is unit length.
// Changed to this (same as RemoveScaling) from RotDeterminant as using two different ways of checking unit length matrix caused inconsistency.
if (!FMessage.Ensure(
(FMath.Abs(1.0f - m.GetScaledAxis(EAxis.X).SizeSquared()) <= FMath.KindaSmallNumber) &&
(FMath.Abs(1.0f - m.GetScaledAxis(EAxis.Y).SizeSquared()) <= FMath.KindaSmallNumber) &&
(FMath.Abs(1.0f - m.GetScaledAxis(EAxis.Z).SizeSquared()) <= FMath.KindaSmallNumber)
, "Make sure the Rotation part of the Matrix is unit length."))
{
this = FQuat.Identity;
return;
}
//#endif
//const MeReal *const t = (MeReal *) tm;
float s;
// Check diagonal (trace)
float tr = m[0, 0] + m[1, 1] + m[2, 2];
if (tr > 0.0f)
{
float invS = FMath.InvSqrt(tr + 1.0f);
this.W = 0.5f * (1.0f / invS);
s = 0.5f * invS;
this.X = (m[1, 2] - m[2, 1]) * s;
this.Y = (m[2, 0] - m[0, 2]) * s;
this.Z = (m[0, 1] - m[1, 0]) * s;
}
else
{
// diagonal is negative
int i = 0;
if (m[1, 1] > m[0, 0])
{
i = 1;
}
if (m[2, 2] > m[i, i])
{
i = 2;
}
int[] nxt = { 1, 2, 0 };
int j = nxt[i];
int k = nxt[j];
s = m[i, i] - m[j, j] - m[k, k] + 1.0f;
float InvS = FMath.InvSqrt(s);
float[] qt = new float[4];
qt[i] = 0.5f * (1.0f / InvS);
s = 0.5f * InvS;
qt[3] = (m[j, k] - m[k, j]) * s;
qt[j] = (m[i, j] + m[j, i]) * s;
qt[k] = (m[i, k] + m[k, i]) * s;
this.X = qt[0];
this.Y = qt[1];
this.Z = qt[2];
this.W = qt[3];
DiagnosticCheckNaN();
}
}
/// <summary>
/// Error measure (angle) between two quaternions, ranged [0..1].
/// Returns the hypersphere-angle between two quaternions; alignment shouldn't matter, though
/// @note normalized input is expected.
/// </summary>
public static float Error(FQuat q1, FQuat q2)
{
float cosom = FMath.Abs(q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W);
return((FMath.Abs(cosom) < 0.9999999f) ? FMath.Acos(cosom) * (1.0f / FMath.PI) : 0.0f);
}
/// <summary>
/// Return the manhattan distance in degrees between this Rotator and the passed in one.
/// </summary>
/// <param name="rotator">the Rotator we are comparing with.</param>
/// <returns>Distance(Manhattan) between the two rotators. </returns>
public float GetManhattanDistance(FRotator rotator)
{
return(FMath.Abs(Yaw - rotator.Yaw) + FMath.Abs(Pitch - rotator.Pitch) + FMath.Abs(Roll - rotator.Roll));
}
/// <summary>
/// Check whether two spheres are the same within specified tolerance.
/// </summary>
/// <param name="other">The other sphere.</param>
/// <param name="tolerance">Error Tolerance.</param>
/// <returns>true if spheres are equal within specified tolerance, otherwise false.</returns>
public bool Equals(FSphere other, float tolerance = FMath.KindaSmallNumber)
{
return(Center.Equals(other.Center, tolerance) && FMath.Abs(W - other.W) <= tolerance);
}
/// <summary>
/// Return true if this quaternion is normalized
/// </summary>
public bool IsNormalized()
{
return(FMath.Abs(1.0f - SizeSquared()) < FMath.THRESH_QUAT_NORMALIZED);
}
/// <summary>
/// Get the maximum absolute value of the vector's components.
/// </summary>
/// <returns>The maximum absolute value of the vector's components.</returns>
public float GetAbsMax()
{
return(FMath.Max(FMath.Abs(X), FMath.Abs(Y)));
}
/// <summary>
/// Utility to check if all of the components of this vector are nearly zero given the tolerance.
/// </summary>
public bool IsNearlyZero3(float tolerance = FMath.KindaSmallNumber)
{
return(FMath.Abs(X) <= tolerance && FMath.Abs(Y) <= tolerance && FMath.Abs(Z) <= tolerance);
}
/// <summary>
/// Check if the vector is of unit length, with specified tolerance.
/// </summary>
/// <param name="lengthSquaredTolerance">Tolerance against squared length.</param>
/// <returns>true if the vector is a unit vector within the specified tolerance.</returns>
public bool IsUnit3(float lengthSquaredTolerance = FMath.KindaSmallNumber)
{
return(FMath.Abs(1.0f - SizeSquared3()) < lengthSquaredTolerance);
}
/// <summary>
/// Get a copy of this vector with absolute value of each component.
/// </summary>
/// <returns>A copy of this vector with absolute value of each component.</returns>
public FVector2D GetAbs()
{
return(new FVector2D(FMath.Abs(X), FMath.Abs(Y)));
}
