/// <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>
/// 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);
}
/// <summary>
/// Returns a random unit vector, uniformly distributed, within the specified cone.
/// </summary>
/// <param name="dir">The center direction of the cone</param>
/// <param name="horizontalConeHalfAngleRad">Horizontal half-angle of cone, in radians.</param>
/// <param name="verticalConeHalfAngleRad">Vertical half-angle of cone, in radians.</param>
/// <returns>Normalized vector within the specified cone.</returns>
public FVector VRandCone(FVector dir, float horizontalConeHalfAngleRad, float verticalConeHalfAngleRad)
{
if ((verticalConeHalfAngleRad > 0.0f) && (horizontalConeHalfAngleRad > 0.0f))
{
float randU = FRand();
float randV = FRand();
// Get spherical coords that have an even distribution over the unit sphere
// Method described at http://mathworld.wolfram.com/SpherePointPicking.html
float theta = 2.0f * FMath.PI * randU;
float phi = FMath.Acos((2.0f * randV) - 1.0f);
// restrict phi to [0, ConeHalfAngleRad]
// where ConeHalfAngleRad is now a function of Theta
// (specifically, radius of an ellipse as a function of angle)
// function is ellipse function (x/a)^2 + (y/b)^2 = 1, converted to polar coords
float coneHalfAngleRad = FMath.Square(FMath.Cos(theta) / verticalConeHalfAngleRad) + FMath.Square(FMath.Sin(theta) / horizontalConeHalfAngleRad);
coneHalfAngleRad = FMath.Sqrt(1.0f / coneHalfAngleRad);
// clamp to make a cone instead of a sphere
phi = FMath.Fmod(phi, coneHalfAngleRad);
// get axes we need to rotate around
FMatrix dirMat = FMatrix.CreateRotation(dir.Rotation());
// note the axis translation, since we want the variation to be around X
FVector dirZ = dirMat.GetUnitAxis(EAxis.X);
FVector dirY = dirMat.GetUnitAxis(EAxis.Y);
FVector result = dir.RotateAngleAxis(phi * 180.0f / FMath.PI, dirY);
result = result.RotateAngleAxis(theta * 180.0f / FMath.PI, dirZ);
// ensure it's a unit vector (might not have been passed in that way)
result = result.GetSafeNormal();
return(result);
}
else
{
return(dir.GetSafeNormal());
}
}
/// <summary>
/// Spherical interpolation. Will correct alignment. Result is NOT normalized.
/// </summary>
public static FQuat Slerp_NotNormalized(FQuat quat1, FQuat quat2, float slerp)
{
// Get cosine of angle between quats.
float rawCosom =
quat1.X * quat2.X +
quat1.Y * quat2.Y +
quat1.Z * quat2.Z +
quat1.W * quat2.W;
// Unaligned quats - compensate, results in taking shorter route.
float cosom = FMath.FloatSelect(rawCosom, rawCosom, -rawCosom);
float scale0, scale1;
if (cosom < 0.9999f)
{
float omega = FMath.Acos(cosom);
float invSin = 1.0f / FMath.Sin(omega);
scale0 = FMath.Sin((1.0f - slerp) * omega) * invSin;
scale1 = FMath.Sin(slerp * omega) * invSin;
}
else
{
// Use linear interpolation.
scale0 = 1.0f - slerp;
scale1 = slerp;
}
// In keeping with our flipped Cosom:
scale1 = FMath.FloatSelect(rawCosom, scale1, -scale1);
FQuat result;
result.X = scale0 * quat1.X + scale1 * quat2.X;
result.Y = scale0 * quat1.Y + scale1 * quat2.Y;
result.Z = scale0 * quat1.Z + scale1 * quat2.Z;
result.W = scale0 * quat1.W + scale1 * quat2.W;
return(result);
}
/// <summary>
/// Returns a random unit vector, uniformly distributed, within the specified cone.
/// </summary>
/// <param name="dir">The center direction of the cone</param>
/// <param name="coneHalfAngleRad">Half-angle of cone, in radians.</param>
/// <returns>Normalized vector within the specified cone.</returns>
public FVector VRandCone(FVector dir, float coneHalfAngleRad)
{
if (coneHalfAngleRad > 0.0f)
{
float randU = FRand();
float randV = FRand();
// Get spherical coords that have an even distribution over the unit sphere
// Method described at http://mathworld.wolfram.com/SpherePointPicking.html
float theta = 2.0f * FMath.PI * randU;
float phi = FMath.Acos((2.0f * randV) - 1.0f);
// restrict phi to [0, ConeHalfAngleRad]
// this gives an even distribution of points on the surface of the cone
// centered at the origin, pointing upward (z), with the desired angle
phi = FMath.Fmod(phi, coneHalfAngleRad);
// get axes we need to rotate around
FMatrix dirMat = FMatrix.CreateRotation(dir.Rotation());
// note the axis translation, since we want the variation to be around X
FVector dirZ = dirMat.GetUnitAxis(EAxis.X);
FVector dirY = dirMat.GetUnitAxis(EAxis.Y);
FVector result = dir.RotateAngleAxis(phi * 180.0f / FMath.PI, dirY);
result = result.RotateAngleAxis(theta * 180.0f / FMath.PI, dirZ);
// ensure it's a unit vector (might not have been passed in that way)
result = result.GetSafeNormal();
return(result);
}
else
{
return(dir.GetSafeNormal());
}
}
/// <summary>
/// Find the angular distance between two rotation quaternions (in radians)
/// </summary>
public float AngularDistance(FQuat q)
{
float InnerProd = X * q.X + Y * q.Y + Z * q.Z + W * q.W;
return(FMath.Acos((2 * InnerProd * InnerProd) - 1.0f));
}
/// <summary>
/// Get the angle of this quaternion
/// </summary>
public float GetAngle()
{
return(2.0f * FMath.Acos(W));
}
/// <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);
}
