From 2b051a710b5e352355a91dd394d95891667e6667 Mon Sep 17 00:00:00 2001 From: Yuki Kitagawa Date: Fri, 1 Nov 2024 21:30:54 +0800 Subject: [PATCH] Add double vector math back --- Quadratic.Carto.csproj | 1 + src/Math/DBasis.cs | 210 ++++++++++++++++++++++++++++++ src/Math/VectorsExt.Conversion.cs | 100 ++++++++++++++ 3 files changed, 311 insertions(+) create mode 100644 src/Math/DBasis.cs diff --git a/Quadratic.Carto.csproj b/Quadratic.Carto.csproj index f6d64a5..39a5382 100644 --- a/Quadratic.Carto.csproj +++ b/Quadratic.Carto.csproj @@ -4,6 +4,7 @@ + net8.0 diff --git a/src/Math/DBasis.cs b/src/Math/DBasis.cs new file mode 100644 index 0000000..c94c55e --- /dev/null +++ b/src/Math/DBasis.cs @@ -0,0 +1,210 @@ +using System; +using Vim.Math3d; + +namespace Quadratic.Carto.MathExt; + +/// +/// A 3x3 matrix of doubles. Made to aid . +/// +public readonly record struct DBasis +{ + public readonly DVector3 col1; + public readonly DVector3 col2; + public readonly DVector3 col3; + + public DVector3 Row1 => new(col1.X, col2.X, col3.X); + public DVector3 Row2 => new(col1.Y, col2.Y, col3.Y); + public DVector3 Row3 => new(col1.Z, col2.Z, col3.Z); + + public DBasis(DVector3 col1, DVector3 col2, DVector3 col3) + { + this.col1 = col1; + this.col2 = col2; + this.col3 = col3; + } + + public DBasis(Godot.Basis basis) + { + col1 = new DVector3(basis.X.X, basis.X.Y, basis.X.Z); + col2 = new DVector3(basis.Y.X, basis.Y.Y, basis.Y.Z); + col3 = new DVector3(basis.Z.X, basis.Z.Y, basis.Z.Z); + } + + public DBasis(double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3) + { + col1 = new DVector3(x1, y1, z1); + col2 = new DVector3(x2, y2, z2); + col3 = new DVector3(x3, y3, z3); + } + + public DBasis(DVector3 x, DVector3 y, bool normalize = false) + { + col1 = x; + col2 = y; + col3 = x.Cross(y); + if (normalize) + { + col3 = col3.Normalize(); + } + } + + public DBasis(double x1, double y1, double z1, double x2, double y2, double z2, bool normalize = false) + { + col1 = new DVector3(x1, y1, z1); + col2 = new DVector3(x2, y2, z2); + col3 = col1.Cross(col2); + if (normalize) + { + col3 = col3.Normalize(); + } + } + + public static DBasis Identity => new( + new DVector3(1, 0, 0), + new DVector3(0, 1, 0), + new DVector3(0, 0, 1) + ); + + /// + /// Transform a vector. + /// + /// + /// + public readonly DVector3 Transform(DVector3 b) + { + return col1 * b.X + col2 * b.Y + col3 * b.Z; + } + + /// + /// Inverse transform a vector. Only works with orthonormal bases. + /// + /// + /// + public readonly DVector3 OrthonormalInvTransform(DVector3 b) + { + return this.Transpose().Transform(b); + } + + public bool IsUnit() + { + return Math.Abs(col1.LengthSquared() - 1) < 1e-6 + && Math.Abs(col2.LengthSquared() - 1) < 1e-6 + && Math.Abs(col3.LengthSquared() - 1) < 1e-6; + } + + public static DBasis operator *(DBasis a, DBasis b) + { + return new DBasis( + new DVector3( + a.col1.X * b.col1.X + a.col2.X * b.col1.Y + a.col3.X * b.col1.Z, + a.col1.Y * b.col1.X + a.col2.Y * b.col1.Y + a.col3.Y * b.col1.Z, + a.col1.Z * b.col1.X + a.col2.Z * b.col1.Y + a.col3.Z * b.col1.Z + ), + new DVector3( + a.col1.X * b.col2.X + a.col2.X * b.col2.Y + a.col3.X * b.col2.Z, + a.col1.Y * b.col2.X + a.col2.Y * b.col2.Y + a.col3.Y * b.col2.Z, + a.col1.Z * b.col2.X + a.col2.Z * b.col2.Y + a.col3.Z * b.col2.Z + ), + new DVector3( + a.col1.X * b.col3.X + a.col2.X * b.col3.Y + a.col3.X * b.col3.Z, + a.col1.Y * b.col3.X + a.col2.Y * b.col3.Y + a.col3.Y * b.col3.Z, + a.col1.Z * b.col3.X + a.col2.Z * b.col3.Y + a.col3.Z * b.col3.Z + ) + ); + } + + public static DVector3 operator *(DBasis a, DVector3 b) + { + return a.Transform(b); + } + + public static DBasis operator *(DBasis a, double b) + { + return new DBasis(a.col1 * b, a.col2 * b, a.col3 * b); + } + + public static DBasis operator +(DBasis a, DBasis b) + { + return new DBasis(a.col1 + b.col1, a.col2 + b.col2, a.col3 + b.col3); + } + + public static DBasis operator -(DBasis a, DBasis b) + { + return new DBasis(a.col1 - b.col1, a.col2 - b.col2, a.col3 - b.col3); + } + + public static DBasis operator -(DBasis a) + { + return new DBasis(-a.col1, -a.col2, -a.col3); + } + + public static DBasis operator /(DBasis a, double b) + { + return new DBasis(a.col1 / b, a.col2 / b, a.col3 / b); + } + + public static DBasis operator /(DBasis a, DBasis b) + { + return a * b.Transpose(); + } + + public readonly DBasis Transpose() + { + return new DBasis(Row1, Row2, Row3); + } + + [Obsolete("The implementation is flawed and awaiting a fix")] + public readonly DBasis Inverse() + { + double + m00 = col1.X, + m01 = col1.Y, + m02 = col1.Z, + m10 = col2.X, + m11 = col2.Y, + m12 = col2.Z, + m20 = col3.X, + m21 = col3.Y, + m22 = col3.Z; + + double det = m00 * (m11 * m22 - m21 * m12) - + m01 * (m10 * m22 - m12 * m20) + + m02 * (m10 * m21 - m11 * m20); + + if (Math.Abs(det) < 1e-6) + return Identity; + + double invDet = 1 / det; + + double + mi00 = (m11 * m22 - m21 * m12) * invDet, + mi01 = (m02 * m21 - m01 * m22) * invDet, + mi02 = (m01 * m12 - m02 * m11) * invDet, + mi10 = (m12 * m20 - m10 * m22) * invDet, + mi11 = (m00 * m22 - m02 * m20) * invDet, + mi12 = (m02 * m10 - m00 * m12) * invDet, + mi20 = (m10 * m21 - m20 * m11) * invDet, + mi21 = (m20 * m01 - m00 * m21) * invDet, + mi22 = (m00 * m11 - m10 * m01) * invDet; + + return new DBasis( + new DVector3(mi00, mi10, mi20), + new DVector3(mi01, mi11, mi21), + new DVector3(mi02, mi12, mi22) + ); + } + + public readonly Godot.Basis AsBasis() + { + return new Godot.Basis( + (float)col1.X, (float)col1.Y, (float)col1.Z, + (float)col2.X, (float)col2.Y, (float)col2.Z, + (float)col3.X, (float)col3.Y, (float)col3.Z + ); + } + + public override string ToString() + { + return $"({col1}, {col2}, {col3})"; + } +} diff --git a/src/Math/VectorsExt.Conversion.cs b/src/Math/VectorsExt.Conversion.cs index 2a2b159..79a8c85 100644 --- a/src/Math/VectorsExt.Conversion.cs +++ b/src/Math/VectorsExt.Conversion.cs @@ -3,6 +3,26 @@ namespace Quadratic.Carto.MathExt; public static partial class VectorsExt { + public static Godot.Vector3 AsGodot(this Vim.Math3d.Vector3 v) + { + return new Godot.Vector3(v.X, v.Y, v.Z); + } + + public static Vim.Math3d.Vector3 AsVim(this Godot.Vector3 v) + { + return new Vim.Math3d.Vector3(v.X, v.Y, v.Z); + } + + public static System.Numerics.Vector3 AsSystem(this Vim.Math3d.Vector3 v) + { + return new System.Numerics.Vector3(v.X, v.Y, v.Z); + } + + public static Vim.Math3d.Vector3 AsVim(this System.Numerics.Vector3 v) + { + return new Vim.Math3d.Vector3(v.X, v.Y, v.Z); + } + public static Godot.Vector3 AsGodot(this System.Numerics.Vector3 v) { return new Godot.Vector3(v.X, v.Y, v.Z); @@ -13,6 +33,36 @@ public static partial class VectorsExt return new System.Numerics.Vector3(v.X, v.Y, v.Z); } + public static DVector3 AsDouble(this Vim.Math3d.Vector3 v) + { + return new DVector3(v.X, v.Y, v.Z); + } + + public static Vim.Math3d.Vector3 AsSingle(this DVector3 v) + { + return new Vim.Math3d.Vector3((float)v.X, (float)v.Y, (float)v.Z); + } + + public static Godot.Vector2 AsGodot(this Vim.Math3d.Vector2 v) + { + return new Godot.Vector2(v.X, v.Y); + } + + public static Vim.Math3d.Vector2 AsVim(this Godot.Vector2 v) + { + return new Vim.Math3d.Vector2(v.X, v.Y); + } + + public static System.Numerics.Vector2 AsSystem(this Vim.Math3d.Vector2 v) + { + return new System.Numerics.Vector2(v.X, v.Y); + } + + public static Vim.Math3d.Vector2 AsVim(this System.Numerics.Vector2 v) + { + return new Vim.Math3d.Vector2(v.X, v.Y); + } + public static Godot.Vector2 AsGodot(this System.Numerics.Vector2 v) { return new Godot.Vector2(v.X, v.Y); @@ -23,6 +73,26 @@ public static partial class VectorsExt return new System.Numerics.Vector2(v.X, v.Y); } + public static Godot.Quaternion AsGodot(this Vim.Math3d.Quaternion q) + { + return new Godot.Quaternion(q.X, q.Y, q.Z, q.W); + } + + public static Vim.Math3d.Quaternion AsVim(this Godot.Quaternion q) + { + return new Vim.Math3d.Quaternion(q.X, q.Y, q.Z, q.W); + } + + public static System.Numerics.Quaternion AsSystem(this Vim.Math3d.Quaternion q) + { + return new System.Numerics.Quaternion(q.X, q.Y, q.Z, q.W); + } + + public static Vim.Math3d.Quaternion AsVim(this System.Numerics.Quaternion q) + { + return new Vim.Math3d.Quaternion(q.X, q.Y, q.Z, q.W); + } + public static Godot.Quaternion AsGodot(this System.Numerics.Quaternion q) { return new Godot.Quaternion(q.X, q.Y, q.Z, q.W); @@ -33,6 +103,26 @@ public static partial class VectorsExt return new System.Numerics.Quaternion(q.X, q.Y, q.Z, q.W); } + public static Godot.Vector4 AsGodot(this Vim.Math3d.Vector4 v) + { + return new Godot.Vector4(v.X, v.Y, v.Z, v.W); + } + + public static Vim.Math3d.Vector4 AsVim(this Godot.Vector4 v) + { + return new Vim.Math3d.Vector4(v.X, v.Y, v.Z, v.W); + } + + public static System.Numerics.Vector4 AsSystem(this Vim.Math3d.Vector4 v) + { + return new System.Numerics.Vector4(v.X, v.Y, v.Z, v.W); + } + + public static Vim.Math3d.Vector4 AsVim(this System.Numerics.Vector4 v) + { + return new Vim.Math3d.Vector4(v.X, v.Y, v.Z, v.W); + } + public static Godot.Vector4 AsGodot(this System.Numerics.Vector4 v) { return new Godot.Vector4(v.X, v.Y, v.Z, v.W); @@ -42,6 +132,16 @@ public static partial class VectorsExt { return new System.Numerics.Vector4(v.X, v.Y, v.Z, v.W); } + + public static Vim.Math3d.Ray AsVim(this Ray ray) + { + return new Vim.Math3d.Ray(ray.origin.AsVim(), ray.direction.AsVim()); + } + + public static Ray AsGodot(this Vim.Math3d.Ray ray) + { + return new Ray(ray.Position.AsGodot(), ray.Direction.AsGodot()); + } }