public double[] MakeFromX(FVector XAxis)
{
double[] mt = new double[9];
FVector NewX = XAxis.GetSafeNormal();
// try to use up if possible
FVector UpVector = new FVector(0, 0, 1.0);// (Math.Abs(NewX.Z) < (1.0 - KINDA_SMALL_NUMBER)) ? FVector(0, 0, 1.f) : FVector(1.f, 0, 0);
FVector NewY = (UpVector.CrossProduct(NewX)).GetSafeNormal();
FVector NewZ = NewX.CrossProduct(NewY);
//FRotator Rotator = new FRotator(Math.Atan2(NewX.Z, Math.Sqrt(NewX.X * NewX.X + NewX.Y * NewX.Y)) * 180.0 / PI,
// Math.Atan2(XAxis.Y, XAxis.X) * 180.0 / PI,
// 0);
//const FVector SYAxis = FRotationMatrix(Rotator).GetScaledAxis(EAxis::Y);
//Rotator.Roll = FMath::Atan2(ZAxis | SYAxis, YAxis | SYAxis) * 180.f / PI;
mt[0] = NewX.X;
mt[1] = NewX.Y;
mt[2] = NewX.Z;
mt[3] = NewY.X;
mt[4] = NewY.Y;
mt[5] = NewY.Z;
mt[6] = NewZ.X;
mt[7] = NewZ.Y;
mt[8] = NewZ.Z;
return(mt);
}
/// <summary>
/// Rotate a vector by the inverse of this quaternion.
/// </summary>
/// <param name="v">the vector to be rotated</param>
/// <returns>vector after rotation by the inverse of this quaternion.</returns>
public FVector UnrotateVector(FVector v)
{
//return Inverse().RotateVector(V);
FVector q = new FVector(-X, -Y, -Z); // Inverse
FVector t = 2.0f * FVector.CrossProduct(q, v);
FVector result = v + (W * t) + FVector.CrossProduct(q, t);
return(result);
}
public double[] GetRightDirMtx()
{
double CP, SP, CY, SY;
SinCos(out SP, out CP, DegreesToRadians(Pitch));
SinCos(out SY, out CY, DegreesToRadians(Yaw));
FVector V = new FVector(CP * CY, CP * SY, SP);
FVector up = new FVector(0, 0, 1);
FVector right = V.CrossProduct(up);
FRotator outRot = new FRotator(0, 0, 0);
return(outRot.MakeFromX(right));
}
/// <summary>
/// Rotate a vector by this quaternion.
/// </summary>
/// <param name="v">the vector to be rotated</param>
/// <returns>vector after rotation</returns>
public FVector RotateVector(FVector v)
{
// http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
// V' = V + 2w(Q x V) + (2Q x (Q x V))
// refactor:
// V' = V + w(2(Q x V)) + (Q x (2(Q x V)))
// T = 2(Q x V);
// V' = V + w*(T) + (Q x T)
FVector q = new FVector(X, Y, Z);
FVector t = 2.0f * FVector.CrossProduct(q, v);
FVector result = v + (W * t) + FVector.CrossProduct(q, t);
return(result);
}
public double[] GetLeftDirMtx()
{
double CP, SP, CY, SY;
SinCos(out SP, out CP, DegreesToRadians(Pitch));
SinCos(out SY, out CY, DegreesToRadians(Yaw));
FVector V = new FVector(CP * CY, CP * SY, SP);
FVector up = new FVector(0, 0, 1);
FVector right = V.CrossProduct(up);
FVector left = right;
left.X *= -1.0 * right.X;
left.Y *= -1.0 * right.Y;
left.Z *= -1.0 * right.Z;
FRotator outRot = new FRotator(0, 0, 0);
return(outRot.MakeFromX(left));
}
