From c45fa5ca469f2989541e11d1cc4b04aec612efd3 Mon Sep 17 00:00:00 2001 From: DomCR Date: Tue, 24 Sep 2024 07:29:34 +0200 Subject: [PATCH 1/3] transform class --- CSMath.Tests/AssertUtils.cs | 29 ++++ CSMath.Tests/CSMathRandom.cs | 82 +++++++++++ CSMath.Tests/TestVariables.cs | 9 ++ CSMath.Tests/TransformTests.cs | 61 ++++++++ CSMath/CSMath.projitems | 1 + CSMath/Transform.cs | 257 +++++++++++++++++++++++++++++++++ 6 files changed, 439 insertions(+) create mode 100644 CSMath.Tests/AssertUtils.cs create mode 100644 CSMath.Tests/CSMathRandom.cs create mode 100644 CSMath.Tests/TestVariables.cs create mode 100644 CSMath.Tests/TransformTests.cs create mode 100644 CSMath/Transform.cs diff --git a/CSMath.Tests/AssertUtils.cs b/CSMath.Tests/AssertUtils.cs new file mode 100644 index 0000000..0b178c8 --- /dev/null +++ b/CSMath.Tests/AssertUtils.cs @@ -0,0 +1,29 @@ +using Xunit; + +namespace CSMath.Tests +{ + public static class AssertUtils + { + public static void AreEqual(T expected, T actual, string varname = "undefined") + { + switch (expected, actual) + { + case (double d1, double d2): + Assert.Equal(d1, d2, 10); + break; + case (XY xy1, XY xy2): + Assert.True(xy1.IsEqual(xy2, TestVariables.DecimalPrecision), $"Different {varname}"); + break; + case (XYZ xyz1, XYZ xyz2): + Assert.True(xyz1.IsEqual(xyz2, TestVariables.DecimalPrecision), $"Different {varname}"); + break; + case (Quaternion q1, Quaternion q2): + Assert.True(q1.Equals(q2, TestVariables.DecimalPrecision), $"Different {varname}"); + break; + default: + Assert.Equal(expected, actual); + break; + } + } + } +} diff --git a/CSMath.Tests/CSMathRandom.cs b/CSMath.Tests/CSMathRandom.cs new file mode 100644 index 0000000..b541477 --- /dev/null +++ b/CSMath.Tests/CSMathRandom.cs @@ -0,0 +1,82 @@ +using System; +using System.Linq; + +namespace CSMath.Tests +{ + public class CSMathRandom : Random + { + public CSMathRandom() : base() { } + + public CSMathRandom(int seed) : base(seed) { } + + public short NextShort() + { + return (short)Next(short.MinValue, short.MaxValue); + } + + public object Next(Type t) + { + object value = Activator.CreateInstance(t); + + return setValue(value); + } + + public T Next() + where T : struct + { + T value = default(T); + + return setValue(value); + } + + public XY NextXY() + { + return new XY(this.NextDouble(), this.NextDouble()); + } + + public XYZ NextXYZ() + { + return new XYZ(this.NextDouble(), this.NextDouble(), this.NextDouble()); + } + + public string RandomString(int length) + { + const string chars = "ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789"; + return new string(Enumerable.Repeat(chars, length) + .Select(s => s[this.Next(s.Length)]).ToArray()); + } + + private T setValue(T value) + { + switch (value) + { + case bool: + return (T)Convert.ChangeType(this.Next(0, 1) == 1, typeof(bool)); + case byte: + return (T)Convert.ChangeType(this.Next(byte.MinValue, byte.MaxValue), typeof(byte)); + case char: + return (T)Convert.ChangeType(this.Next(byte.MinValue, byte.MaxValue), typeof(char)); + case short: + return (T)Convert.ChangeType(this.Next(short.MinValue, short.MaxValue), typeof(short)); + case ushort: + return (T)Convert.ChangeType(this.Next(ushort.MinValue, ushort.MaxValue), typeof(ushort)); + case int: + return (T)Convert.ChangeType(this.Next(int.MinValue, int.MaxValue), typeof(int)); + case double: + return (T)Convert.ChangeType(this.NextDouble(), typeof(double)); + case long: + return (T)Convert.ChangeType(this.Next(int.MinValue, int.MaxValue), typeof(long)); + case ulong: + return (T)Convert.ChangeType(this.Next(0, int.MaxValue), typeof(ulong)); + case string: + return (T)Convert.ChangeType(RandomString(10), typeof(string)); + case XY: + return (T)Convert.ChangeType(NextXY(), typeof(XY)); + case XYZ: + return (T)Convert.ChangeType(NextXYZ(), typeof(XYZ)); + default: + throw new NotImplementedException(); + } + } + } +} diff --git a/CSMath.Tests/TestVariables.cs b/CSMath.Tests/TestVariables.cs new file mode 100644 index 0000000..a545573 --- /dev/null +++ b/CSMath.Tests/TestVariables.cs @@ -0,0 +1,9 @@ +namespace CSMath.Tests +{ + public static class TestVariables + { + public const double Delta = 0.00001d; + + public const int DecimalPrecision = 5; + } +} diff --git a/CSMath.Tests/TransformTests.cs b/CSMath.Tests/TransformTests.cs new file mode 100644 index 0000000..e7b4e0d --- /dev/null +++ b/CSMath.Tests/TransformTests.cs @@ -0,0 +1,61 @@ +using Xunit; + +namespace CSMath.Tests +{ + public class TransformTests + { + private CSMathRandom _random = new CSMathRandom(); + + [Fact()] + public void TranslationTest() + { + XYZ translation = _random.Next(); + XYZ scale = _random.Next(); + XYZ rotation = _random.Next(); + + Transform transform = new Transform(translation, scale, rotation); + + Assert.Equal(translation, transform.Translation); + } + + [Fact()] + public void ScaleTest() + { + XYZ translation = _random.Next(); + XYZ scale = _random.Next(); + XYZ rotation = _random.Next(); + + Transform transform = new Transform(translation, scale, rotation); + + Assert.Equal(scale.X, transform.Scale.X, 15); + } + + [Fact] + public void RotationTest() + { + XYZ translation = _random.Next(); + XYZ scale = _random.Next(); + XYZ rotation = _random.Next(); + + Transform transform = new Transform(translation, scale, rotation); + + Assert.Equal(rotation, transform.EulerRotation); + } + + [Fact()] + public void DecomposeTest() + { + XYZ translation = _random.Next(); + XYZ scale = _random.Next(); + XYZ rotation = new XYZ(90, 0, 0); + + Transform transform = new Transform(translation, scale, rotation); + + transform.TryDecompose(out XYZ t, out XYZ s, out Quaternion r); + + AssertUtils.AreEqual(transform.Translation, t, "translation"); + AssertUtils.AreEqual(transform.Scale, s, "scale"); + AssertUtils.AreEqual(transform.Quaternion, r, "rotation"); + } + } +} diff --git a/CSMath/CSMath.projitems b/CSMath/CSMath.projitems index 450de48..ee72513 100644 --- a/CSMath/CSMath.projitems +++ b/CSMath/CSMath.projitems @@ -19,6 +19,7 @@ + diff --git a/CSMath/Transform.cs b/CSMath/Transform.cs new file mode 100644 index 0000000..4f3ff68 --- /dev/null +++ b/CSMath/Transform.cs @@ -0,0 +1,257 @@ +namespace CSMath +{ + /// + /// Contains the information for translate/scale/rotation or transform matrix to apply to a geometric shape. + /// + public class Transform + { + /// + /// Translation applied in the transformation. + /// + public XYZ Translation + { + get { return this._translation; } + set + { + this._translation = value; + this.updateMatrix(); + } + } + + /// + /// Scale applied in the transformation. + /// + public XYZ Scale + { + get + { + return this._scale; + } + set + { + if (value.X == 0 || value.Y == 0 || value.Z == 0) + throw new ArgumentException("Scale value cannot be 0"); + + this._scale = value; + this.updateMatrix(); + } + } + + /// + /// Rotation in Euler angles, the value is in degrees. + /// + public XYZ EulerRotation + { + get { return this._rotation; } + set + { + this._rotation = value; + this.updateMatrix(); + } + } + + /// + /// Rotation represented in quaternion form. + /// + public Quaternion Quaternion + { + get + { + XYZ rot = new XYZ(); + rot[0] = MathUtils.DegToRad(this._rotation.X); + rot[1] = MathUtils.DegToRad(this._rotation.Y); + rot[2] = MathUtils.DegToRad(this._rotation.Z); + return Quaternion.CreateFromYawPitchRoll(rot); + } + } + + /// + /// Transform matrix. + /// + public Matrix4 Matrix { get { return this._matrix; } } + + private XYZ _translation = XYZ.Zero; + private XYZ _scale = new XYZ(1, 1, 1); + private XYZ _rotation = XYZ.Zero; + + private Matrix4 _matrix; + + /// + /// Default constructor. + /// + public Transform() + { + this.Translation = XYZ.Zero; + this.EulerRotation = XYZ.Zero; + this.Scale = new XYZ(1, 1, 1); + } + + /// + /// Initialize a transform instance with the specified values. + /// + /// + /// + /// + public Transform(XYZ translation, XYZ scale, XYZ rotation) : this() + { + this.Translation = translation; + this.Scale = scale; + this.EulerRotation = rotation; + } + + /// + /// Initialize a transform instance with the specified matrix. + /// + /// + public Transform(Matrix4 matrix) + { + this._matrix = matrix; + } + + /// + /// Apply transform to a . + /// + /// + /// + public XYZ ApplyTransform(XYZ xyz) + { + return this._matrix * xyz; + } + + /// + /// Try to decompose the transform into it's components. + /// + /// + /// + /// + /// true, if the decompose has succeeded. + public bool TryDecompose(out XYZ translation, out XYZ scaling, out Quaternion rotation) + { + Matrix4 matrix = this._matrix; + + translation = new XYZ(); + scaling = new XYZ(); + rotation = new Quaternion(); + var XYZDouble = new XYZ(); + + if (matrix.m33 == 0.0) + return false; + + Matrix4 matrix4_3 = matrix; + matrix4_3.m03 = 0.0; + matrix4_3.m13 = 0.0; + matrix4_3.m23 = 0.0; + matrix4_3.m33 = 1.0; + + if (matrix4_3.GetDeterminant() == 0.0) + return false; + + if (matrix.m03 != 0.0 || matrix.m13 != 0.0 || matrix.m23 != 0.0) + { + if (!Matrix4.Inverse(matrix, out Matrix4 inverse)) + { + return false; + } + + matrix.m03 = matrix.m13 = matrix.m23 = 0.0; + matrix.m33 = 1.0; + } + + translation.X = matrix.m30; + matrix.m30 = 0.0; + translation.Y = matrix.m31; + matrix.m31 = 0.0; + translation.Z = matrix.m32; + matrix.m32 = 0.0; + + XYZ[] cols = new XYZ[3] + { + new XYZ(matrix.m00, matrix.m01, matrix.m02), + new XYZ(matrix.m10, matrix.m11, matrix.m12), + new XYZ(matrix.m20, matrix.m21, matrix.m22) + }; + + scaling.X = cols[0].GetLength(); + cols[0] = cols[0].Normalize(); + XYZDouble.X = cols[0].Dot(cols[1]); + cols[1] = cols[1] * 1 + cols[0] * -XYZDouble.X; + + scaling.Y = cols[1].GetLength(); + cols[1] = cols[1].Normalize(); + XYZDouble.Y = cols[0].Dot(cols[2]); + cols[2] = cols[2] * 1 + cols[0] * -XYZDouble.Y; + + XYZDouble.Z = cols[1].Dot(cols[2]); + cols[2] = cols[2] * 1 + cols[1] * -XYZDouble.Z; + scaling.Z = cols[2].GetLength(); + cols[2] = cols[2].Normalize(); + + XYZ rhs = XYZ.Cross(cols[1], cols[2]); + if (cols[0].Dot(rhs) < 0.0) + { + for (int index = 0; index < 3; ++index) + { + scaling.X *= -1.0; + cols[index].X *= -1.0; + cols[index].Y *= -1.0; + cols[index].Z *= -1.0; + } + } + + double trace = cols[0].X + cols[1].Y + cols[2].Z + 1.0; + double qx; + double qy; + double qz; + double qw; + + if (trace > 0) + { + double s = 0.5 / Math.Sqrt(trace); + qx = (cols[2].Y - cols[1].Z) * s; + qy = (cols[0].Z - cols[2].X) * s; + qz = (cols[1].X - cols[0].Y) * s; + qw = 0.25 / s; + } + else if (cols[0].X > cols[1].Y && cols[0].X > cols[2].Z) + { + double s = Math.Sqrt(1.0 + cols[0].X - cols[1].Y - cols[2].Z) * 2.0; + qx = 0.25 * s; + qy = (cols[0].Y + cols[1].X) / s; + qz = (cols[0].Z + cols[2].X) / s; + qw = (cols[2].Y - cols[1].Z) / s; + } + else if (cols[1].Y > cols[2].Z) + { + double s = Math.Sqrt(1.0 + cols[1].Y - cols[0].X - cols[2].Z) * 2.0; + qx = (cols[0].Y + cols[1].X) / s; + qy = 0.25 * s; + qz = (cols[1].Z + cols[2].Y) / s; + qw = (cols[0].Z - cols[2].X) / s; + } + else + { + double s = Math.Sqrt(1.0 + cols[2].Z - cols[0].X - cols[1].Y) * 2.0; + qx = (cols[0].Z + cols[2].X) / s; + qy = (cols[1].Z + cols[2].Y) / s; + qz = 0.25 * s; + qw = (cols[1].X - cols[0].Y) / s; + } + + rotation.X = -qx; + rotation.Y = -qy; + rotation.Z = -qz; + rotation.W = qw; + + return true; + } + + private void updateMatrix() + { + Matrix4 translationMatrix = Matrix4.CreateTranslation(this._translation); + Matrix4 rotationMatrix = Matrix4.CreateFromQuaternion(this.Quaternion); + Matrix4 scaleMatrix = Matrix4.CreateScale(this._scale); + + this._matrix = translationMatrix * rotationMatrix * scaleMatrix; + } + } +} From b4cfcabcb734d8f9e82b07f3bc4dc5f735442ba4 Mon Sep 17 00:00:00 2001 From: DomCR Date: Tue, 24 Sep 2024 08:04:59 +0200 Subject: [PATCH 2/3] ApplyScaleTest --- CSMath.Tests/TransformTests.cs | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/CSMath.Tests/TransformTests.cs b/CSMath.Tests/TransformTests.cs index e7b4e0d..a042ad6 100644 --- a/CSMath.Tests/TransformTests.cs +++ b/CSMath.Tests/TransformTests.cs @@ -21,13 +21,34 @@ public void TranslationTest() [Fact()] public void ScaleTest() { + XYZ xyz = _random.Next(); + XYZ translation = _random.Next(); XYZ scale = _random.Next(); XYZ rotation = _random.Next(); + XYZ result = xyz * scale; + + Transform transform = new Transform(translation, scale, rotation); + + AssertUtils.AreEqual(scale, transform.Scale, "Scale"); + } + + [Fact()] + public void ApplyScaleTest() + { + XYZ xyz = new XYZ(1, 1, 1); + + XYZ translation = XYZ.Zero; + XYZ scale = _random.Next(); + XYZ rotation = XYZ.Zero; + + XYZ expected = xyz * scale; + Transform transform = new Transform(translation, scale, rotation); + XYZ result = transform.ApplyTransform(xyz); - Assert.Equal(scale.X, transform.Scale.X, 15); + AssertUtils.AreEqual(expected, result); } [Fact] From 825d11055b9bb0b7d575ae4869c3ccf81c323200 Mon Sep 17 00:00:00 2001 From: DomCR Date: Tue, 24 Sep 2024 11:03:59 +0200 Subject: [PATCH 3/3] rotation --- CSMath.Tests/AssertUtils.cs | 2 +- CSMath.Tests/TransformTests.cs | 38 ++++++++++++++++++++++++++++++---- CSMath/Transform.cs | 13 +++++++++--- CSMath/VectorExtensions.cs | 19 +++++++++++++++++ 4 files changed, 64 insertions(+), 8 deletions(-) diff --git a/CSMath.Tests/AssertUtils.cs b/CSMath.Tests/AssertUtils.cs index 0b178c8..9b040b0 100644 --- a/CSMath.Tests/AssertUtils.cs +++ b/CSMath.Tests/AssertUtils.cs @@ -15,7 +15,7 @@ public static void AreEqual(T expected, T actual, string varname = "undefined Assert.True(xy1.IsEqual(xy2, TestVariables.DecimalPrecision), $"Different {varname}"); break; case (XYZ xyz1, XYZ xyz2): - Assert.True(xyz1.IsEqual(xyz2, TestVariables.DecimalPrecision), $"Different {varname}"); + Assert.True(xyz1.IsEqual(xyz2, TestVariables.DecimalPrecision), $"Different {varname}: expected {xyz1} actual {xyz2}"); break; case (Quaternion q1, Quaternion q2): Assert.True(q1.Equals(q2, TestVariables.DecimalPrecision), $"Different {varname}"); diff --git a/CSMath.Tests/TransformTests.cs b/CSMath.Tests/TransformTests.cs index a042ad6..df2ba03 100644 --- a/CSMath.Tests/TransformTests.cs +++ b/CSMath.Tests/TransformTests.cs @@ -19,16 +19,29 @@ public void TranslationTest() } [Fact()] - public void ScaleTest() + public void ApplyTranslationTest() { - XYZ xyz = _random.Next(); + XYZ xyz = XYZ.Zero; + + XYZ translation = XYZ.Zero; + XYZ scale = new XYZ(1, 1, 1); + XYZ rotation = XYZ.Zero; + + XYZ expected = xyz + translation; + + Transform transform = new Transform(translation, scale, rotation); + XYZ result = transform.ApplyTransform(xyz); + + AssertUtils.AreEqual(expected, result); + } + [Fact()] + public void ScaleTest() + { XYZ translation = _random.Next(); XYZ scale = _random.Next(); XYZ rotation = _random.Next(); - XYZ result = xyz * scale; - Transform transform = new Transform(translation, scale, rotation); AssertUtils.AreEqual(scale, transform.Scale, "Scale"); @@ -63,6 +76,23 @@ public void RotationTest() Assert.Equal(rotation, transform.EulerRotation); } + [Fact()] + public void ApplyRotationTest() + { + XYZ xyz = XYZ.AxisX; + + XYZ translation = XYZ.Zero; + XYZ scale = new XYZ(1, 1, 1); + XYZ rotation = new XYZ(0, 0, 90); + + XYZ expected = new XYZ(0, 1, 0); + + Transform transform = new Transform(translation, scale, rotation); + XYZ result = transform.ApplyTransform(xyz); + + AssertUtils.AreEqual(expected, result); + } + [Fact()] public void DecomposeTest() { diff --git a/CSMath/Transform.cs b/CSMath/Transform.cs index 4f3ff68..0c35f59 100644 --- a/CSMath/Transform.cs +++ b/CSMath/Transform.cs @@ -91,7 +91,7 @@ public Transform() /// /// /// - /// + /// Rotation value in degrees. public Transform(XYZ translation, XYZ scale, XYZ rotation) : this() { this.Translation = translation; @@ -113,9 +113,16 @@ public Transform(Matrix4 matrix) /// /// /// - public XYZ ApplyTransform(XYZ xyz) + public XYZ ApplyTransform(XYZ xyz, bool roundZero = true) { - return this._matrix * xyz; + XYZ value = this._matrix * xyz; + + if (roundZero) + { + return value.RoundZero(); + } + + return value; } /// diff --git a/CSMath/VectorExtensions.cs b/CSMath/VectorExtensions.cs index 201bc18..d055108 100644 --- a/CSMath/VectorExtensions.cs +++ b/CSMath/VectorExtensions.cs @@ -348,6 +348,25 @@ public static IEnumerable ToEnumerable(this T v) } } + /// + /// Round the zeros within the threshold defined by . + /// + /// + /// + /// + public static T RoundZero(this T vector) + where T : IVector, new() + { + T result = new T(); + + for (int i = 0; i < result.Dimension; i++) + { + result[i] = MathUtils.IsZero(vector[i]) ? 0 : vector[i]; + } + + return result; + } + // Applies a function in all the components of a vector by order private static T applyFunctionByComponentIndex(this T left, T right, Func op) where T : IVector, new()