// Transform by matrix
public void PlaneTransformTest1()
{
PlaneD target = new PlaneD(1, 2, 3, 4);
target = PlaneD.Normalize(target);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
m.M41 = 10.0f;
m.M42 = 20.0f;
m.M43 = 30.0f;
PlaneD expected = new PlaneD();
Matrix4x4D.Invert(m, out var inv);
Matrix4x4D itm = Matrix4x4D.Transpose(inv);
double x = target.Normal.X, y = target.Normal.Y, z = target.Normal.Z, w = target.D;
expected.Normal = new Vector3D(
x * itm.M11 + y * itm.M21 + z * itm.M31 + w * itm.M41,
x * itm.M12 + y * itm.M22 + z * itm.M32 + w * itm.M42,
x * itm.M13 + y * itm.M23 + z * itm.M33 + w * itm.M43);
expected.D = x * itm.M14 + y * itm.M24 + z * itm.M34 + w * itm.M44;
PlaneD actual;
actual = PlaneD.Transform(target, m);
Assert.True(MathHelper.Equal(expected, actual), "PlaneD.Transform did not return the expected value.");
}
// Transform by QuaternionD
public void PlaneTransformTest2()
{
PlaneD target = new PlaneD(1, 2, 3, 4);
target = PlaneD.Normalize(target);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
QuaternionD q = QuaternionD.CreateFromRotationMatrix(m);
PlaneD expected = new PlaneD();
double x = target.Normal.X, y = target.Normal.Y, z = target.Normal.Z, w = target.D;
expected.Normal = new Vector3D(
x * m.M11 + y * m.M21 + z * m.M31 + w * m.M41,
x * m.M12 + y * m.M22 + z * m.M32 + w * m.M42,
x * m.M13 + y * m.M23 + z * m.M33 + w * m.M43);
expected.D = x * m.M14 + y * m.M24 + z * m.M34 + w * m.M44;
PlaneD actual;
actual = PlaneD.Transform(target, q);
Assert.True(MathHelper.Equal(expected, actual), "PlaneD.Transform did not return the expected value.");
}
public void Matrix3x3DDeterminantTest()
{
Matrix3x3D target = As3x3(
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0)));
double val = 1.0;
double det = target.GetDeterminant();
Assert.AreEqual(val, det, 1e-15, "Matrix3x3D.Determinant was not set correctly.");
}
public static bool InCircleXZ(Vector3D p1, Vector3D p2, Vector3D p3, Vector3D p4)
{
Matrix4x4D m = Matrix4x4D.identity;
Vector3D[] a =
{
p1, p2, p3, p4,
};
for (int i = 0; i < 4; i++)
{
m.SetRow(i, new Vector4D(a[i].x, a[i].z, (a[i].x * a[i].x) + (a[i].z * a[i].z), 1));
}
return(m.determinant < 0);
}
public void Vector3TransformByQuaternionTest()
{
Vector3D v = new Vector3D(1.0f, 2.0f, 3.0f);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
QuaternionD q = QuaternionD.CreateFromRotationMatrix(m);
Vector3D expected = Vector3D.Transform(v, m);
Vector3D actual = Vector3D.Transform(v, q);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D.Transform did not return the expected value.");
}
public void Vector2TransformByQuaternionTest()
{
Vector2D v = new Vector2D(1.0, 2.0);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0));
QuaternionD q = QuaternionD.CreateFromRotationMatrix(m);
Vector2D expected = Vector2D.Transform(v, m);
Vector2D actual = Vector2D.Transform(v, q);
Assert.That(actual, Is.EqualTo(expected).Using <Vector2D>((a, b) => a.Equals(b, 1e-15)), "Vector2D.Transform did not return the expected value.");
}
public void QuaternionDFromRotationMatrixTest1()
{
Matrix4x4D matrix = Matrix4x4D.Identity;
QuaternionD expected = new QuaternionD(0.0f, 0.0f, 0.0f, 1.0f);
QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.Equal(expected, actual),
$"QuaternionD.CreateFromRotationMatrix did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"QuaternionD.CreateFromQuaternionD did not return the expected value: matrix {matrix} m2 {m2}");
}
public void QuaternionDFromRotationMatrixTest4()
{
for (double angle = 0.0f; angle < 720.0f; angle += 10.0f)
{
Matrix4x4D matrix = Matrix4x4D.CreateRotationZ(angle);
QuaternionD expected = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, angle);
QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.EqualRotation(expected, actual),
$"QuaternionD.CreateFromRotationMatrix angle:{angle} did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"QuaternionD.CreateFromQuaternionD angle:{angle} did not return the expected value: matrix {matrix} m2 {m2}");
}
}
public void QuaternionDFromRotationMatrixWithScaledMatrixTest3()
{
double angle = MathHelper.ToRadians(180.0f);
Matrix4x4D matrix = Matrix4x4D.CreateRotationX(angle) * Matrix4x4D.CreateRotationY(angle);
QuaternionD expected = QuaternionD.CreateFromAxisAngle(Vector3D.UnitY, angle) * QuaternionD.CreateFromAxisAngle(Vector3D.UnitX, angle);
QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.EqualRotation(expected, actual),
$"QuaternionD.CreateFromRotationMatrix did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"QuaternionD.CreateFromQuaternionD did not return the expected value: matrix {matrix} m2 {m2}");
}
public void Vector3TransformTest()
{
Vector3D v = new Vector3D(1.0f, 2.0f, 3.0f);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f));
m.M41 = 10.0f;
m.M42 = 20.0f;
m.M43 = 30.0f;
Vector3D expected = new Vector3D(12.191987f, 21.533493f, 32.616024f);
Vector3D actual;
actual = Vector3D.Transform(v, m);
Assert.True(MathHelper.Equal(expected, actual), "Vector3D.Transform did not return the expected value.");
}
public void Vector2TransformTest()
{
Vector2D v = new Vector2D(1.0, 2.0);
Matrix4x4D m =
Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0)) *
Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0));
m.M41 = 10.0;
m.M42 = 20.0;
m.M43 = 30.0;
Vector2D expected = new Vector2D(10.316987298107781, 22.18301270189222);
Vector2D actual;
actual = Vector2D.Transform(v, m);
Assert.AreEqual(expected, actual, "Vector2D.Transform did not return the expected value.");
}
public override void OnExecute(SceneState state) // runs every frame
{
CameraData camera;
if (state.TryGetData <CameraData>(out camera))
{
transform = camera.Snapshot.TransformMatrix;
view = camera.View.Size;
isReady = true;
if ((oldData == null) || (!AreSnapshotsEqual(oldData.Snapshot, camera.Snapshot)))
{
CameraChanged(this, new EventArgs());
}
oldData = camera;
}
}
public void PostApply(ConfigNode node)
{
rotationMatrix = new Matrix4x4D();
var xc = Math.Cos(XAngle * Mathf.Deg2Rad);
var yc = Math.Cos(YAngle * Mathf.Deg2Rad);
var zc = Math.Cos(ZAngle * Mathf.Deg2Rad);
var xs = Math.Sin(XAngle * Mathf.Deg2Rad);
var ys = Math.Sin(YAngle * Mathf.Deg2Rad);
var zs = Math.Sin(ZAngle * Mathf.Deg2Rad);
//x = 0
//y = 1
//z = 2
rotationMatrix.m00 = ys * xs * zs + yc * zc;
rotationMatrix.m10 = xc * zs;
rotationMatrix.m20 = ys * zc - yc * xs * zs;
rotationMatrix.m01 = ys * xs * zc - yc * zs;
rotationMatrix.m11 = xc * zc;
rotationMatrix.m21 = -yc * xs * zc - ys * zs;
rotationMatrix.m02 = -ys * xc;
rotationMatrix.m12 = xs;
rotationMatrix.m22 = yc * xc;
}
public static Vector3D Transform(Vector3D vector, Matrix4x4D matrix) =>
new Vector3D(vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31 + matrix.M41,
vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32 + matrix.M42,
vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33 + matrix.M43);
public Vector2D TransformNormal(Matrix4x4D matrix) => Transform(this, matrix);
public Vector3D Transform(Matrix4x4D matrix) => Transform(this, matrix);
public static bool Equal(Matrix4x4D a, Matrix4x4D b) => Equal(a.M11, b.M11) && Equal(a.M12, b.M12) && Equal(a.M13, b.M13) && Equal(a.M14, b.M14) && Equal(a.M21, b.M21) && Equal(a.M22, b.M22) && Equal(a.M23, b.M23) && Equal(a.M24, b.M24) && Equal(a.M31, b.M31) && Equal(a.M32, b.M32) && Equal(a.M33, b.M33) && Equal(a.M34, b.M34) && Equal(a.M41, b.M41) && Equal(a.M42, b.M42) && Equal(a.M43, b.M43) && Equal(a.M44, b.M44);
public Vector4D TransformV4(Matrix4x4D matrix) => Vector4D.Transform(this, matrix);
//static Matrix4x4D As4x4(Matrix3x3D m) => // Matrix4x4D.Identity.With(m11: m.M11, m12: m.M12, m13: m.M13, m21: m.M21, m22: m.M22, m23: m.M23, m31: m.M31, m32: m.M32, m33: m.M33); private static Matrix3x3D As3x3(Matrix4x4D m) => new Matrix3x3D(m.M11, m.M12, m.M13, m.M21, m.M22, m.M23, m.M31, m.M32, m.M33);
public static Vector3D TransformNormal(Vector3D normal, Matrix4x4D matrix) =>
new Vector3D(normal.X * matrix.M11 + normal.Y * matrix.M21 + normal.Z * matrix.M31,
normal.X * matrix.M12 + normal.Y * matrix.M22 + normal.Z * matrix.M32,
normal.X * matrix.M13 + normal.Y * matrix.M23 + normal.Z * matrix.M33);
public static Vector2D TransformNormal(Vector2D normal, Matrix4x4D matrix) =>
new Vector2D(normal.X * matrix.M11 + normal.Y * matrix.M21,
normal.X * matrix.M12 + normal.Y * matrix.M22);
public static Vector2D Transform(Vector2D vector, Matrix4x4D matrix) =>
new Vector2D(vector.X * matrix.M11 + vector.Y * matrix.M21 + matrix.M41,
vector.X * matrix.M12 + vector.Y * matrix.M22 + matrix.M42);
public PositionStep(StepManager stepManager)
: base(stepManager)
{
transform = new Matrix4x4D();
view = new Size();
}
