// 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);
}
public FRotator GroundVUYawPitchRow(string rotData)
{
if (rotData.Length < 1)
{
return(new FRotator(0, 0, 0));
}
char[] charSeparators = new char[] { ' ' };
var values = rotData.Split(charSeparators, StringSplitOptions.RemoveEmptyEntries);
FVector XAxis = new FVector(Convert.ToDouble(values[0]), Convert.ToDouble(values[1]), Convert.ToDouble(values[2]));
//XAxis = XAxis.GetSafeNormal();
FVector YAxis = new FVector(-1.0 * Convert.ToDouble(values[3]), -1.0 * Convert.ToDouble(values[4]), -1.0 * Convert.ToDouble(values[5]));
// YAxis = YAxis.GetSafeNormal();
FVector ZAxis = new FVector(1.0 * Convert.ToDouble(values[6]), 1.0 * Convert.ToDouble(values[7]), -1.0 * Convert.ToDouble(values[8]));
//ZAxis = ZAxis.GetSafeNormal();
//double sy = Math.Sqrt(XAxis.X * XAxis.X + XAxis.Y * XAxis.Y);
//FRotator Rotator = new FRotator(Math.Atan2(-YAxis.Z, YAxis.Y) * 180.0 / PI,Math.Atan2(-ZAxis.X, sy) * 180.0 / PI, 0);
FRotator Rotator = new FRotator(Math.Atan2(XAxis.Z, Math.Sqrt(XAxis.X * XAxis.X + XAxis.Y * XAxis.Y)) * 180.0 / PI,
Math.Atan2(XAxis.Y, XAxis.X) * 180.0 / PI, 0);
//FRotator Rotator = new FRotator(
// Math.Atan2(XAxis.Y, XAxis.X) * 180.0 / PI, Math.Atan2(XAxis.Z, Math.Sqrt(XAxis.X * XAxis.X + XAxis.Y * XAxis.Y)) * 180.0 / PI, 0);
//
double[] mt = BuildRotationMatrix(Rotator);
FVector SYAxis = new FVector(mt[3], mt[4], mt[5]);// FRotationMatrix(Rotator).GetScaledAxis(EAxis::Y);
Rotator.Roll = Math.Atan2(ZAxis.DotProduct(SYAxis), YAxis.DotProduct(SYAxis)) * 180.0 / PI;
Rotator.Pitch *= -1.0;
return(Rotator);
}
/// <summary>
/// Gets a bounding volume transformed by a matrix.
/// </summary>
/// <param name="m">The matrix.</param>
/// <returns>The transformed volume.</returns>
public FBoxSphereBounds TransformBy(FMatrix m)
{
FBoxSphereBounds result;
FVector vecOrigin = Origin;
FVector vecExtent = BoxExtent;
FVector m0 = m.GetRow(0);
FVector m1 = m.GetRow(1);
FVector m2 = m.GetRow(2);
FVector m3 = m.GetRow(3);
FVector newOrigin = FVector.Replicate(vecOrigin, 0) * m0;
newOrigin += (FVector.Replicate(vecOrigin, 1) * m1);
newOrigin += (FVector.Replicate(vecOrigin, 2) * m2);
newOrigin = newOrigin + m3;
FVector newExtent = (FVector.Replicate(vecExtent, 0) * m0).GetAbs();
newExtent += (FVector.Replicate(vecExtent, 1) * m1).GetAbs();
newExtent += (FVector.Replicate(vecExtent, 2) * m2).GetAbs();
result.BoxExtent = newExtent;
result.Origin = newOrigin;
FVector maxRadius = m0 * m0;
maxRadius += m1 * m1;
maxRadius += m2 * m2;
maxRadius = FVector.ComponentMax(FVector.ComponentMax(maxRadius, FVector.Replicate(maxRadius, 1)), FVector.Replicate(maxRadius, 2));
result.SphereRadius = FMath.Sqrt(maxRadius[0]) * SphereRadius;
// For non-uniform scaling, computing sphere radius from a box results in a smaller sphere.
float boxExtentMagnitude = FMath.Sqrt(FVector.DotProduct(newExtent, newExtent));
result.SphereRadius = FMath.Min(result.SphereRadius, boxExtentMagnitude);
result.DiagnosticCheckNaN();
return(result);
}
/// <summary>
/// Get the swing and twist decomposition for a specified axis (assumes normalised quaternion and twist axis).
/// </summary>
/// <param name="twistAxis">Axis to use for decomposition</param>
/// <param name="swing">swing component quaternion</param>
/// <param name="twist">Twist component quaternion</param>
public void ToSwingTwist(FVector twistAxis, out FQuat swing, out FQuat twist)
{
// Vector part projected onto twist axis
FVector Projection = FVector.DotProduct(twistAxis, new FVector(X, Y, Z)) * twistAxis;
// Twist quaternion
twist = new FQuat(Projection.X, Projection.Y, Projection.Z, W);
// Singularity close to 180deg
if (twist.SizeSquared() == 0.0f)
{
twist = FQuat.Identity;
}
else
{
twist.Normalize();
}
// Set swing
swing = this * twist.Inverse();
}
