diff --git a/.github/workflows/jitter-tests.yml b/.github/workflows/jitter-tests.yml index ab69f315..66399320 100644 --- a/.github/workflows/jitter-tests.yml +++ b/.github/workflows/jitter-tests.yml @@ -4,7 +4,6 @@ on: push: branches: [ main ] pull_request: - types: [opened, reopened, edited] permissions: id-token: write @@ -12,8 +11,12 @@ permissions: checks: write actions: write +env: + CONFIGURATION: Release + TEST_RESULTS_DIR: ./TestResults + jobs: - build: + build-and-test: runs-on: ubuntu-latest @@ -22,23 +25,28 @@ jobs: working-directory: ./src/JitterTests steps: - - uses: actions/checkout@v4 - - name: Setup .NET - uses: actions/setup-dotnet@v4 - with: - dotnet-version: 9.0.x - - name: Restore dependencies - run: dotnet restore - - name: Build - run: dotnet build --no-restore - - name: Test - run: dotnet test -c Release --no-restore --test-adapter-path:. --logger "trx;LogFileName=test-results.trx" - #run: dotnet test -c Release --no-restore --test-adapter-path:. --logger:"junit;LogFilePath=test-result.xml;MethodFormat=Class;FailureBodyFormat=Verbose" - - - name: TestReport - uses: dorny/test-reporter@v1 - if: success() || failure() # run this step even if previous step failed - with: - name: JitterTests # Name of the check run which will be created - path: ./src/JitterTests/TestResults/test-results.trx # Path to test results - reporter: dotnet-trx # Format of test results + - uses: actions/checkout@v4 + + - name: Setup .NET + uses: actions/setup-dotnet@v4 + with: + dotnet-version: 9.0.x + + - name: Restore dependencies + run: dotnet restore + + - name: Build + run: dotnet build --configuration ${{ env.CONFIGURATION }} --no-restore + + - name: Run Tests + run: | + mkdir -p ${{ env.TEST_RESULTS_DIR }} + + # Run Single Precision Tests + dotnet test --configuration ${{ env.CONFIGURATION }} --no-restore \ + --test-adapter-path:. --logger "trx;LogFileName=single-precision.trx" + + # Run Double Precision Tests + dotnet test --configuration ${{ env.CONFIGURATION }} -p:DefineConstants="USE_DOUBLE_PRECISION" --no-restore \ + --test-adapter-path:. --logger "trx;LogFileName=double-precision.trx" + if: always() \ No newline at end of file diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml index 2089c377..0aed3bcc 100644 --- a/.github/workflows/publish.yml +++ b/.github/workflows/publish.yml @@ -16,14 +16,13 @@ on: env: DOTNET_SKIP_FIRST_TIME_EXPERIENCE: 1 DOTNET_NOLOGO: true - NuGetDirectory: ${{ github.workspace}}/nuget + NuGetDirectory: ${{ github.workspace }}/nuget defaults: run: shell: pwsh jobs: - create_nuget: runs-on: ubuntu-latest @@ -33,59 +32,63 @@ jobs: working-directory: ./src/Jitter2 steps: - - uses: actions/checkout@v4 - with: - fetch-depth: 0 # Get all history to allow automatic versioning using MinVer + - uses: actions/checkout@v4 + with: + fetch-depth: 0 # Get all history to allow automatic versioning using MinVer - # Install the .NET SDK indicated in the global.json file - - name: Setup .NET - uses: actions/setup-dotnet@v4 - with: - dotnet-version: 9.0.x + - name: Setup .NET + uses: actions/setup-dotnet@v4 + with: + dotnet-version: 9.0.x - - run: dotnet add package MinVer + - name: Add MinVer and Reproducible Builds + run: | + dotnet add package MinVer + dotnet add package DotNet.ReproducibleBuilds - - run: dotnet add package DotNet.ReproducibleBuilds + # Pack single-precision version + - name: Pack Single Precision + run: dotnet pack --configuration Release --output ${{ env.NuGetDirectory }} - # Create the NuGet package in the folder from the environment variable NuGetDirectory - - run: dotnet pack --configuration Release --output ${{ env.NuGetDirectory }} + # Pack double-precision version + - name: Pack Double Precision + run: dotnet pack --configuration Release -p:DoublePrecision=true --output ${{ env.NuGetDirectory }} - # Publish the NuGet package as an artifact, so they can be used in the following jobs - - uses: actions/upload-artifact@v4 - with: - name: nuget - if-no-files-found: error - retention-days: 7 - path: ${{ env.NuGetDirectory }}/* + # Upload artifacts + - uses: actions/upload-artifact@v4 + with: + name: nuget + if-no-files-found: error + retention-days: 7 + path: ${{ env.NuGetDirectory }}/* validate_nuget: - runs-on: ubuntu-latest - needs: [ create_nuget ] + runs-on: ubuntu-latest + needs: [create_nuget] steps: - # Install the .NET SDK indicated in the global.json file - name: Setup .NET uses: actions/setup-dotnet@v4 with: dotnet-version: 9.0.x - # Download the NuGet package created in the previous job - - uses: actions/download-artifact@v4 + - name: Download NuGet Packages + uses: actions/download-artifact@v4 with: name: nuget path: ${{ env.NuGetDirectory }} - - name: Install nuget validator + - name: Install NuGet Validator run: dotnet tool update Meziantou.Framework.NuGetPackageValidation.Tool --global - # Validate metadata and content of the NuGet package - # https://www.nuget.org/packages/Meziantou.Framework.NuGetPackageValidation.Tool#readme-body-tab - # If some rules are not applicable, you can disable them - # using the --excluded-rules or --excluded-rule-ids option - - name: Validate package - run: meziantou.validate-nuget-package (Get-ChildItem "${{ env.NuGetDirectory }}/*.nupkg") + - name: Validate NuGet Packages + run: | + foreach ($file in Get-ChildItem "${{ env.NuGetDirectory }}" -Recurse -Include *.nupkg) { + meziantou.validate-nuget-package $file + } run_test: + runs-on: ubuntu-latest defaults: @@ -93,43 +96,40 @@ jobs: working-directory: ./src/Jitter2 steps: - - uses: actions/checkout@v4 - - name: Setup .NET - uses: actions/setup-dotnet@v4 - with: - dotnet-version: 9.0.x + - uses: actions/checkout@v4 + - name: Setup .NET + uses: actions/setup-dotnet@v4 + with: + dotnet-version: 9.0.x + + - name: Run Tests - Single Precision + run: dotnet test --configuration Release - - name: Run tests - run: dotnet test --configuration Release + - name: Run Tests - Double Precision + run: dotnet test --configuration Release -p:DoublePrecision=true deploy: - # Publish only when creating a GitHub Release - # https://docs.github.com/en/repositories/releasing-projects-on-github/managing-releases-in-a-repository - # You can update this logic if you want to manage releases differently + if: github.event_name == 'release' runs-on: ubuntu-latest - needs: [ validate_nuget, run_test ] + needs: [validate_nuget, run_test] steps: - # Download the NuGet package created in the previous job - - uses: actions/download-artifact@v4 - with: - name: nuget - path: ${{ env.NuGetDirectory }} - - # Install the .NET SDK indicated in the global.json file - - name: Setup .NET Core + - name: Setup .NET uses: actions/setup-dotnet@v4 with: dotnet-version: 9.0.x - # Publish all NuGet packages to NuGet.org - # Use --skip-duplicate to prevent errors if a package with the same version already exists. - # If you retry a failed workflow, already published packages will be skipped without error. - - name: Publish NuGet package + - name: Download NuGet Packages + uses: actions/download-artifact@v4 + with: + name: nuget + path: ${{ env.NuGetDirectory }} + + - name: Publish NuGet Packages run: | dotnet nuget add source --username notgiven688 --password ${{ secrets.GITHUB_TOKEN }} --store-password-in-clear-text --name github "https://nuget.pkg.github.com/notgiven688/index.json" - foreach($file in (Get-ChildItem "${{ env.NuGetDirectory }}" -Recurse -Include *.nupkg)) { + foreach ($file in Get-ChildItem "${{ env.NuGetDirectory }}" -Recurse -Include *.nupkg) { dotnet nuget push $file --api-key "${{ secrets.NUGET_APIKEY }}" --source https://api.nuget.org/v3/index.json --skip-duplicate dotnet nuget push $file --source "github" --skip-duplicate } diff --git a/src/.gitignore b/src/.gitignore index 558b013d..d3949636 100644 --- a/src/.gitignore +++ b/src/.gitignore @@ -1,4 +1,5 @@ .vscode +.vs imgui.ini [Bb]in/ [Oo]bj/ diff --git a/src/Jitter2/Collision/CollisionFilter/INarrowPhaseFilter.cs b/src/Jitter2/Collision/CollisionFilter/INarrowPhaseFilter.cs index 8a7102bb..2b5ea587 100644 --- a/src/Jitter2/Collision/CollisionFilter/INarrowPhaseFilter.cs +++ b/src/Jitter2/Collision/CollisionFilter/INarrowPhaseFilter.cs @@ -39,5 +39,5 @@ public interface INarrowPhaseFilter /// False if the collision should be filtered out, true otherwise. bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, ref JVector pointA, ref JVector pointB, - ref JVector normal, ref float penetration); + ref JVector normal, ref Real penetration); } \ No newline at end of file diff --git a/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs b/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs index 931cb356..caa31db5 100644 --- a/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs +++ b/src/Jitter2/Collision/CollisionFilter/TriangleEdgeCollisionFilter.cs @@ -41,14 +41,14 @@ public class TriangleEdgeCollisionFilter : INarrowPhaseFilter /// A tweakable parameter. Collision points that are closer than this value to a triangle edge /// are considered as edge collisions and might be modified or discarded entirely. /// - public float EdgeThreshold { get; set; } = 0.05f; + public Real EdgeThreshold { get; set; } = (Real)0.05; - private float cosAT = 0.99f; + private Real cosAT = (Real)0.99; /// /// A tweakable parameter. /// - public float ProjectionThreshold { get; set; } = 0.5f; + public Real ProjectionThreshold { get; set; } = (Real)0.5; /// /// A tweakable parameter that defines the threshold to determine when two normals @@ -56,13 +56,13 @@ public class TriangleEdgeCollisionFilter : INarrowPhaseFilter /// public JAngle AngleThreshold { - get => JAngle.FromRadiant(MathF.Acos(cosAT)); - set => cosAT = MathF.Cos(value.Radiant); + get => JAngle.FromRadiant(MathR.Acos(cosAT)); + set => cosAT = MathR.Cos(value.Radiant); } /// public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, - ref JVector pointA, ref JVector pointB, ref JVector normal, ref float penetration) + ref JVector pointA, ref JVector pointB, ref JVector normal, ref Real penetration) { TriangleShape? ts1 = shapeA as TriangleShape; TriangleShape? ts2 = shapeB as TriangleShape; @@ -100,22 +100,22 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, tshape.GetWorldVertices(out JVector a, out JVector b, out JVector c); JVector n, pma; - float d0, d1, d2; + Real d0, d1, d2; // TODO: this can be optimized n = b - a; pma = collP - a; - d0 = (pma - JVector.Dot(pma, n) * n * (1.0f / n.LengthSquared())).LengthSquared(); + d0 = (pma - JVector.Dot(pma, n) * n * ((Real)1.0 / n.LengthSquared())).LengthSquared(); n = c - a; pma = collP - a; - d1 = (pma - JVector.Dot(pma, n) * n * (1.0f / n.LengthSquared())).LengthSquared(); + d1 = (pma - JVector.Dot(pma, n) * n * ((Real)1.0 / n.LengthSquared())).LengthSquared(); n = c - b; pma = collP - b; - d2 = (pma - JVector.Dot(pma, n) * n * (1.0f / n.LengthSquared())).LengthSquared(); + d2 = (pma - JVector.Dot(pma, n) * n * ((Real)1.0 / n.LengthSquared())).LengthSquared(); - if (MathF.Min(MathF.Min(d0, d1), d2) > EdgeThreshold) return true; + if (MathR.Min(MathR.Min(d0, d1), d2) > EdgeThreshold) return true; JVector nnormal; @@ -153,8 +153,8 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, { // tnormal and nnormal are the same // -------------------------------- - float f5 = JVector.Dot(normal, nnormal); - float f6 = JVector.Dot(normal, tnormal); + Real f5 = JVector.Dot(normal, nnormal); + Real f6 = JVector.Dot(normal, tnormal); if (f5 > f6) { @@ -211,16 +211,16 @@ public bool Filter(RigidBodyShape shapeA, RigidBodyShape shapeB, // \ // \ // \ tnormal - float f1 = proj % nnormal * cross; - float f2 = proj % tnormal * cross; + Real f1 = proj % nnormal * cross; + Real f2 = proj % tnormal * cross; - bool between = f1 * f2 <= 0.0f; + bool between = f1 * f2 <= (Real)0.0; if (!between) { // not in-between, snap normal - float f3 = JVector.Dot(normal, nnormal); - float f4 = JVector.Dot(normal, tnormal); + Real f3 = JVector.Dot(normal, nnormal); + Real f4 = JVector.Dot(normal, tnormal); if (f3 > f4) { diff --git a/src/Jitter2/Collision/CollisionHelper.cs b/src/Jitter2/Collision/CollisionHelper.cs index 8eacea83..a830a906 100644 --- a/src/Jitter2/Collision/CollisionHelper.cs +++ b/src/Jitter2/Collision/CollisionHelper.cs @@ -47,17 +47,17 @@ public static bool ProjectedPointOnTriangle(in JVector a, in JVector b, in JVect JVector v = a - c; JVector normal = u % v; - float t = normal.LengthSquared(); + Real t = normal.LengthSquared(); JVector at = a - point; JVector.Cross(u, at, out JVector tmp); - float gamma = JVector.Dot(tmp, normal) / t; + Real gamma = JVector.Dot(tmp, normal) / t; JVector.Cross(at, v, out tmp); - float beta = JVector.Dot(tmp, normal) / t; - float alpha = 1.0f - gamma - beta; + Real beta = JVector.Dot(tmp, normal) / t; + Real alpha = (Real)1.0 - gamma - beta; - return alpha > 0.0f && beta > 0.0f && gamma > 0.0f; + return alpha > (Real)0.0 && beta > (Real)0.0 && gamma > (Real)0.0; } /// @@ -73,20 +73,20 @@ public static bool ProjectedPointOnTriangle(in JVector a, in JVector b, in JVect /// Returns true if the ray intersects with the triangle, otherwise returns false. public static bool RayTriangle(in JVector a, in JVector b, in JVector c, in JVector rayStart, in JVector rayDir, - out float lambda, out JVector normal) + out Real lambda, out JVector normal) { JVector u = b - a; JVector v = c - a; normal = v % u; - float t = normal.LengthSquared(); + Real t = normal.LengthSquared(); // triangle is expected to span an area - Debug.Assert(t > 1e-06f); + Debug.Assert(t > (Real)1e-06); - float denom = JVector.Dot(rayDir, normal); + Real denom = JVector.Dot(rayDir, normal); - if (Math.Abs(denom) < 1e-06f) + if (Math.Abs(denom) < 1e-06) { // triangle and ray are parallel lambda = 0; @@ -95,7 +95,7 @@ public static bool RayTriangle(in JVector a, in JVector b, in JVector c, } lambda = JVector.Dot(a - rayStart, normal); - if (lambda > 0.0f) return false; + if (lambda > (Real)0.0) return false; lambda /= denom; // point where the ray intersects the plane of the triangle. @@ -103,10 +103,10 @@ public static bool RayTriangle(in JVector a, in JVector b, in JVector c, JVector at = a - hitPoint; JVector.Cross(u, at, out JVector tmp); - float gamma = JVector.Dot(tmp, normal) / t; + Real gamma = JVector.Dot(tmp, normal) / t; JVector.Cross(at, v, out tmp); - float beta = JVector.Dot(tmp, normal) / t; - float alpha = 1.0f - gamma - beta; + Real beta = JVector.Dot(tmp, normal) / t; + Real alpha = (Real)1.0 - gamma - beta; return alpha > 0 && beta > 0 && gamma > 0; } diff --git a/src/Jitter2/Collision/DynamicTree.RayCast.cs b/src/Jitter2/Collision/DynamicTree.RayCast.cs index 42629cbe..40aad593 100644 --- a/src/Jitter2/Collision/DynamicTree.RayCast.cs +++ b/src/Jitter2/Collision/DynamicTree.RayCast.cs @@ -35,7 +35,7 @@ public partial class DynamicTree public struct RayCastResult { public IDynamicTreeProxy Entity; - public float Lambda; + public Real Lambda; public JVector Normal; } @@ -59,7 +59,7 @@ private struct Ray public RayCastFilterPost? FilterPost; public RayCastFilterPre? FilterPre; - public float Lambda; + public Real Lambda; public Ray(in JVector origin, in JVector direction) { @@ -67,7 +67,7 @@ public Ray(in JVector origin, in JVector direction) Direction = direction; FilterPost = null; FilterPre = null; - Lambda = float.MaxValue; + Lambda = Real.MaxValue; } } @@ -83,7 +83,7 @@ public Ray(in JVector origin, in JVector direction) /// Distance from the origin to the ray hit point in units of the ray's direction. /// True if the ray hits, false otherwise. public bool RayCast(JVector origin, JVector direction, RayCastFilterPre? pre, RayCastFilterPost? post, - out IDynamicTreeProxy? proxy, out JVector normal, out float lambda) + out IDynamicTreeProxy? proxy, out JVector normal, out Real lambda) { Ray ray = new(origin, direction) { @@ -97,10 +97,10 @@ public bool RayCast(JVector origin, JVector direction, RayCastFilterPre? pre, Ra return hit; } - /// + /// /// Maximum lambda of the ray's length to consider for intersections. - public bool RayCast(JVector origin, JVector direction, float maxLambda, RayCastFilterPre? pre, RayCastFilterPost? post, - out IDynamicTreeProxy? proxy, out JVector normal, out float lambda) + public bool RayCast(JVector origin, JVector direction, Real maxLambda, RayCastFilterPre? pre, RayCastFilterPost? post, + out IDynamicTreeProxy? proxy, out JVector normal, out Real lambda) { Ray ray = new(origin, direction) { @@ -161,8 +161,8 @@ private bool QueryRay(in Ray ray, out RayCastResult result) ref Node lNode = ref Nodes[node.Left]; ref Node rNode = ref Nodes[node.Right]; - bool lRes = lNode.ExpandedBox.RayIntersect(ray.Origin, ray.Direction, out float lEnter); - bool rRes = rNode.ExpandedBox.RayIntersect(ray.Origin, ray.Direction, out float rEnter); + bool lRes = lNode.ExpandedBox.RayIntersect(ray.Origin, ray.Direction, out Real lEnter); + bool rRes = rNode.ExpandedBox.RayIntersect(ray.Origin, ray.Direction, out Real rEnter); if (lEnter > result.Lambda) lRes = false; if (rEnter > result.Lambda) rRes = false; diff --git a/src/Jitter2/Collision/DynamicTree.cs b/src/Jitter2/Collision/DynamicTree.cs index bd85f6d5..17d8b63f 100644 --- a/src/Jitter2/Collision/DynamicTree.cs +++ b/src/Jitter2/Collision/DynamicTree.cs @@ -38,7 +38,7 @@ public interface IUpdatableBoundingBox /// /// Updates the bounding box. /// - public void UpdateWorldBoundingBox(float dt); + public void UpdateWorldBoundingBox(Real dt); } /// @@ -65,7 +65,7 @@ public interface IRayCastable /// /// true if the ray intersects with the object; otherwise, false. /// - public bool RayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda); + public bool RayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda); } /// @@ -92,13 +92,13 @@ public partial class DynamicTree /// Specifies the factor by which the bounding box in the dynamic tree structure is expanded. The expansion is calculated as /// * ExpandFactor * alpha, where alpha is a pseudo-random number in the range [1,2]. /// - public const float ExpandFactor = 0.1f; + public const Real ExpandFactor = (Real)0.1; /// /// Specifies a small additional expansion of the bounding box in the AABB tree structure to prevent /// the creation of bounding boxes with zero volume. /// - public const float ExpandEps = 0.01f; + public const Real ExpandEps = (Real)0.01; /// /// Represents a node in the AABB tree. @@ -181,7 +181,7 @@ public enum Timings /// Updates all entities that are marked as active in the active list. /// /// A boolean indicating whether to perform a multi-threaded update. - public void Update(bool multiThread, float dt) + public void Update(bool multiThread, Real dt) { long time = Stopwatch.GetTimestamp(); double invFrequency = 1.0d / Stopwatch.Frequency; @@ -259,7 +259,7 @@ void SetTime(Timings type) } } - private float step_dt; + private Real step_dt; private void UpdateBoundingBoxesCallback(Parallel.Batch batch) { @@ -473,15 +473,15 @@ public void Query(T hits, in JBBox box) where T : ICollection /// The number of times to iterate over all proxies in the tree. Must be greater than zero. /// The chance of a proxy to be removed and re-added to the tree for each sweep. Must be between 0 and 1. - public void Optimize(int sweeps = 100, float chance = 0.01f) + public void Optimize(int sweeps = 100, Real chance = (Real)0.01) { optimizeRandom ??= new Random(0); Optimize(optimizeRandom, sweeps, chance); } - /// + /// /// Provide an instance of a random class. - public void Optimize(Random random, int sweeps, float chance) + public void Optimize(Random random, int sweeps, Real chance) { if (sweeps <= 0) throw new ArgumentOutOfRangeException(nameof(sweeps), "Sweeps must be greater than zero."); if (chance < 0 || chance > 1) throw new ArgumentOutOfRangeException(nameof(chance), "Chance must be between 0 and 1."); @@ -623,13 +623,13 @@ private void ScanForOverlaps(int fraction, bool add) private static void ExpandBoundingBox(ref JBBox box, in JVector direction) { - if (direction.X < 0.0f) box.Min.X += direction.X; + if (direction.X < (Real)0.0) box.Min.X += direction.X; else box.Max.X += direction.X; - if (direction.Y < 0.0f) box.Min.Y += direction.Y; + if (direction.Y < (Real)0.0) box.Min.Y += direction.Y; else box.Max.Y += direction.Y; - if (direction.Z < 0.0f) box.Min.Z += direction.Z; + if (direction.Z < (Real)0.0) box.Min.Z += direction.Z; else box.Max.Z += direction.Z; box.Min.X -= ExpandEps; @@ -641,7 +641,7 @@ private static void ExpandBoundingBox(ref JBBox box, in JVector direction) box.Max.Z += ExpandEps; } - private static float GenerateRandom(ulong seed) + private static Real GenerateRandom(ulong seed) { const uint a = 21_687_443; const uint b = 35_253_893; @@ -651,7 +651,7 @@ private static float GenerateRandom(ulong seed) seed ^= seed << 5; uint randomBits = (uint)seed * a + b; - return MathF.Abs((float)randomBits / uint.MaxValue); + return MathR.Abs((Real)randomBits / uint.MaxValue); } private void InternalAddProxy(IDynamicTreeProxy proxy) @@ -659,9 +659,9 @@ private void InternalAddProxy(IDynamicTreeProxy proxy) JBBox box = proxy.WorldBoundingBox; int index = AllocateNode(); - float pseudoRandomExt = GenerateRandom((ulong)index); + Real pseudoRandomExt = GenerateRandom((ulong)index); - ExpandBoundingBox(ref box, proxy.Velocity * ExpandFactor * (1.0f + pseudoRandomExt)); + ExpandBoundingBox(ref box, proxy.Velocity * ExpandFactor * ((Real)1.0 + pseudoRandomExt)); Nodes[index].Proxy = proxy; Nodes[index].Height = 1; diff --git a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs index 0789c37d..1e8de1b5 100644 --- a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs +++ b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs @@ -49,8 +49,8 @@ public struct Triangle public JVector Normal; public JVector ClosestToOrigin; - public float NormalSq; - public float ClosestToOriginSq; + public Real NormalSq; + public Real ClosestToOriginSq; } private struct Edge @@ -70,7 +70,7 @@ public static bool Equals(in Edge a, in Edge b) } } - private const float NumericEpsilon = 1e-16f; + private const Real NumericEpsilon = (Real)1e-16; // (*) Euler-characteristic: V (vertices) - E (edges) + F (faces) = 2 // We have triangles T instead of faces: F = T @@ -127,60 +127,60 @@ private bool CalcBarycentric(in Triangle tri, out JVector result) JVector ab = b - a; JVector ac = c - a; - float d1 = JVector.Dot(ab, a); - float d2 = JVector.Dot(ac, a); + Real d1 = JVector.Dot(ab, a); + Real d2 = JVector.Dot(ac, a); - if (d1 > 0.0f && d2 > 0.0f) + if (d1 > (Real)0.0 && d2 > (Real)0.0) { - result = new JVector(1, 0, 0); + result = new JVector((Real)1.0, (Real)0.0, (Real)0.0); return true; } - float d3 = JVector.Dot(ab, b); - float d4 = JVector.Dot(ac, b); - if (d3 < 0.0f && d4 > d3) + Real d3 = JVector.Dot(ab, b); + Real d4 = JVector.Dot(ac, b); + if (d3 < (Real)0.0 && d4 > d3) { result = new JVector(0, 1, 0); return true; } - float vc = d1 * d4 - d3 * d2; - if (vc <= 0.0f && d1 < 0.0f && d3 > 0.0f) + Real vc = d1 * d4 - d3 * d2; + if (vc <= (Real)0.0 && d1 < (Real)0.0 && d3 > (Real)0.0) { - float v = d1 / (d1 - d3); - result = new JVector(1.0f - v, v, 0); + Real v = d1 / (d1 - d3); + result = new JVector((Real)1.0 - v, v, 0); return true; } - float d5 = JVector.Dot(ab, c); - float d6 = JVector.Dot(ac, c); - if (d6 < 0.0f && d5 > d6) + Real d5 = JVector.Dot(ab, c); + Real d6 = JVector.Dot(ac, c); + if (d6 < (Real)0.0 && d5 > d6) { result = new JVector(0, 0, 1); return true; } - float vb = d5 * d2 - d1 * d6; - if (vb <= 0.0f && d2 < 0.0f && d6 > 0.0f) + Real vb = d5 * d2 - d1 * d6; + if (vb <= (Real)0.0 && d2 < (Real)0.0 && d6 > (Real)0.0) { - float w = d2 / (d2 - d6); - result = new JVector(1.0f - w, 0, w); + Real w = d2 / (d2 - d6); + result = new JVector((Real)1.0 - w, 0, w); return true; } - float va = d3 * d6 - d5 * d4; - if (va <= 0.0f && (d4 - d3) < 0.0f && (d5 - d6) < 0.0f) + Real va = d3 * d6 - d5 * d4; + if (va <= (Real)0.0 && (d4 - d3) < (Real)0.0 && (d5 - d6) < (Real)0.0) { - float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - result = new JVector(0, 1.0f - w, w); + Real w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + result = new JVector(0, (Real)1.0 - w, w); return true; } - float d = 1.0f / (va + vb + vc); - float vf = vb * d; - float wf = vc * d; + Real d = (Real)1.0 / (va + vb + vc); + Real vf = vb * d; + Real wf = vc * d; - result = new JVector(1.0f - vf - wf, vf, wf); + result = new JVector((Real)1.0 - vf - wf, vf, wf); return false; } @@ -213,7 +213,7 @@ private bool CreateTriangle(short a, short b, short c) } // do we need to flip the triangle? (the origin of the md has to be enclosed) - float delta = JVector.Dot(triangle.Normal, vertices[a].V - center); + Real delta = JVector.Dot(triangle.Normal, vertices[a].V - center); if (delta < 0) { @@ -222,7 +222,7 @@ private bool CreateTriangle(short a, short b, short c) } delta = JVector.Dot(triangle.Normal, vertices[a].V); - triangle.FacingOrigin = delta >= 0.0f; + triangle.FacingOrigin = delta >= (Real)0.0; if (CalcBarycentric(triangle, out JVector bc)) { @@ -248,7 +248,7 @@ private bool CreateTriangle(short a, short b, short c) public ref Triangle GetClosestTriangle() { int closestIndex = -1; - float currentMin = float.MaxValue; + Real currentMin = Real.MaxValue; // We can skip the test for enclosed origin if the origin was // already enclosed once. @@ -279,7 +279,7 @@ public void InitTetrahedron() vPointer = 4; tPointer = 0; - center = 0.25f * (vertices[0].V + vertices[1].V + vertices[2].V + vertices[3].V); + center = (Real)0.25 * (vertices[0].V + vertices[1].V + vertices[2].V + vertices[3].V); CreateTriangle(0, 2, 1); CreateTriangle(0, 1, 3); @@ -297,11 +297,11 @@ public void InitTetrahedron(in JVector point) tPointer = 0; center = point; - const float scale = 1e-2f; // minkowski sums not allowed to be thinner - vertices[0] = new Vertex(center + scale * new JVector(MathF.Sqrt(8.0f / 9.0f), 0.0f, -1.0f / 3.0f)); - vertices[1] = new Vertex(center + scale * new JVector(-MathF.Sqrt(2.0f / 9.0f), MathF.Sqrt(2.0f / 3.0f), -1.0f / 3.0f)); - vertices[2] = new Vertex(center + scale * new JVector(-MathF.Sqrt(2.0f / 9.0f), -MathF.Sqrt(2.0f / 3.0f), -1.0f / 3.0f)); - vertices[3] = new Vertex(center + scale * new JVector(0.0f, 0.0f, 1.0f)); + const Real scale = (Real)1e-2; // minkowski sums not allowed to be thinner + vertices[0] = new Vertex(center + scale * new JVector(MathR.Sqrt((Real)(8.0 / 9.0)), (Real)0.0, -(Real)(1.0 / 3.0))); + vertices[1] = new Vertex(center + scale * new JVector(-MathR.Sqrt((Real)(2.0 / 9.0)), MathR.Sqrt((Real)(2.0 / 3.0)), -(Real)(1.0 / 3.0))); + vertices[2] = new Vertex(center + scale * new JVector(-MathR.Sqrt((Real)(2.0 / 9.0)), -MathR.Sqrt((Real)(2.0 / 3.0)), -(Real)(1.0 / 3.0))); + vertices[3] = new Vertex(center + scale * new JVector((Real)0.0, (Real)0.0, (Real)1.0)); CreateTriangle(2, 0, 1); CreateTriangle(1, 0, 3); diff --git a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs index 142a5f5c..dd42a584 100644 --- a/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs +++ b/src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs @@ -35,7 +35,7 @@ namespace Jitter2.Collision; /// public static class NarrowPhase { - private const float NumericEpsilon = 1e-16f; + private const Real NumericEpsilon = (Real)1e-16; private struct Solver { @@ -43,7 +43,7 @@ private struct Solver public bool PointTest(in ISupportMappable supportA, in JVector origin) { - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 34; JVector x = origin; @@ -56,16 +56,16 @@ public bool PointTest(in ISupportMappable supportA, in JVector origin) int maxIter = MaxIter; - float distSq = v.LengthSquared(); + Real distSq = v.LengthSquared(); while (distSq > CollideEpsilon * CollideEpsilon && maxIter-- != 0) { supportA.SupportMap(v, out JVector p); JVector.Subtract(x, p, out JVector w); - float vw = JVector.Dot(v, w); + Real vw = JVector.Dot(v, w); - if (vw >= 0.0f) + if (vw >= (Real)0.0) { return false; } @@ -83,13 +83,13 @@ public bool PointTest(in ISupportMappable supportA, in JVector origin) return true; } - public bool RayCast(in ISupportMappable supportA, in JVector origin, in JVector direction, out float lambda, out JVector normal) + public bool RayCast(in ISupportMappable supportA, in JVector origin, in JVector direction, out Real lambda, out JVector normal) { - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 34; normal = JVector.Zero; - lambda = 0.0f; + lambda = (Real)0.0; JVector r = direction; JVector x = origin; @@ -102,7 +102,7 @@ public bool RayCast(in ISupportMappable supportA, in JVector origin, in JVector int maxIter = MaxIter; - float distSq = v.LengthSquared(); + Real distSq = v.LengthSquared(); while (distSq > CollideEpsilon * CollideEpsilon && maxIter-- != 0) { @@ -110,15 +110,15 @@ public bool RayCast(in ISupportMappable supportA, in JVector origin, in JVector JVector.Subtract(x, p, out JVector w); - float VdotW = JVector.Dot(v, w); + Real VdotW = JVector.Dot(v, w); - if (VdotW > 0.0f) + if (VdotW > (Real)0.0) { - float VdotR = JVector.Dot(v, r); + Real VdotR = JVector.Dot(v, r); if (VdotR >= -NumericEpsilon) { - lambda = float.PositiveInfinity; + lambda = Real.PositiveInfinity; return false; } @@ -140,20 +140,20 @@ public bool RayCast(in ISupportMappable supportA, in JVector origin, in JVector converged: - float nlen2 = normal.LengthSquared(); + Real nlen2 = normal.LengthSquared(); if (nlen2 > NumericEpsilon) { - normal *= 1.0f / MathF.Sqrt(nlen2); + normal *= (Real)1.0 / MathR.Sqrt(nlen2); } return true; } public bool Sweep(ref MinkowskiDifference mkd, in JVector sweep, - out JVector p1, out JVector p2, out JVector normal, out float lambda) + out JVector p1, out JVector p2, out JVector normal, out Real lambda) { - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 34; Unsafe.SkipInit(out SimplexSolverAB simplexSolver); @@ -163,7 +163,7 @@ public bool Sweep(ref MinkowskiDifference mkd, in JVector sweep, JVector posB = mkd.PositionB; - lambda = 0.0f; + lambda = (Real)0.0; p1 = p2 = JVector.Zero; @@ -174,22 +174,22 @@ public bool Sweep(ref MinkowskiDifference mkd, in JVector sweep, int iter = MaxIter; - float distSq = float.MaxValue; + Real distSq = Real.MaxValue; while ((distSq > CollideEpsilon * CollideEpsilon) && (iter-- != 0)) { mkd.Support(v, out Vertex vertex); var w = vertex.V; - float VdotW = -JVector.Dot(v, w); + Real VdotW = -JVector.Dot(v, w); - if (VdotW > 0.0f) + if (VdotW > (Real)0.0) { - float VdotR = JVector.Dot(v, r); + Real VdotR = JVector.Dot(v, r); - if (VdotR >= -1e-12f) + if (VdotR >= -(Real)1e-12) { - lambda = float.PositiveInfinity; + lambda = Real.PositiveInfinity; return false; } @@ -213,19 +213,19 @@ public bool Sweep(ref MinkowskiDifference mkd, in JVector sweep, simplexSolver.GetClosest(out p1, out p2); - float nlen2 = normal.LengthSquared(); + Real nlen2 = normal.LengthSquared(); if (nlen2 > NumericEpsilon) { - normal *= 1.0f / MathF.Sqrt(nlen2); + normal *= (Real)1.0 / MathR.Sqrt(nlen2); } return true; } - private bool SolveMPREPA(in MinkowskiDifference mkd, ref JVector point1, ref JVector point2, ref JVector normal, ref float penetration) + private bool SolveMPREPA(in MinkowskiDifference mkd, ref JVector point1, ref JVector point2, ref JVector normal, ref Real penetration) { - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 85; convexPolytope.InitTetrahedron(); @@ -239,7 +239,7 @@ private bool SolveMPREPA(in MinkowskiDifference mkd, ref JVector point1, ref JVe ctri = convexPolytope.GetClosestTriangle(); JVector searchDir = ctri.ClosestToOrigin; - float searchDirSq = ctri.ClosestToOriginSq; + Real searchDirSq = ctri.ClosestToOriginSq; if (ctri.ClosestToOriginSq < NumericEpsilon) { @@ -250,7 +250,7 @@ private bool SolveMPREPA(in MinkowskiDifference mkd, ref JVector point1, ref JVe mkd.Support(searchDir, out Vertex vertex); // compare with the corresponding code in SolveGJKEPA. - float deltaDist = JVector.Dot(ctri.ClosestToOrigin - vertex.V, searchDir); + Real deltaDist = JVector.Dot(ctri.ClosestToOrigin - vertex.V, searchDir); if (deltaDist * deltaDist <= CollideEpsilon * CollideEpsilon * searchDirSq) { @@ -271,14 +271,14 @@ private bool SolveMPREPA(in MinkowskiDifference mkd, ref JVector point1, ref JVe convexPolytope.CalculatePoints(ctri, out point1, out point2); - normal = ctri.Normal * (1.0f / MathF.Sqrt(ctri.NormalSq)); - penetration = MathF.Sqrt(ctri.ClosestToOriginSq); + normal = ctri.Normal * ((Real)1.0 / MathR.Sqrt(ctri.NormalSq)); + penetration = MathR.Sqrt(ctri.ClosestToOriginSq); return true; } public bool SolveMPR(in MinkowskiDifference mkd, - out JVector pointA, out JVector pointB, out JVector normal, out float penetration) + out JVector pointA, out JVector pointB, out JVector normal, out Real penetration) { /* XenoCollide is available under the zlib license: @@ -301,12 +301,12 @@ would be appreciated but is not required. not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 34; // If MPR reports a penetration deeper than this value we do not trust // MPR to have found the global minimum and perform an EPA run. - const float EPAPenetrationThreshold = 0.02f; + const Real EPAPenetrationThreshold = (Real)0.02; Unsafe.SkipInit(out Vertex v0); Unsafe.SkipInit(out Vertex v1); @@ -318,7 +318,7 @@ 3. This notice may not be removed or altered from any source distribution. Unsafe.SkipInit(out JVector temp2); Unsafe.SkipInit(out JVector temp3); - penetration = 0.0f; + penetration = (Real)0.0; mkd.GetCenter(out v0); @@ -327,7 +327,7 @@ 3. This notice may not be removed or altered from any source distribution. Math.Abs(v0.V.Z) < NumericEpsilon) { // any direction is fine - v0.V.X = 1e-05f; + v0.V.X = (Real)1e-05; } JVector.Negate(v0.V, out normal); @@ -337,7 +337,7 @@ 3. This notice may not be removed or altered from any source distribution. pointA = v1.A; pointB = v1.B; - if (JVector.Dot(v1.V, normal) <= 0.0f) return false; + if (JVector.Dot(v1.V, normal) <= (Real)0.0) return false; JVector.Cross(v1.V, v0.V, out normal); if (normal.LengthSquared() < NumericEpsilon) @@ -354,17 +354,17 @@ 3. This notice may not be removed or altered from any source distribution. mkd.Support(normal, out v2); - if (JVector.Dot(v2.V, normal) <= 0.0f) return false; + if (JVector.Dot(v2.V, normal) <= (Real)0.0) return false; // Determine whether origin is on + or - side of plane (v1.V,v0.V,v2.V) JVector.Subtract(v1.V, v0.V, out temp1); JVector.Subtract(v2.V, v0.V, out temp2); JVector.Cross(temp1, temp2, out normal); - float dist = JVector.Dot(normal, v0.V); + Real dist = JVector.Dot(normal, v0.V); // If the origin is on the - side of the plane, reverse the direction of the plane - if (dist > 0.0f) + if (dist > (Real)0.0) { JVector.Swap(ref v1.V, ref v2.V); JVector.Swap(ref v1.A, ref v2.A); @@ -385,14 +385,14 @@ 3. This notice may not be removed or altered from any source distribution. mkd.Support(normal, out v3); - if (JVector.Dot(v3.V, normal) <= 0.0f) + if (JVector.Dot(v3.V, normal) <= (Real)0.0) { return false; } // If origin is outside (v1.V,v0.V,v3.V), then eliminate v2.V and loop JVector.Cross(v1.V, v3.V, out temp1); - if (JVector.Dot(temp1, v0.V) < 0.0f) + if (JVector.Dot(temp1, v0.V) < (Real)0.0) { v2 = v3; JVector.Subtract(v1.V, v0.V, out temp1); @@ -403,7 +403,7 @@ 3. This notice may not be removed or altered from any source distribution. // If origin is outside (v3.V,v0.V,v2.V), then eliminate v1.V and loop JVector.Cross(v3.V, v2.V, out temp1); - if (JVector.Dot(temp1, v0.V) < 0.0f) + if (JVector.Dot(temp1, v0.V) < (Real)0.0) { v1 = v3; JVector.Subtract(v3.V, v0.V, out temp1); @@ -427,7 +427,7 @@ 3. This notice may not be removed or altered from any source distribution. JVector.Cross(temp1, temp2, out normal); // normal.Normalize(); - float normalSq = normal.LengthSquared(); + Real normalSq = normal.LengthSquared(); // Can this happen??? Can it be handled more cleanly? if (normalSq < NumericEpsilon) @@ -441,7 +441,7 @@ 3. This notice may not be removed or altered from any source distribution. if (!hit) { // Compute distance from origin to wedge face - float d = JVector.Dot(normal, v1.V); + Real d = JVector.Dot(normal, v1.V); // If the origin is inside the wedge, we have a hit hit = d >= 0; } @@ -449,17 +449,17 @@ 3. This notice may not be removed or altered from any source distribution. mkd.Support(normal, out v4); JVector.Subtract(v4.V, v3.V, out temp3); - float delta = JVector.Dot(temp3, normal); + Real delta = JVector.Dot(temp3, normal); penetration = JVector.Dot(v4.V, normal); // If the boundary is thin enough or the origin is outside the support plane for the newly discovered // vertex, then we can terminate - if (delta * delta <= CollideEpsilon * CollideEpsilon * normalSq || penetration <= 0.0f || + if (delta * delta <= CollideEpsilon * CollideEpsilon * normalSq || penetration <= (Real)0.0 || phase2 > MaxIter) { if (hit) { - float invnormal = 1.0f / (float)Math.Sqrt(normalSq); + Real invnormal = (Real)1.0 / MathR.Sqrt(normalSq); penetration *= invnormal; @@ -479,10 +479,10 @@ 3. This notice may not be removed or altered from any source distribution. // Compute the barycentric coordinates of the origin JVector.Cross(v1.V, temp1, out temp3); - float gamma = JVector.Dot(temp3, normal) * invnormal; + Real gamma = JVector.Dot(temp3, normal) * invnormal; JVector.Cross(temp2, v1.V, out temp3); - float beta = JVector.Dot(temp3, normal) * invnormal; - float alpha = 1.0f - gamma - beta; + Real beta = JVector.Dot(temp3, normal) * invnormal; + Real alpha = (Real)1.0 - gamma - beta; pointA = alpha * v1.A + beta * v2.A + gamma * v3.A; pointB = alpha * v1.B + beta * v2.B + gamma * v3.B; @@ -493,13 +493,13 @@ 3. This notice may not be removed or altered from any source distribution. // Compute the tetrahedron dividing face (v4.V,v0.V,v3.V) JVector.Cross(v4.V, v0.V, out temp1); - float dot = JVector.Dot(temp1, v1.V); + Real dot = JVector.Dot(temp1, v1.V); - if (dot >= 0.0f) + if (dot >= (Real)0.0) { dot = JVector.Dot(temp1, v2.V); - if (dot >= 0.0f) + if (dot >= (Real)0.0) { v1 = v4; // Inside d1 & inside d2 -> eliminate v1.V } @@ -512,7 +512,7 @@ 3. This notice may not be removed or altered from any source distribution. { dot = JVector.Dot(temp1, v3.V); - if (dot >= 0.0f) + if (dot >= (Real)0.0) { v2 = v4; // Outside d1 & inside d3 -> eliminate v2.V } @@ -525,9 +525,9 @@ 3. This notice may not be removed or altered from any source distribution. } public bool Distance(in MinkowskiDifference mkd, - out JVector point1, out JVector point2, out float distance) + out JVector point1, out JVector point2, out Real distance) { - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 34; Unsafe.SkipInit(out SimplexSolverAB simplexSolver); @@ -537,7 +537,7 @@ public bool Distance(in MinkowskiDifference mkd, mkd.GetCenter(out var center); JVector v = center.V; - float distSq = v.LengthSquared(); + Real distSq = v.LengthSquared(); while (maxIter-- != 0) { @@ -545,7 +545,7 @@ public bool Distance(in MinkowskiDifference mkd, distSq = v.LengthSquared(); - float deltaDist = JVector.Dot(v - w.V, v); + Real deltaDist = JVector.Dot(v - w.V, v); if (deltaDist * deltaDist < CollideEpsilon * CollideEpsilon * distSq) { break; @@ -554,13 +554,13 @@ public bool Distance(in MinkowskiDifference mkd, if (distSq < CollideEpsilon * CollideEpsilon || !simplexSolver.AddVertex(w, out v)) { - distance = 0.0f; + distance = (Real)0.0; point1 = point2 = JVector.Zero; return false; } } - distance = MathF.Sqrt(distSq); + distance = MathR.Sqrt(distSq); simplexSolver.GetClosest(out point1, out point2); return true; @@ -568,7 +568,7 @@ public bool Distance(in MinkowskiDifference mkd, public bool Overlap(in MinkowskiDifference mkd) { - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 34; Unsafe.SkipInit(out SimplexSolverAB simplexSolver); @@ -578,13 +578,13 @@ public bool Overlap(in MinkowskiDifference mkd) mkd.GetCenter(out var center); JVector v = center.V; - float distSq = v.LengthSquared(); + Real distSq = v.LengthSquared(); while (distSq > CollideEpsilon * CollideEpsilon && maxIter-- != 0) { mkd.Support(-v, out var w); - float vw = JVector.Dot(v, w.V); - if (vw >= 0.0f) + Real vw = JVector.Dot(v, w.V); + if (vw >= (Real)0.0) return false; if (!simplexSolver.AddVertex(w, out v)) return true; distSq = v.LengthSquared(); @@ -594,9 +594,9 @@ public bool Overlap(in MinkowskiDifference mkd) } public bool Collision(in MinkowskiDifference mkd, - out JVector point1, out JVector point2, out JVector normal, out float penetration) + out JVector point1, out JVector point2, out JVector normal, out Real penetration) { - const float CollideEpsilon = 1e-4f; + const Real CollideEpsilon = (Real)1e-4; const int MaxIter = 85; mkd.GetCenter(out Vertex centerVertex); @@ -614,7 +614,7 @@ public bool Collision(in MinkowskiDifference mkd, ctri = convexPolytope.GetClosestTriangle(); JVector searchDir = ctri.ClosestToOrigin; - float searchDirSq = ctri.ClosestToOriginSq; + Real searchDirSq = ctri.ClosestToOriginSq; if (!convexPolytope.OriginEnclosed) searchDir.Negate(); @@ -633,7 +633,7 @@ public bool Collision(in MinkowskiDifference mkd, // s = searchDir // // abs(dot(c - v, s)) / len(s) < e <=> [dot(c - v, s)]^2 = e*e*s^2 - float deltaDist = JVector.Dot(ctri.ClosestToOrigin - vertex.V, searchDir); + Real deltaDist = JVector.Dot(ctri.ClosestToOrigin - vertex.V, searchDir); if (deltaDist * deltaDist <= CollideEpsilon * CollideEpsilon * searchDirSq) { @@ -647,7 +647,7 @@ public bool Collision(in MinkowskiDifference mkd, } point1 = point2 = normal = JVector.Zero; - penetration = 0.0f; + penetration = (Real)0.0; Trace.WriteLine($"EPA: Could not converge within {MaxIter} iterations."); @@ -657,11 +657,11 @@ public bool Collision(in MinkowskiDifference mkd, convexPolytope.CalculatePoints(ctri, out point1, out point2); - penetration = MathF.Sqrt(ctri.ClosestToOriginSq); - if (!convexPolytope.OriginEnclosed) penetration *= -1.0f; + penetration = MathR.Sqrt(ctri.ClosestToOriginSq); + if (!convexPolytope.OriginEnclosed) penetration *= -(Real)1.0; - if (MathF.Abs(penetration) > NumericEpsilon) normal = ctri.ClosestToOrigin * (1.0f / penetration); - else normal = ctri.Normal * (1.0f / MathF.Sqrt(ctri.NormalSq)); + if (MathR.Abs(penetration) > NumericEpsilon) normal = ctri.ClosestToOrigin * ((Real)1.0 / penetration); + else normal = ctri.Normal * ((Real)1.0 / MathR.Sqrt(ctri.NormalSq)); return true; } @@ -714,7 +714,7 @@ public static bool PointTest(in ISupportMappable support, in JMatrix orientation /// Returns true if the ray intersects with the shape; otherwise, false. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool RayCast(in ISupportMappable support, in JQuaternion orientation, - in JVector position, in JVector origin, in JVector direction, out float lambda, out JVector normal) + in JVector position, in JVector origin, in JVector direction, out Real lambda, out JVector normal) { // rotate the ray into the reference frame of bodyA.. JVector tdirection = JVector.TransposedTransform(direction, orientation); @@ -741,7 +741,7 @@ public static bool RayCast(in ISupportMappable support, in JQuaternion orientati /// /// Returns true if the ray intersects with the shape; otherwise, false. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool RayCast(in ISupportMappable support, in JVector origin, in JVector direction, out float lambda, out JVector normal) + public static bool RayCast(in ISupportMappable support, in JVector origin, in JVector direction, out Real lambda, out JVector normal) { return solver.RayCast(support, origin, direction, out lambda, out normal); } @@ -777,7 +777,7 @@ public static bool RayCast(in ISupportMappable support, in JVector origin, in JV [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool Collision(in ISupportMappable supportA, in ISupportMappable supportB, in JQuaternion orientationB, in JVector positionB, - out JVector pointA, out JVector pointB, out JVector normal, out float penetration) + out JVector pointA, out JVector pointB, out JVector normal, out Real penetration) { Unsafe.SkipInit(out MinkowskiDifference mkd); mkd.SupportA = supportA; @@ -824,7 +824,7 @@ public static bool Collision(in ISupportMappable supportA, in ISupportMappable s public static bool Collision(in ISupportMappable supportA, in ISupportMappable supportB, in JQuaternion orientationA, in JQuaternion orientationB, in JVector positionA, in JVector positionB, - out JVector pointA, out JVector pointB, out JVector normal, out float penetration) + out JVector pointA, out JVector pointB, out JVector normal, out Real penetration) { Unsafe.SkipInit(out MinkowskiDifference mkd); mkd.SupportA = supportA; @@ -865,7 +865,7 @@ public static bool Collision(in ISupportMappable supportA, in ISupportMappable s [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool Distance(in ISupportMappable supportA, in ISupportMappable supportB, in JQuaternion orientationB, in JVector positionB, - out JVector pointA, out JVector pointB, out float distance) + out JVector pointA, out JVector pointB, out Real distance) { Unsafe.SkipInit(out MinkowskiDifference mkd); mkd.SupportA = supportA; @@ -895,7 +895,7 @@ public static bool Distance(in ISupportMappable supportA, in ISupportMappable su public static bool Distance(in ISupportMappable supportA, in ISupportMappable supportB, in JQuaternion orientationA, in JQuaternion orientationB, in JVector positionA, in JVector positionB, - out JVector pointA, out JVector pointB, out float distance) + out JVector pointA, out JVector pointB, out Real distance) { Unsafe.SkipInit(out MinkowskiDifference mkd); mkd.SupportA = supportA; @@ -996,7 +996,7 @@ public static bool Overlap(in ISupportMappable supportA, in ISupportMappable sup public static bool MPREPA(in ISupportMappable supportA, in ISupportMappable supportB, in JQuaternion orientationA, in JQuaternion orientationB, in JVector positionA, in JVector positionB, - out JVector pointA, out JVector pointB, out JVector normal, out float penetration) + out JVector pointA, out JVector pointB, out JVector normal, out Real penetration) { Unsafe.SkipInit(out MinkowskiDifference mkd); mkd.SupportA = supportA; @@ -1043,7 +1043,7 @@ public static bool MPREPA(in ISupportMappable supportA, in ISupportMappable supp [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool MPREPA(in ISupportMappable supportA, in ISupportMappable supportB, in JQuaternion orientationB, in JVector positionB, - out JVector pointA, out JVector pointB, out JVector normal, out float penetration) + out JVector pointA, out JVector pointB, out JVector normal, out Real penetration) { Unsafe.SkipInit(out MinkowskiDifference mkd); mkd.SupportA = supportA; @@ -1072,7 +1072,7 @@ public static bool Sweep(in ISupportMappable supportA, in ISupportMappable suppo in JQuaternion orientationA, in JQuaternion orientationB, in JVector positionA, in JVector positionB, in JVector sweepA, in JVector sweepB, - out JVector pointA, out JVector pointB, out JVector normal, out float lambda) + out JVector pointA, out JVector pointB, out JVector normal, out Real lambda) { Unsafe.SkipInit(out MinkowskiDifference mkd); @@ -1123,7 +1123,7 @@ public static bool Sweep(in ISupportMappable supportA, in ISupportMappable suppo [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool Sweep(in ISupportMappable supportA, in ISupportMappable supportB, in JQuaternion orientationB, in JVector positionB, in JVector sweepB, - out JVector pointA, out JVector pointB, out JVector normal, out float lambda) + out JVector pointA, out JVector pointB, out JVector normal, out Real lambda) { Unsafe.SkipInit(out MinkowskiDifference mkd); diff --git a/src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs b/src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs index 47048383..51fde3ee 100644 --- a/src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs +++ b/src/Jitter2/Collision/NarrowPhase/SimplexSolver.cs @@ -33,7 +33,7 @@ namespace Jitter2.Collision; public unsafe struct SimplexSolver { - const float Epsilon = 1e-8f; + const Real Epsilon = (Real)1e-8; private JVector v0; private JVector v1; @@ -54,13 +54,13 @@ private JVector ClosestSegment(int i0, int i1, out uint mask) JVector b = ptr[i1]; JVector v = b - a; - float vsq = v.LengthSquared(); + Real vsq = v.LengthSquared(); bool degenerate = vsq < Epsilon; - float t = -JVector.Dot(a, v) / vsq; - float lambda0 = 1 - t; - float lambda1 = t; + Real t = -JVector.Dot(a, v) / vsq; + Real lambda0 = 1 - t; + Real lambda1 = t; mask = (1u << i0 | 1u << i1); @@ -94,25 +94,25 @@ private JVector ClosestTriangle(int i0, int i1, int i2, out uint mask) JVector normal = u % v; - float t = normal.LengthSquared(); - float it = 1.0f / t; + Real t = normal.LengthSquared(); + Real it = (Real)1.0 / t; bool degenerate = t < Epsilon; JVector.Cross(u, a, out var c1); JVector.Cross(a, v, out var c2); - float lambda2 = JVector.Dot(c1, normal) * it; - float lambda1 = JVector.Dot(c2, normal) * it; - float lambda0 = 1.0f - lambda2 - lambda1; + Real lambda2 = JVector.Dot(c1, normal) * it; + Real lambda1 = JVector.Dot(c2, normal) * it; + Real lambda0 = (Real)1.0 - lambda2 - lambda1; - float bestDistance = float.MaxValue; + Real bestDistance = Real.MaxValue; Unsafe.SkipInit(out JVector closestPt); - if (lambda0 < 0.0f || degenerate) + if (lambda0 < (Real)0.0 || degenerate) { var closest = ClosestSegment(i1, i2, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { mask = m; @@ -121,10 +121,10 @@ private JVector ClosestTriangle(int i0, int i1, int i2, out uint mask) } } - if (lambda1 < 0.0f || degenerate) + if (lambda1 < (Real)0.0 || degenerate) { var closest = ClosestSegment(i0, i2, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { mask = m; @@ -133,10 +133,10 @@ private JVector ClosestTriangle(int i0, int i1, int i2, out uint mask) } } - if (lambda2 < 0.0f || degenerate) + if (lambda2 < (Real)0.0 || degenerate) { var closest = ClosestSegment(i0, i1, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { mask = m; @@ -153,31 +153,31 @@ private JVector ClosestTriangle(int i0, int i1, int i2, out uint mask) private JVector ClosestTetrahedron(out uint mask) { [MethodImpl(MethodImplOptions.AggressiveInlining)] - float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) + Real Determinant(in JVector a, in JVector b, in JVector c, in JVector d) { return JVector.Dot(b - a, JVector.Cross(c - a, d - a)); } - float detT = Determinant(v0, v1, v2, v3); - float inverseDetT = 1.0f / detT; + Real detT = Determinant(v0, v1, v2, v3); + Real inverseDetT = (Real)1.0 / detT; bool degenerate = detT * detT < Epsilon; - float lambda0 = Determinant(JVector.Zero, v1, v2, v3) * inverseDetT; - float lambda1 = Determinant(v0, JVector.Zero, v2, v3) * inverseDetT; - float lambda2 = Determinant(v0, v1, JVector.Zero, v3) * inverseDetT; - float lambda3 = 1.0f - lambda0 - lambda1 - lambda2; + Real lambda0 = Determinant(JVector.Zero, v1, v2, v3) * inverseDetT; + Real lambda1 = Determinant(v0, JVector.Zero, v2, v3) * inverseDetT; + Real lambda2 = Determinant(v0, v1, JVector.Zero, v3) * inverseDetT; + Real lambda3 = (Real)1.0 - lambda0 - lambda1 - lambda2; - float bestDistance = float.MaxValue; + Real bestDistance = Real.MaxValue; Unsafe.SkipInit(out JVector closestPt); mask = 0; - if (lambda0 < 0.0f || degenerate) + if (lambda0 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(1, 2, 3, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { mask = m; @@ -186,10 +186,10 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) } } - if (lambda1 < 0.0f || degenerate) + if (lambda1 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(0, 2, 3, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { mask = m; @@ -198,10 +198,10 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) } } - if (lambda2 < 0.0f || degenerate) + if (lambda2 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(0, 1, 3, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { mask = m; @@ -210,10 +210,10 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) } } - if (lambda3 < 0.0f || degenerate) + if (lambda3 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(0, 1, 2, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { mask = m; diff --git a/src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs b/src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs index aa2aaaf2..cc042a0f 100644 --- a/src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs +++ b/src/Jitter2/Collision/NarrowPhase/SimplexSolverAB.cs @@ -35,19 +35,19 @@ namespace Jitter2.Collision; public unsafe struct SimplexSolverAB { - const float Epsilon = 1e-8f; + const Real Epsilon = (Real)1e-8; private struct Barycentric { - public float Lambda0; - public float Lambda1; - public float Lambda2; - public float Lambda3; + public Real Lambda0; + public Real Lambda1; + public Real Lambda2; + public Real Lambda3; - public float this[int i] + public Real this[int i] { - get => ((float*)Unsafe.AsPointer(ref this.Lambda0))[i]; - set => ((float*)Unsafe.AsPointer(ref this.Lambda0))[i] = value; + get => ((Real*)Unsafe.AsPointer(ref this.Lambda0))[i]; + set => ((Real*)Unsafe.AsPointer(ref this.Lambda0))[i] = value; } } @@ -71,13 +71,13 @@ private JVector ClosestSegment(int i0, int i1, ref Barycentric bc, out uint mask JVector b = ptr[i1].V; JVector v = b - a; - float vsq = v.LengthSquared(); + Real vsq = v.LengthSquared(); bool degenerate = vsq < Epsilon; - float t = -JVector.Dot(a, v) / vsq; - float lambda0 = 1 - t; - float lambda1 = t; + Real t = -JVector.Dot(a, v) / vsq; + Real lambda0 = 1 - t; + Real lambda1 = t; mask = (1u << i0 | 1u << i1); @@ -114,26 +114,26 @@ private JVector ClosestTriangle(int i0, int i1, int i2, ref Barycentric bc, out JVector normal = u % v; - float t = normal.LengthSquared(); - float it = 1.0f / t; + Real t = normal.LengthSquared(); + Real it = (Real)1.0 / t; bool degenerate = t < Epsilon; JVector.Cross(u, a, out var c1); JVector.Cross(a, v, out var c2); - float lambda2 = JVector.Dot(c1, normal) * it; - float lambda1 = JVector.Dot(c2, normal) * it; - float lambda0 = 1.0f - lambda2 - lambda1; + Real lambda2 = JVector.Dot(c1, normal) * it; + Real lambda1 = JVector.Dot(c2, normal) * it; + Real lambda0 = (Real)1.0 - lambda2 - lambda1; - float bestDistance = float.MaxValue; + Real bestDistance = Real.MaxValue; Unsafe.SkipInit(out JVector closestPt); Unsafe.SkipInit(out Barycentric b0); - if (lambda0 < 0.0f || degenerate) + if (lambda0 < (Real)0.0 || degenerate) { var closest = ClosestSegment(i1, i2, ref b0, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { bc = b0; @@ -143,10 +143,10 @@ private JVector ClosestTriangle(int i0, int i1, int i2, ref Barycentric bc, out } } - if (lambda1 < 0.0f || degenerate) + if (lambda1 < (Real)0.0 || degenerate) { var closest = ClosestSegment(i0, i2, ref b0, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { bc = b0; @@ -156,10 +156,10 @@ private JVector ClosestTriangle(int i0, int i1, int i2, ref Barycentric bc, out } } - if (lambda2 < 0.0f || degenerate) + if (lambda2 < (Real)0.0 || degenerate) { var closest = ClosestSegment(i0, i1, ref b0, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { bc = b0; @@ -181,32 +181,32 @@ private JVector ClosestTriangle(int i0, int i1, int i2, ref Barycentric bc, out private JVector ClosestTetrahedron(ref Barycentric bc, out uint mask) { [MethodImpl(MethodImplOptions.AggressiveInlining)] - float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) + Real Determinant(in JVector a, in JVector b, in JVector c, in JVector d) { return JVector.Dot(b - a, JVector.Cross(c - a, d - a)); } - float detT = Determinant(v0.V, v1.V, v2.V, v3.V); - float inverseDetT = 1.0f / detT; + Real detT = Determinant(v0.V, v1.V, v2.V, v3.V); + Real inverseDetT = (Real)1.0 / detT; bool degenerate = detT * detT < Epsilon; - float lambda0 = Determinant(JVector.Zero, v1.V, v2.V, v3.V) * inverseDetT; - float lambda1 = Determinant(v0.V, JVector.Zero, v2.V, v3.V) * inverseDetT; - float lambda2 = Determinant(v0.V, v1.V, JVector.Zero, v3.V) * inverseDetT; - float lambda3 = 1.0f - lambda0 - lambda1 - lambda2; + Real lambda0 = Determinant(JVector.Zero, v1.V, v2.V, v3.V) * inverseDetT; + Real lambda1 = Determinant(v0.V, JVector.Zero, v2.V, v3.V) * inverseDetT; + Real lambda2 = Determinant(v0.V, v1.V, JVector.Zero, v3.V) * inverseDetT; + Real lambda3 = (Real)1.0 - lambda0 - lambda1 - lambda2; - float bestDistance = float.MaxValue; + Real bestDistance = Real.MaxValue; Unsafe.SkipInit(out JVector closestPt); Unsafe.SkipInit(out Barycentric b0); mask = 0; - if (lambda0 < 0.0f || degenerate) + if (lambda0 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(1, 2, 3, ref b0, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { bc = b0; @@ -216,10 +216,10 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) } } - if (lambda1 < 0.0f || degenerate) + if (lambda1 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(0, 2, 3, ref b0, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { bc = b0; @@ -229,10 +229,10 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) } } - if (lambda2 < 0.0f || degenerate) + if (lambda2 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(0, 1, 3, ref b0, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { bc = b0; @@ -242,10 +242,10 @@ float Determinant(in JVector a, in JVector b, in JVector c, in JVector d) } } - if (lambda3 < 0.0f || degenerate) + if (lambda3 < (Real)0.0 || degenerate) { var closest = ClosestTriangle(0, 1, 2, ref b0, out uint m); - float dist = closest.LengthSquared(); + Real dist = closest.LengthSquared(); if (dist < bestDistance) { bc = b0; @@ -314,7 +314,7 @@ public bool AddVertex(in Vertex vertex, out JVector closest) int i0 = ix[0]; closest = ptr[i0].V; usageMask = 1u << i0; - barycentric[i0] = 1.0f; + barycentric[i0] = (Real)1.0; return true; } case 2: diff --git a/src/Jitter2/Collision/Shapes/BoxShape.cs b/src/Jitter2/Collision/Shapes/BoxShape.cs index f5f0f848..7cc60c2f 100644 --- a/src/Jitter2/Collision/Shapes/BoxShape.cs +++ b/src/Jitter2/Collision/Shapes/BoxShape.cs @@ -38,10 +38,10 @@ public class BoxShape : RigidBodyShape /// public JVector Size { - get => 2.0f * halfSize; + get => (Real)2.0 * halfSize; set { - halfSize = value * 0.5f; + halfSize = value * (Real)0.5; UpdateWorldBoundingBox(); } } @@ -52,7 +52,7 @@ public JVector Size /// The dimensions of the box. public BoxShape(JVector size) { - halfSize = 0.5f * size; + halfSize = (Real)0.5 * size; UpdateWorldBoundingBox(); } @@ -60,9 +60,9 @@ public BoxShape(JVector size) /// Creates a cube shape with the specified side length. /// /// The length of each side of the cube. - public BoxShape(float size) + public BoxShape(Real size) { - halfSize = new JVector(size * 0.5f); + halfSize = new JVector(size * (Real)0.5); UpdateWorldBoundingBox(); } @@ -72,9 +72,9 @@ public BoxShape(float size) /// The length of the box. /// The height of the box. /// The width of the box. - public BoxShape(float length, float height, float width) + public BoxShape(Real length, Real height, Real width) { - halfSize = 0.5f * new JVector(length, height, width); + halfSize = (Real)0.5 * new JVector(length, height, width); UpdateWorldBoundingBox(); } @@ -85,23 +85,23 @@ public override void SupportMap(in JVector direction, out JVector result) result.Z = Math.Sign(direction.Z) * halfSize.Z; } - public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda) { - float epsilon = 1e-22f; + Real epsilon = (Real)1e-22; JVector min = -halfSize; JVector max = halfSize; normal = JVector.Zero; - lambda = 0.0f; + lambda = (Real)0.0; - float exit = float.PositiveInfinity; + Real exit = Real.PositiveInfinity; - if (MathF.Abs(direction.X) > epsilon) + if (MathR.Abs(direction.X) > epsilon) { - float ix = 1.0f / direction.X; - float t0 = (min.X - origin.X) * ix; - float t1 = (max.X - origin.X) * ix; + Real ix = (Real)1.0 / direction.X; + Real t0 = (min.X - origin.X) * ix; + Real t1 = (max.X - origin.X) * ix; if (t0 > t1) (t0, t1) = (t1, t0); @@ -110,7 +110,7 @@ public override bool LocalRayCast(in JVector origin, in JVector direction, out J if (t0 > lambda) { lambda = t0; - normal = direction.X < 0.0f ? JVector.UnitX : -JVector.UnitX; + normal = direction.X < (Real)0.0 ? JVector.UnitX : -JVector.UnitX; } if (t1 < exit) exit = t1; @@ -120,11 +120,11 @@ public override bool LocalRayCast(in JVector origin, in JVector direction, out J return false; } - if (MathF.Abs(direction.Y) > epsilon) + if (MathR.Abs(direction.Y) > epsilon) { - float iy = 1.0f / direction.Y; - float t0 = (min.Y - origin.Y) * iy; - float t1 = (max.Y - origin.Y) * iy; + Real iy = (Real)1.0 / direction.Y; + Real t0 = (min.Y - origin.Y) * iy; + Real t1 = (max.Y - origin.Y) * iy; if (t0 > t1) (t0, t1) = (t1, t0); @@ -133,7 +133,7 @@ public override bool LocalRayCast(in JVector origin, in JVector direction, out J if (t0 > lambda) { lambda = t0; - normal = direction.Y < 0.0f ? JVector.UnitY : -JVector.UnitY; + normal = direction.Y < (Real)0.0 ? JVector.UnitY : -JVector.UnitY; } if (t1 < exit) exit = t1; @@ -143,11 +143,11 @@ public override bool LocalRayCast(in JVector origin, in JVector direction, out J return false; } - if (MathF.Abs(direction.Z) > epsilon) + if (MathR.Abs(direction.Z) > epsilon) { - float iz = 1.0f / direction.Z; - float t0 = (min.Z - origin.Z) * iz; - float t1 = (max.Z - origin.Z) * iz; + Real iz = (Real)1.0 / direction.Z; + Real t0 = (min.Z - origin.Z) * iz; + Real t1 = (max.Z - origin.Z) * iz; if (t0 > t1) (t0, t1) = (t1, t0); @@ -156,7 +156,7 @@ public override bool LocalRayCast(in JVector origin, in JVector direction, out J if (t0 > lambda) { lambda = t0; - normal = direction.Z < 0.0f ? JVector.UnitZ : -JVector.UnitZ; + normal = direction.Z < (Real)0.0 ? JVector.UnitZ : -JVector.UnitZ; } //if (t1 < exit) exit = t1; } @@ -181,15 +181,15 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector box.Max = position + ths; } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { - JVector size = halfSize * 2.0f; + JVector size = halfSize * (Real)2.0; mass = size.X * size.Y * size.Z; inertia = JMatrix.Identity; - inertia.M11 = 1.0f / 12.0f * mass * (size.Y * size.Y + size.Z * size.Z); - inertia.M22 = 1.0f / 12.0f * mass * (size.X * size.X + size.Z * size.Z); - inertia.M33 = 1.0f / 12.0f * mass * (size.X * size.X + size.Y * size.Y); + inertia.M11 = (Real)(1.0 / 12.0) * mass * (size.Y * size.Y + size.Z * size.Z); + inertia.M22 = (Real)(1.0 / 12.0) * mass * (size.X * size.X + size.Z * size.Z); + inertia.M33 = (Real)(1.0 / 12.0) * mass * (size.X * size.X + size.Y * size.Y); com = JVector.Zero; } diff --git a/src/Jitter2/Collision/Shapes/CapsuleShape.cs b/src/Jitter2/Collision/Shapes/CapsuleShape.cs index b535d1b8..0af6ee78 100644 --- a/src/Jitter2/Collision/Shapes/CapsuleShape.cs +++ b/src/Jitter2/Collision/Shapes/CapsuleShape.cs @@ -31,13 +31,13 @@ namespace Jitter2.Collision.Shapes; /// public class CapsuleShape : RigidBodyShape { - private float radius; - private float halfLength; + private Real radius; + private Real halfLength; /// /// Gets or sets the radius of the capsule. /// - public float Radius + public Real Radius { get => radius; set @@ -50,12 +50,12 @@ public float Radius /// /// Gets or sets the length of the cylindrical part of the capsule, excluding the half-spheres on both ends. /// - public float Length + public Real Length { - get => 2.0f * halfLength; + get => (Real)2.0 * halfLength; set { - halfLength = value / 2.0f; + halfLength = value / (Real)2.0; UpdateWorldBoundingBox(); } } @@ -65,10 +65,10 @@ public float Length /// /// The radius of the capsule. /// The length of the cylindrical part of the capsule, excluding the half-spheres at both ends. - public CapsuleShape(float radius = 0.5f, float length = 1.0f) + public CapsuleShape(Real radius = (Real)0.5, Real length = (Real)1.0) { this.radius = radius; - halfLength = 0.5f * length; + halfLength = (Real)0.5 * length; UpdateWorldBoundingBox(); } @@ -86,7 +86,7 @@ public override void SupportMap(in JVector direction, out JVector result) // we have to calculate the dot-product with the direction // vector to decide whether p_1 or p_2 is the correct support point - result.Y += MathF.Sign(direction.Y) * halfLength; + result.Y += MathR.Sign(direction.Y) * halfLength; } public override void GetCenter(out JVector point) @@ -98,9 +98,9 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector { JVector delta = halfLength * orientation.GetBasisY(); - box.Max.X = +radius + MathF.Abs(delta.X); - box.Max.Y = +radius + MathF.Abs(delta.Y); - box.Max.Z = +radius + MathF.Abs(delta.Z); + box.Max.X = +radius + MathR.Abs(delta.X); + box.Max.Y = +radius + MathR.Abs(delta.Y); + box.Max.Z = +radius + MathR.Abs(delta.Z); box.Min = -box.Max; @@ -108,20 +108,20 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector box.Max += position; } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { - float length = 2.0f * halfLength; + Real length = (Real)2.0 * halfLength; - float massSphere = 4.0f / 3.0f * MathF.PI * radius * radius * radius; - float massCylinder = MathF.PI * radius * radius * length; + Real massSphere = (Real)(4.0 / 3.0) * MathR.PI * radius * radius * radius; + Real massCylinder = MathR.PI * radius * radius * length; inertia = JMatrix.Identity; - inertia.M11 = massCylinder * (1.0f / 12.0f * length * length + 1.0f / 4.0f * radius * radius) + massSphere * - (2.0f / 5.0f * radius * radius + 1.0f / 4.0f * length * length + 3.0f / 8.0f * length * radius); - inertia.M22 = 1.0f / 2.0f * massCylinder * radius * radius + 2.0f / 5.0f * massSphere * radius * radius; - inertia.M33 = massCylinder * (1.0f / 12.0f * length * length + 1.0f / 4.0f * radius * radius) + massSphere * - (2.0f / 5.0f * radius * radius + 1.0f / 4.0f * length * length + 3.0f / 8.0f * length * radius); + inertia.M11 = massCylinder * ((Real)(1.0 / 12.0) * length * length + (Real)(1.0 / 4.0) * radius * radius) + massSphere * + ((Real)(2.0 / 5.0) * radius * radius + (Real)(1.0 / 4.0) * length * length + (Real)(3.0 / 8.0) * length * radius); + inertia.M22 = (Real)(1.0 / 2.0) * massCylinder * radius * radius + (Real)(2.0 / 5.0) * massSphere * radius * radius; + inertia.M33 = massCylinder * ((Real)(1.0 / 12.0) * length * length + (Real)(1.0 / 4.0) * radius * radius) + massSphere * + ((Real)(2.0 / 5.0) * radius * radius + (Real)(1.0 / 4.0) * length * length + (Real)(3.0 / 8.0) * length * radius); mass = massCylinder + massSphere; com = JVector.Zero; diff --git a/src/Jitter2/Collision/Shapes/ConeShape.cs b/src/Jitter2/Collision/Shapes/ConeShape.cs index d782d1ad..48ba56b7 100644 --- a/src/Jitter2/Collision/Shapes/ConeShape.cs +++ b/src/Jitter2/Collision/Shapes/ConeShape.cs @@ -31,13 +31,13 @@ namespace Jitter2.Collision.Shapes; /// public class ConeShape : RigidBodyShape { - private float radius; - private float height; + private Real radius; + private Real height; /// /// Gets or sets the radius of the cone at its base. /// - public float Radius + public Real Radius { get => radius; set @@ -50,7 +50,7 @@ public float Radius /// /// Gets or sets the height of the cone. /// - public float Height + public Real Height { get => height; set @@ -65,7 +65,7 @@ public float Height /// /// The radius of the cone at its base. /// The height of the cone. - public ConeShape(float radius = 0.5f, float height = 1.0f) + public ConeShape(Real radius = (Real)0.5, Real height = (Real)1.0) { this.radius = radius; this.height = height; @@ -74,29 +74,29 @@ public ConeShape(float radius = 0.5f, float height = 1.0f) public override void SupportMap(in JVector direction, out JVector result) { - const float ZeroEpsilon = 1e-12f; + const Real ZeroEpsilon = (Real)1e-12; // cone = disk + point // center of mass of a cone is at 0.25 height JVector ndir = direction; - ndir.Y = 0.0f; - float ndir2 = ndir.LengthSquared(); + ndir.Y = (Real)0.0; + Real ndir2 = ndir.LengthSquared(); if (ndir2 > ZeroEpsilon) { - ndir *= radius / MathF.Sqrt(ndir2); + ndir *= radius / MathR.Sqrt(ndir2); } - ndir.Y = -0.25f * height; + ndir.Y = -(Real)0.25 * height; // disk support point vs (0, 0.75 * height, 0) - if (JVector.Dot(direction, ndir) >= direction.Y * 0.75f * height) + if (JVector.Dot(direction, ndir) >= direction.Y * (Real)0.75 * height) { result = ndir; } else { - result = new JVector(0, 0.75f * height, 0); + result = new JVector(0, (Real)0.75 * height, 0); } } @@ -107,40 +107,40 @@ public override void GetCenter(out JVector point) public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box) { - const float ZeroEpsilon = 1e-12f; + const Real ZeroEpsilon = (Real)1e-12; JVector upa = orientation.GetBasisY(); - float xx = upa.X * upa.X; - float yy = upa.Y * upa.Y; - float zz = upa.Z * upa.Z; + Real xx = upa.X * upa.X; + Real yy = upa.Y * upa.Y; + Real zz = upa.Z * upa.Z; - float l1 = yy + zz; - float l2 = xx + zz; - float l3 = xx + yy; + Real l1 = yy + zz; + Real l2 = xx + zz; + Real l3 = xx + yy; - float xext = 0, yext = 0, zext = 0; + Real xext = 0, yext = 0, zext = 0; if (l1 > ZeroEpsilon) { - float sl = 1.0f / MathF.Sqrt(l1); + Real sl = (Real)1.0 / MathR.Sqrt(l1); xext = (yy + zz) * sl * radius; } if (l2 > ZeroEpsilon) { - float sl = 1.0f / MathF.Sqrt(l2); + Real sl = (Real)1.0 / MathR.Sqrt(l2); yext = (xx + zz) * sl * radius; } if (l3 > ZeroEpsilon) { - float sl = 1.0f / MathF.Sqrt(l3); + Real sl = (Real)1.0 / MathR.Sqrt(l3); zext = (xx + yy) * sl * radius; } - JVector p1 = -0.25f * height * upa; - JVector p2 = +0.75f * height * upa; + JVector p1 = -(Real)0.25 * height * upa; + JVector p2 = +(Real)0.75 * height * upa; box.Min = p1 - new JVector(xext, yext, zext); box.Max = p1 + new JVector(xext, yext, zext); @@ -151,14 +151,14 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector box.Max += position; } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { - mass = 1.0f / 3.0f * MathF.PI * radius * radius * height; + mass = (Real)(1.0 / 3.0) * MathR.PI * radius * radius * height; inertia = JMatrix.Identity; - inertia.M11 = mass * (3.0f / 20.0f * radius * radius + 3.0f / 80.0f * height * height); - inertia.M22 = 3.0f / 10.0f * mass * radius * radius; - inertia.M33 = mass * (3.0f / 20.0f * radius * radius + 3.0f / 80.0f * height * height); + inertia.M11 = mass * ((Real)(3.0 / 20.0) * radius * radius + (Real)(3.0 / 80.0) * height * height); + inertia.M22 = (Real)(3.0 / 10.0) * mass * radius * radius; + inertia.M33 = mass * ((Real)(3.0 / 20.0) * radius * radius + (Real)(3.0 / 80.0) * height * height); com = JVector.Zero; } diff --git a/src/Jitter2/Collision/Shapes/ConvexHullShape.cs b/src/Jitter2/Collision/Shapes/ConvexHullShape.cs index 81510016..c32b6586 100644 --- a/src/Jitter2/Collision/Shapes/ConvexHullShape.cs +++ b/src/Jitter2/Collision/Shapes/ConvexHullShape.cs @@ -75,7 +75,7 @@ public CHullTriangle(ushort a, ushort b, ushort c) private JBBox cachedBoundingBox; private JMatrix cachedInertia; - private float cachedMass; + private Real cachedMass; private JVector cachedCenter; private CHullVector[] vertices = null!; @@ -208,7 +208,7 @@ public void UpdateShape() CalcInitBox(); } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { inertia = cachedInertia; com = cachedCenter; @@ -221,8 +221,8 @@ public void CalculateMassInertia() cachedInertia = JMatrix.Zero; cachedMass = 0; - const float a = 1.0f / 60.0f; - const float b = 1.0f / 120.0f; + const Real a = (Real)(1.0 / 60.0); + const Real b = (Real)(1.0 / 120.0); JMatrix C = new(a, b, b, b, a, b, b, b, a); JVector pointWithin = JVector.Zero; @@ -232,7 +232,7 @@ public void CalculateMassInertia() pointWithin += vertices[i].Vertex; } - pointWithin = pointWithin * (1.0f / vertices.Length) + shifted; + pointWithin = pointWithin * ((Real)1.0 / vertices.Length) + shifted; foreach (CHullTriangle tri in indices) { @@ -243,8 +243,8 @@ public void CalculateMassInertia() // check winding { JVector normal = (column1 - column0) % (column2 - column0); - float ddot = JVector.Dot(normal, column0 - pointWithin); - if (ddot < 0.0f) + Real ddot = JVector.Dot(normal, column0 - pointWithin); + if (ddot < (Real)0.0) { (column0, column1) = (column1, column0); } @@ -255,12 +255,12 @@ public void CalculateMassInertia() column0.Y, column1.Y, column2.Y, column0.Z, column1.Z, column2.Z); - float detA = A.Determinant(); + Real detA = A.Determinant(); JMatrix tetrahedronInertia = JMatrix.Multiply(A * C * JMatrix.Transpose(A), detA); - JVector tetrahedronCom = 1.0f / 4.0f * (column0 + column1 + column2); - float tetrahedronMass = 1.0f / 6.0f * detA; + JVector tetrahedronCom = (Real)(1.0 / 4.0) * (column0 + column1 + column2); + Real tetrahedronMass = (Real)(1.0 / 6.0) * detA; cachedInertia += tetrahedronInertia; cachedCenter += tetrahedronMass * tetrahedronCom; @@ -268,13 +268,13 @@ public void CalculateMassInertia() } cachedInertia = JMatrix.Multiply(JMatrix.Identity, cachedInertia.Trace()) - cachedInertia; - cachedCenter *= 1.0f / cachedMass; + cachedCenter *= (Real)1.0 / cachedMass; } public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box) { - JVector halfSize = 0.5f * (cachedBoundingBox.Max - cachedBoundingBox.Min); - JVector center = 0.5f * (cachedBoundingBox.Max + cachedBoundingBox.Min); + JVector halfSize = (Real)0.5 * (cachedBoundingBox.Max - cachedBoundingBox.Min); + JVector center = (Real)0.5 * (cachedBoundingBox.Max + cachedBoundingBox.Min); JMatrix ori = JMatrix.CreateFromQuaternion(orientation); JMatrix.Absolute(in ori, out JMatrix abs); @@ -318,7 +318,7 @@ private void CalcInitBox() private ushort InternalSupportMap(in JVector direction, out JVector result) { ushort current = 0; - float dotProduct = JVector.Dot(vertices[current].Vertex, direction); + Real dotProduct = JVector.Dot(vertices[current].Vertex, direction); again: var min = vertices[current].NeighborMinIndex; @@ -327,7 +327,7 @@ private ushort InternalSupportMap(in JVector direction, out JVector result) for (int i = min; i < max; i++) { ushort nb = neighborList[i]; - float nbProduct = JVector.Dot(vertices[nb].Vertex, direction); + Real nbProduct = JVector.Dot(vertices[nb].Vertex, direction); if (nbProduct > dotProduct) { diff --git a/src/Jitter2/Collision/Shapes/CylinderShape.cs b/src/Jitter2/Collision/Shapes/CylinderShape.cs index 0a7fab34..8d6c8fd7 100644 --- a/src/Jitter2/Collision/Shapes/CylinderShape.cs +++ b/src/Jitter2/Collision/Shapes/CylinderShape.cs @@ -31,13 +31,13 @@ namespace Jitter2.Collision.Shapes; /// public class CylinderShape : RigidBodyShape { - private float radius; - private float height; + private Real radius; + private Real height; /// /// Gets or sets the radius of the cylinder. /// - public float Radius + public Real Radius { get => radius; set @@ -50,7 +50,7 @@ public float Radius /// /// Gets or sets the height of the cylinder. /// - public float Height + public Real Height { get => height; set @@ -65,7 +65,7 @@ public float Height /// /// The height of the cylinder. /// The radius of the cylinder at its base. - public CylinderShape(float height, float radius) + public CylinderShape(Real height, Real radius) { this.radius = radius; this.height = height; @@ -79,58 +79,58 @@ public override void GetCenter(out JVector point) public override void SupportMap(in JVector direction, out JVector result) { - float sigma = (float)Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z); + Real sigma = (Real)Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z); - if (sigma > 0.0f) + if (sigma > (Real)0.0) { result.X = direction.X / sigma * radius; - result.Y = Math.Sign(direction.Y) * height * 0.5f; + result.Y = Math.Sign(direction.Y) * height * (Real)0.5; result.Z = direction.Z / sigma * radius; } else { - result.X = 0.0f; - result.Y = Math.Sign(direction.Y) * height * 0.5f; - result.Z = 0.0f; + result.X = (Real)0.0; + result.Y = Math.Sign(direction.Y) * height * (Real)0.5; + result.Z = (Real)0.0; } } public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box) { - const float zeroEpsilon = 1e-12f; + const Real zeroEpsilon = (Real)1e-12; JVector upa = orientation.GetBasisY(); - float xx = upa.X * upa.X; - float yy = upa.Y * upa.Y; - float zz = upa.Z * upa.Z; + Real xx = upa.X * upa.X; + Real yy = upa.Y * upa.Y; + Real zz = upa.Z * upa.Z; - float l1 = yy + zz; - float l2 = xx + zz; - float l3 = xx + yy; + Real l1 = yy + zz; + Real l2 = xx + zz; + Real l3 = xx + yy; - float xext = 0, yext = 0, zext = 0; + Real xext = 0, yext = 0, zext = 0; if (l1 > zeroEpsilon) { - float sl = 1.0f / MathF.Sqrt(l1); + Real sl = (Real)1.0 / MathR.Sqrt(l1); xext = (yy + zz) * sl * radius; } if (l2 > zeroEpsilon) { - float sl = 1.0f / MathF.Sqrt(l2); + Real sl = (Real)1.0 / MathR.Sqrt(l2); yext = (xx + zz) * sl * radius; } if (l3 > zeroEpsilon) { - float sl = 1.0f / MathF.Sqrt(l3); + Real sl = (Real)1.0 / MathR.Sqrt(l3); zext = (xx + yy) * sl * radius; } - JVector p1 = -0.5f * height * upa; - JVector p2 = +0.5f * height * upa; + JVector p1 = -(Real)0.5 * height * upa; + JVector p2 = +(Real)0.5 * height * upa; JVector delta = JVector.Max(p1, p2) + new JVector(xext, yext, zext); @@ -138,15 +138,15 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector box.Max = position + delta; } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { - mass = MathF.PI * radius * radius * height; + mass = MathR.PI * radius * radius * height; inertia = JMatrix.Identity; - inertia.M11 = 1.0f / 4.0f * mass * radius * radius + 1.0f / 12.0f * mass * height * height; - inertia.M22 = 1.0f / 2.0f * mass * radius * radius; - inertia.M33 = 1.0f / 4.0f * mass * radius * radius + 1.0f / 12.0f * mass * height * height; + inertia.M11 = (Real)(1.0 / 4.0) * mass * radius * radius + (Real)(1.0 / 12.0) * mass * height * height; + inertia.M22 = (Real)(1.0 / 2.0) * mass * radius * radius; + inertia.M33 = (Real)(1.0 / 4.0) * mass * radius * radius + (Real)(1.0 / 12.0) * mass * height * height; com = JVector.Zero; } diff --git a/src/Jitter2/Collision/Shapes/FatTriangleShape.cs b/src/Jitter2/Collision/Shapes/FatTriangleShape.cs index 92d13031..c44f2467 100644 --- a/src/Jitter2/Collision/Shapes/FatTriangleShape.cs +++ b/src/Jitter2/Collision/Shapes/FatTriangleShape.cs @@ -33,18 +33,18 @@ namespace Jitter2.Collision.Shapes; /// public class FatTriangleShape : TriangleShape { - private float thickness; + private Real thickness; /// /// Set or get the thickness of the triangle. /// /// Thickness must be larger than 0.01 length units. - public float Thickness + public Real Thickness { get => thickness; set { - const float minimumThickness = 0.01f; + const Real minimumThickness = (Real)0.01; if (value < minimumThickness) { @@ -60,7 +60,7 @@ public float Thickness /// /// The triangle mesh to which this triangle belongs. /// The index representing the position of the triangle within the mesh. - public FatTriangleShape(TriangleMesh mesh, int index, float thickness = 0.2f) : base(mesh, index) + public FatTriangleShape(TriangleMesh mesh, int index, Real thickness = (Real)0.2) : base(mesh, index) { this.thickness = thickness; UpdateWorldBoundingBox(); @@ -102,8 +102,8 @@ public override void SupportMap(in JVector direction, out JVector result) JVector b = Mesh.Vertices[triangle.IndexB]; JVector c = Mesh.Vertices[triangle.IndexC]; - float min = JVector.Dot(a, direction); - float dot = JVector.Dot(b, direction); + Real min = JVector.Dot(a, direction); + Real dot = JVector.Dot(b, direction); result = a; @@ -120,7 +120,7 @@ public override void SupportMap(in JVector direction, out JVector result) result = c; } - if (JVector.Dot(triangle.Normal, direction) < 0.0f) + if (JVector.Dot(triangle.Normal, direction) < (Real)0.0) result -= triangle.Normal * Thickness; } } \ No newline at end of file diff --git a/src/Jitter2/Collision/Shapes/PointCloudShape.cs b/src/Jitter2/Collision/Shapes/PointCloudShape.cs index 868799da..2c5343a1 100644 --- a/src/Jitter2/Collision/Shapes/PointCloudShape.cs +++ b/src/Jitter2/Collision/Shapes/PointCloudShape.cs @@ -36,7 +36,7 @@ public class PointCloudShape : RigidBodyShape { private JBBox cachedBoundingBox; private JMatrix cachedInertia; - private float cachedMass; + private Real cachedMass; private JVector cachedCenter; private List vertices; @@ -103,7 +103,7 @@ public void CalculateMassInertia() ShapeHelper.CalculateMassInertia(this, out cachedInertia, out cachedCenter, out cachedMass); } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { inertia = cachedInertia; com = cachedCenter; @@ -112,8 +112,8 @@ public override void CalculateMassInertia(out JMatrix inertia, out JVector com, public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box) { - JVector halfSize = 0.5f * (cachedBoundingBox.Max - cachedBoundingBox.Min); - JVector center = 0.5f * (cachedBoundingBox.Max + cachedBoundingBox.Min); + JVector halfSize = (Real)0.5 * (cachedBoundingBox.Max - cachedBoundingBox.Min); + JVector center = (Real)0.5 * (cachedBoundingBox.Max + cachedBoundingBox.Min); JMatrix ori = JMatrix.CreateFromQuaternion(orientation); JMatrix.Absolute(in ori, out JMatrix abs); @@ -156,9 +156,9 @@ private void CalcInitBox() public override void SupportMap(in JVector direction, out JVector result) { - float maxDotProduct = float.MinValue; + Real maxDotProduct = Real.MinValue; int maxIndex = 0; - float dotProduct; + Real dotProduct; for (int i = 0; i < vertices.Count; i++) { diff --git a/src/Jitter2/Collision/Shapes/RigidBodyShape.cs b/src/Jitter2/Collision/Shapes/RigidBodyShape.cs index d6d2b542..1112f028 100644 --- a/src/Jitter2/Collision/Shapes/RigidBodyShape.cs +++ b/src/Jitter2/Collision/Shapes/RigidBodyShape.cs @@ -35,7 +35,7 @@ public abstract class RigidBodyShape : Shape public sealed override JVector Velocity => RigidBody?.Velocity ?? JVector.Zero; - public sealed override void UpdateWorldBoundingBox(float dt = 0.0f) + public sealed override void UpdateWorldBoundingBox(Real dt = (Real)0.0) { JBBox box; @@ -63,7 +63,7 @@ public virtual void CalculateBoundingBox(in JQuaternion orientation, in JVector /// constructed using the support map function. /// [ReferenceFrame(ReferenceFrame.Local)] - public virtual void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public virtual void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { ShapeHelper.CalculateMassInertia(this, out inertia, out com, out mass); } @@ -89,13 +89,13 @@ public virtual void CalculateMassInertia(out JMatrix inertia, out JVector com, o /// true if the ray intersects with the object; otherwise, false. /// [ReferenceFrame(ReferenceFrame.Local)] - public virtual bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + public virtual bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda) { return NarrowPhase.RayCast(this, origin, direction, out lambda, out normal); } [ReferenceFrame(ReferenceFrame.World)] - public sealed override bool RayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + public sealed override bool RayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda) { ref var data = ref RigidBody.Data; diff --git a/src/Jitter2/Collision/Shapes/Shape.cs b/src/Jitter2/Collision/Shapes/Shape.cs index 69578b84..c563d367 100644 --- a/src/Jitter2/Collision/Shapes/Shape.cs +++ b/src/Jitter2/Collision/Shapes/Shape.cs @@ -52,25 +52,25 @@ public abstract class Shape : IDynamicTreeProxy, IUpdatableBoundingBox, ISupport int IDynamicTreeProxy.NodePtr { get; set; } - protected void SweptExpandBoundingBox(float dt) + protected void SweptExpandBoundingBox(Real dt) { JVector sweptDirection = dt * Velocity; JBBox box = WorldBoundingBox; - float sxa = MathF.Abs(sweptDirection.X); - float sya = MathF.Abs(sweptDirection.Y); - float sza = MathF.Abs(sweptDirection.Z); + Real sxa = MathR.Abs(sweptDirection.X); + Real sya = MathR.Abs(sweptDirection.Y); + Real sza = MathR.Abs(sweptDirection.Z); - float max = MathF.Max(MathF.Max(sxa, sya), sza); + Real max = MathR.Max(MathR.Max(sxa, sya), sza); - if (sweptDirection.X < 0.0f) box.Min.X -= max; + if (sweptDirection.X < (Real)0.0) box.Min.X -= max; else box.Max.X += max; - if (sweptDirection.Y < 0.0f) box.Min.Y -= max; + if (sweptDirection.Y < (Real)0.0) box.Min.Y -= max; else box.Max.Y += max; - if (sweptDirection.Z < 0.0f) box.Min.Z -= max; + if (sweptDirection.Z < (Real)0.0) box.Min.Z -= max; else box.Max.Z += max; WorldBoundingBox = box; @@ -82,10 +82,10 @@ protected void SweptExpandBoundingBox(float dt) public abstract JVector Velocity { get; } [ReferenceFrame(ReferenceFrame.World)] - public abstract void UpdateWorldBoundingBox(float dt = 0.0f); + public abstract void UpdateWorldBoundingBox(Real dt = (Real)0.0); [ReferenceFrame(ReferenceFrame.World)] - public abstract bool RayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda); + public abstract bool RayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda); /// [ReferenceFrame(ReferenceFrame.Local)] diff --git a/src/Jitter2/Collision/Shapes/ShapeHelper.cs b/src/Jitter2/Collision/Shapes/ShapeHelper.cs index 1e8d2bf9..3ebbdc68 100644 --- a/src/Jitter2/Collision/Shapes/ShapeHelper.cs +++ b/src/Jitter2/Collision/Shapes/ShapeHelper.cs @@ -31,7 +31,7 @@ namespace Jitter2.Collision.Shapes; /// public static class ShapeHelper { - private const float GoldenRatio = 1.6180339887498948482045f; + private const Real GoldenRatio = (Real)1.6180339887498948482045; private static readonly JVector[] icosahedronVertices = new JVector[12] { @@ -77,7 +77,7 @@ private static void Subdivide(ISupportMappable support, T hullCollection, { JVector n = (p3 - p1) % (p2 - p1); - if (n.LengthSquared() > 1e-16f) + if (n.LengthSquared() > (Real)1e-16) { hullCollection.Add(new JTriangle(p1, p2, p3)); } @@ -85,9 +85,9 @@ private static void Subdivide(ISupportMappable support, T hullCollection, return; } - JVector h1 = (v1 + v2) * 0.5f; - JVector h2 = (v2 + v3) * 0.5f; - JVector h3 = (v3 + v1) * 0.5f; + JVector h1 = (v1 + v2) * (Real)0.5; + JVector h2 = (v2 + v3) * (Real)0.5; + JVector h3 = (v3 + v1) * (Real)0.5; support.SupportMap(h1, out JVector sp1); support.SupportMap(h2, out JVector sp2); @@ -150,13 +150,13 @@ public static void CalculateBoundingBox(ISupportMappable support, /// Output parameter for the calculated center of mass vector. /// Output parameter for the calculated mass. public static void CalculateMassInertia(ISupportMappable support, out JMatrix inertia, out JVector centerOfMass, - out float mass, int subdivisions = 4) + out Real mass, int subdivisions = 4) { centerOfMass = JVector.Zero; inertia = JMatrix.Zero; mass = 0; - const float a = 1.0f / 60.0f, b = 1.0f / 120.0f; + const Real a = (Real)(1.0 / 60.0), b = (Real)(1.0 / 120.0); JMatrix C = new(a, b, b, b, a, b, b, b, a); foreach (JTriangle triangle in MakeHull(support, subdivisions)) @@ -170,14 +170,14 @@ public static void CalculateMassInertia(ISupportMappable support, out JMatrix in column0.Y, column1.Y, column2.Y, column0.Z, column1.Z, column2.Z); - float detA = A.Determinant(); + Real detA = A.Determinant(); // now transform this canonical tetrahedron to the target tetrahedron // inertia by a linear transformation A JMatrix tetrahedronInertia = JMatrix.Multiply(A * C * JMatrix.Transpose(A), detA); - JVector tetrahedronCOM = 1.0f / 4.0f * (column0 + column1 + column2); - float tetrahedronMass = 1.0f / 6.0f * detA; + JVector tetrahedronCOM = (Real)(1.0 / 4.0) * (column0 + column1 + column2); + Real tetrahedronMass = (Real)(1.0 / 6.0) * detA; inertia += tetrahedronInertia; centerOfMass += tetrahedronMass * tetrahedronCOM; @@ -185,6 +185,6 @@ public static void CalculateMassInertia(ISupportMappable support, out JMatrix in } inertia = JMatrix.Multiply(JMatrix.Identity, inertia.Trace()) - inertia; - centerOfMass *= 1.0f / mass; + centerOfMass *= (Real)1.0 / mass; } } \ No newline at end of file diff --git a/src/Jitter2/Collision/Shapes/SphereShape.cs b/src/Jitter2/Collision/Shapes/SphereShape.cs index 4a49f71d..432d0ad0 100644 --- a/src/Jitter2/Collision/Shapes/SphereShape.cs +++ b/src/Jitter2/Collision/Shapes/SphereShape.cs @@ -31,12 +31,12 @@ namespace Jitter2.Collision.Shapes; /// public class SphereShape : RigidBodyShape { - private float radius; + private Real radius; /// /// Gets or sets the radius of the sphere. /// - public float Radius + public Real Radius { get => radius; set @@ -50,8 +50,8 @@ public float Radius /// Initializes a new instance of the class with an optional radius parameter. /// The default radius is 1.0 units. /// - /// The radius of the sphere. Defaults to 1.0f. - public SphereShape(float radius = 1.0f) + /// The radius of the sphere. Defaults to (Real)1.0. + public SphereShape(Real radius = (Real)1.0) { this.radius = radius; UpdateWorldBoundingBox(); @@ -82,41 +82,41 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector JVector.Add(box.Max, position, out box.Max); } - public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda) { normal = JVector.Zero; - lambda = 0.0f; + lambda = (Real)0.0; - float disq = 1.0f / direction.LengthSquared(); - float p = JVector.Dot(direction, origin) * disq; - float d = p * p - (origin.LengthSquared() - radius * radius) * disq; + Real disq = (Real)1.0 / direction.LengthSquared(); + Real p = JVector.Dot(direction, origin) * disq; + Real d = p * p - (origin.LengthSquared() - radius * radius) * disq; - if (d < 0.0f) return false; + if (d < (Real)0.0) return false; - float sqrtd = MathF.Sqrt(d); + Real sqrtd = MathR.Sqrt(d); - float t0 = -p - sqrtd; - float t1 = -p + sqrtd; + Real t0 = -p - sqrtd; + Real t1 = -p + sqrtd; - if (t0 >= 0.0f) + if (t0 >= (Real)0.0) { lambda = t0; JVector.Normalize(origin + t0 * direction, out normal); return true; } - return t1 > 0.0f; + return t1 > (Real)0.0; } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { - mass = 4.0f / 3.0f * MathF.PI * radius * radius * radius; + mass = (Real)(4.0 / 3.0) * MathR.PI * radius * radius * radius; // (0,0,0) is the center of mass inertia = JMatrix.Identity; - inertia.M11 = 2.0f / 5.0f * mass * radius * radius; - inertia.M22 = 2.0f / 5.0f * mass * radius * radius; - inertia.M33 = 2.0f / 5.0f * mass * radius * radius; + inertia.M11 = (Real)(2.0 / 5.0) * mass * radius * radius; + inertia.M22 = (Real)(2.0 / 5.0) * mass * radius * radius; + inertia.M33 = (Real)(2.0 / 5.0) * mass * radius * radius; com = JVector.Zero; } diff --git a/src/Jitter2/Collision/Shapes/TransformedShape.cs b/src/Jitter2/Collision/Shapes/TransformedShape.cs index 1a06f9b7..97c76933 100644 --- a/src/Jitter2/Collision/Shapes/TransformedShape.cs +++ b/src/Jitter2/Collision/Shapes/TransformedShape.cs @@ -136,7 +136,7 @@ public override void GetCenter(out JVector point) point = JVector.Transform(point, transformation) + translation; } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { OriginalShape.CalculateMassInertia(out JMatrix oinertia, out JVector ocom, out mass); diff --git a/src/Jitter2/Collision/Shapes/TriangleMesh.cs b/src/Jitter2/Collision/Shapes/TriangleMesh.cs index e7d35e7d..733de4ec 100644 --- a/src/Jitter2/Collision/Shapes/TriangleMesh.cs +++ b/src/Jitter2/Collision/Shapes/TriangleMesh.cs @@ -194,7 +194,7 @@ int GetEdge(Edge e) JVector normal = (C - A) % (B - A); - if (MathHelper.CloseToZero(normal, 1e-12f)) + if (MathHelper.CloseToZero(normal, (Real)1e-12)) { throw new DegenerateTriangleException("Degenerate triangle found in mesh. Try to clean the " + "mesh in the editor of your choice first."); diff --git a/src/Jitter2/Collision/Shapes/TriangleShape.cs b/src/Jitter2/Collision/Shapes/TriangleShape.cs index b75b6cbe..fc54388f 100644 --- a/src/Jitter2/Collision/Shapes/TriangleShape.cs +++ b/src/Jitter2/Collision/Shapes/TriangleShape.cs @@ -47,7 +47,7 @@ public TriangleShape(TriangleMesh mesh, int index) UpdateWorldBoundingBox(); } - public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out float mass) + public override void CalculateMassInertia(out JMatrix inertia, out JVector com, out Real mass) { // This method is not supported for 2D objects in a 3D world as they have no mass/inertia. throw new NotSupportedException($"{nameof(TriangleShape)} has no mass properties." + @@ -84,7 +84,7 @@ public void GetWorldVertices(out JVector a, out JVector b, out JVector c) public override void CalculateBoundingBox(in JQuaternion orientation, in JVector position, out JBBox box) { - const float extraMargin = 0.01f; + const Real extraMargin = (Real)0.01; ref var triangle = ref Mesh.Indices[Index]; var a = Mesh.Vertices[triangle.IndexA]; @@ -107,7 +107,7 @@ public override void CalculateBoundingBox(in JQuaternion orientation, in JVector box.Max += position + extra; } - public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + public override bool LocalRayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda) { ref var triangle = ref Mesh.Indices[Index]; var a = Mesh.Vertices[triangle.IndexA]; @@ -124,7 +124,7 @@ public override void GetCenter(out JVector point) JVector b = Mesh.Vertices[triangle.IndexB]; JVector c = Mesh.Vertices[triangle.IndexC]; - point = 1.0f / 3.0f * (a + b + c); + point = (Real)(1.0 / 3.0) * (a + b + c); } public override void SupportMap(in JVector direction, out JVector result) @@ -135,8 +135,8 @@ public override void SupportMap(in JVector direction, out JVector result) JVector b = Mesh.Vertices[triangle.IndexB]; JVector c = Mesh.Vertices[triangle.IndexC]; - float min = JVector.Dot(a, direction); - float dot = JVector.Dot(b, direction); + Real min = JVector.Dot(a, direction); + Real dot = JVector.Dot(b, direction); result = a; diff --git a/src/Jitter2/Dynamics/Constraints/AngularMotor.cs b/src/Jitter2/Dynamics/Constraints/AngularMotor.cs index ded46996..e301fd95 100644 --- a/src/Jitter2/Dynamics/Constraints/AngularMotor.cs +++ b/src/Jitter2/Dynamics/Constraints/AngularMotor.cs @@ -40,7 +40,7 @@ public struct AngularMotorData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -48,13 +48,13 @@ public struct AngularMotorData public JVector LocalAxis1; public JVector LocalAxis2; - public float Velocity; - public float MaxForce; - public float MaxLambda; + public Real Velocity; + public Real MaxForce; + public Real MaxLambda; - public float EffectiveMass; + public Real EffectiveMass; - public float AccumulatedImpulse; + public Real AccumulatedImpulse; } private JHandle handle; @@ -93,7 +93,7 @@ public void Initialize(JVector axis) Initialize(axis, axis); } - public float TargetVelocity + public Real TargetVelocity { get => handle.Data.Velocity; set => handle.Data.Velocity = value; @@ -103,12 +103,12 @@ public float TargetVelocity public JVector LocalAxis2 => handle.Data.LocalAxis2; - public float MaximumForce + public Real MaximumForce { get => handle.Data.MaxForce; set { - if (value < 0.0f) + if (value < (Real)0.0) { throw new ArgumentException("Maximum force must not be negative."); } @@ -117,7 +117,7 @@ public float MaximumForce } } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref AngularMotorData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); @@ -129,15 +129,15 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.EffectiveMass = JVector.Transform(j1, body1.InverseInertiaWorld) * j1 + JVector.Transform(j2, body2.InverseInertiaWorld) * j2; - data.EffectiveMass = 1.0f / data.EffectiveMass; + data.EffectiveMass = (Real)1.0 / data.EffectiveMass; - data.MaxLambda = 1.0f / idt * data.MaxForce; + data.MaxLambda = (Real)1.0 / idt * data.MaxForce; body1.AngularVelocity -= JVector.Transform(j1 * data.AccumulatedImpulse, body1.InverseInertiaWorld); body2.AngularVelocity += JVector.Transform(j2 * data.AccumulatedImpulse, body2.InverseInertiaWorld); } - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref AngularMotorData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -146,11 +146,11 @@ public static void Iterate(ref ConstraintData constraint, float idt) JVector.Transform(data.LocalAxis1, body1.Orientation, out JVector j1); JVector.Transform(data.LocalAxis2, body2.Orientation, out JVector j2); - float jv = -j1 * body1.AngularVelocity + j2 * body2.AngularVelocity; + Real jv = -j1 * body1.AngularVelocity + j2 * body2.AngularVelocity; - float lambda = -(jv - data.Velocity) * data.EffectiveMass; + Real lambda = -(jv - data.Velocity) * data.EffectiveMass; - float olda = data.AccumulatedImpulse; + Real olda = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; diff --git a/src/Jitter2/Dynamics/Constraints/BallSocket.cs b/src/Jitter2/Dynamics/Constraints/BallSocket.cs index 1f07664a..84315ac2 100644 --- a/src/Jitter2/Dynamics/Constraints/BallSocket.cs +++ b/src/Jitter2/Dynamics/Constraints/BallSocket.cs @@ -42,7 +42,7 @@ public struct BallSocketData internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -54,8 +54,8 @@ public struct BallSocketData public JVector R1; public JVector R2; - public float BiasFactor; - public float Softness; + public Real BiasFactor; + public Real Softness; public JMatrix EffectiveMass; public JVector AccumulatedImpulse; @@ -89,11 +89,11 @@ public void Initialize(JVector anchor) JVector.ConjugatedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1); JVector.ConjugatedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2); - data.BiasFactor = 0.2f; - data.Softness = 0.00f; + data.BiasFactor = (Real)0.2; + data.Softness = (Real)0.0; } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref BallSocketData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref data.Body1.Data; @@ -113,7 +113,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) body2.InverseMass * JMatrix.Identity + JMatrix.Multiply(cr2, JMatrix.MultiplyTransposed(body2.InverseInertiaWorld, cr2)); - float softness = data.Softness * idt; + Real softness = data.Softness * idt; data.EffectiveMass.M11 += softness; data.EffectiveMass.M22 += softness; @@ -132,13 +132,13 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) body2.AngularVelocity += JVector.Transform(JVector.Transform(acc, cr2), body2.InverseInertiaWorld); } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; @@ -146,7 +146,7 @@ public float Bias public JVector Impulse => handle.Data.AccumulatedImpulse; - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref BallSocketData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -160,7 +160,7 @@ public static void Iterate(ref ConstraintData constraint, float idt) JVector jv = -body1.Velocity + JVector.Transform(body1.AngularVelocity, cr1) + body2.Velocity - JVector.Transform(body2.AngularVelocity, cr2); - JVector lambda = -1.0f * JVector.Transform(jv + data.Bias + softnessVector, data.EffectiveMass); + JVector lambda = -(Real)1.0 * JVector.Transform(jv + data.Bias + softnessVector, data.EffectiveMass); data.AccumulatedImpulse += lambda; diff --git a/src/Jitter2/Dynamics/Constraints/ConeLimit.cs b/src/Jitter2/Dynamics/Constraints/ConeLimit.cs index de566e9c..5a9ec27d 100644 --- a/src/Jitter2/Dynamics/Constraints/ConeLimit.cs +++ b/src/Jitter2/Dynamics/Constraints/ConeLimit.cs @@ -41,26 +41,26 @@ public struct ConeLimitData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; public JVector LocalAxis1, LocalAxis2; - public float BiasFactor; - public float Softness; + public Real BiasFactor; + public Real Softness; - public float EffectiveMass; - public float AccumulatedImpulse; - public float Bias; + public Real EffectiveMass; + public Real AccumulatedImpulse; + public Real Bias; - public float LimitLow; - public float LimitHigh; + public Real LimitLow; + public Real LimitHigh; public short Clamp; - public MemoryHelper.MemBlock48 J0; + public MemoryHelper.MemBlock6Real J0; } private JHandle handle; @@ -88,14 +88,14 @@ public void Initialize(JVector axis, AngularLimit limit) JVector.ConjugatedTransform(axis, body1.Orientation, out data.LocalAxis1); JVector.ConjugatedTransform(axis, body2.Orientation, out data.LocalAxis2); - data.Softness = 0.001f; - data.BiasFactor = 0.2f; + data.Softness = (Real)0.001; + data.BiasFactor = (Real)0.2; - float lower = (float)limit.From; - float upper = (float)limit.To; + Real lower = (Real)limit.From; + Real upper = (Real)limit.To; - data.LimitLow = MathF.Cos(lower); - data.LimitHigh = MathF.Cos(upper); + data.LimitLow = MathR.Cos(lower); + data.LimitHigh = MathR.Cos(upper); } public JAngle Angle @@ -110,11 +110,11 @@ public JAngle Angle JVector.Transform(data.LocalAxis1, body1.Orientation, out JVector a1); JVector.Transform(data.LocalAxis2, body2.Orientation, out JVector a2); - return (JAngle)MathF.Acos(JVector.Dot(a1, a2)); + return (JAngle)MathR.Acos(JVector.Dot(a1, a2)); } } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref ConeLimitData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); @@ -131,7 +131,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.Clamp = 0; - float error = JVector.Dot(a1, a2); + Real error = JVector.Dot(a1, a2); if (error < data.LimitHigh) { @@ -145,7 +145,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) } else { - data.AccumulatedImpulse = 0.0f; + data.AccumulatedImpulse = (Real)0.0; return; } @@ -154,7 +154,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.EffectiveMass += data.Softness * idt; - data.EffectiveMass = 1.0f / data.EffectiveMass; + data.EffectiveMass = (Real)1.0 / data.EffectiveMass; data.Bias = -error * data.BiasFactor * idt; @@ -165,21 +165,21 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) JVector.Transform(data.AccumulatedImpulse * jacobian[1], body2.InverseInertiaWorld); } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; } - public float Impulse => handle.Data.AccumulatedImpulse; + public Real Impulse => handle.Data.AccumulatedImpulse; - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref ConeLimitData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -189,25 +189,25 @@ public static void Iterate(ref ConstraintData constraint, float idt) var jacobian = new Span(Unsafe.AsPointer(ref data.J0), 2); - float jv = + Real jv = body1.AngularVelocity * jacobian[0] + body2.AngularVelocity * jacobian[1]; - float softnessScalar = data.AccumulatedImpulse * data.Softness * idt; + Real softnessScalar = data.AccumulatedImpulse * data.Softness * idt; - float lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); + Real lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); - float oldacc = data.AccumulatedImpulse; + Real oldacc = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; if (data.Clamp == 1) { - data.AccumulatedImpulse = MathF.Min(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Min(data.AccumulatedImpulse, (Real)0.0); } else { - data.AccumulatedImpulse = MathF.Max(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Max(data.AccumulatedImpulse, (Real)0.0); } lambda = data.AccumulatedImpulse - oldacc; diff --git a/src/Jitter2/Dynamics/Constraints/Constraint.cs b/src/Jitter2/Dynamics/Constraints/Constraint.cs index 5c9b478a..73d08b1e 100644 --- a/src/Jitter2/Dynamics/Constraints/Constraint.cs +++ b/src/Jitter2/Dynamics/Constraints/Constraint.cs @@ -30,11 +30,11 @@ namespace Jitter2.Dynamics.Constraints; [StructLayout(LayoutKind.Sequential, Size = ConstraintSize)] public unsafe struct SmallConstraintData { - public const int ConstraintSize = 128; + public const int ConstraintSize = Jitter2.Precision.ConstraintSizeSmall; internal int _internal; - public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* Iterate; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -43,11 +43,11 @@ public unsafe struct SmallConstraintData [StructLayout(LayoutKind.Sequential, Size = ConstraintSize)] public unsafe struct ConstraintData { - public const int ConstraintSize = 256; + public const int ConstraintSize = Jitter2.Precision.ConstraintSizeFull; internal int _internal; - public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* Iterate; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -82,8 +82,8 @@ internal Constraint() { } - protected unsafe delegate* iterate = null; - protected unsafe delegate* prepareForIteration = null; + protected unsafe delegate* iterate = null; + protected unsafe delegate* prepareForIteration = null; /// /// Enables or disables this constraint temporarily. For a complete removal of the constraint, diff --git a/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs b/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs index 73954a9c..0333fab0 100644 --- a/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs +++ b/src/Jitter2/Dynamics/Constraints/DistanceLimit.cs @@ -42,7 +42,7 @@ public struct DistanceLimitData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -50,18 +50,19 @@ public struct DistanceLimitData public JVector LocalAnchor1; public JVector LocalAnchor2; - public float BiasFactor; - public float Softness; - public float Distance; + public Real BiasFactor; + public Real Softness; + public Real Distance; - public float LimitMin; - public float LimitMax; + public Real LimitMin; + public Real LimitMax; - public float EffectiveMass; - public float AccumulatedImpulse; - public float Bias; + public Real EffectiveMass; + public Real AccumulatedImpulse; + public Real Bias; - public MemoryHelper.MemBlock48 J0; + + public MemoryHelper.MemBlock12Real J0; public short Clamp; } @@ -99,8 +100,8 @@ public void Initialize(JVector anchor1, JVector anchor2, LinearLimit limit) JVector.ConjugatedTransform(data.LocalAnchor1, body1.Orientation, out data.LocalAnchor1); JVector.ConjugatedTransform(data.LocalAnchor2, body2.Orientation, out data.LocalAnchor2); - data.Softness = 0.001f; - data.BiasFactor = 0.2f; + data.Softness = (Real)0.001; + data.BiasFactor = (Real)0.2; data.Distance = (anchor2 - anchor1).Length(); (data.LimitMin, data.LimitMax) = limit; @@ -144,7 +145,7 @@ public JVector Anchor2 } } - public float TargetDistance + public Real TargetDistance { set { @@ -154,7 +155,7 @@ public float TargetDistance get => handle.Data.Distance; } - public float Distance + public Real Distance { get { @@ -174,7 +175,7 @@ public float Distance } } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref DistanceLimitData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref data.Body1.Data; @@ -188,7 +189,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) JVector.Subtract(p2, p1, out JVector dp); - float error = dp.Length() - data.Distance; + Real error = dp.Length() - data.Distance; data.Clamp = 0; @@ -204,18 +205,18 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) } else { - data.AccumulatedImpulse = 0.0f; + data.AccumulatedImpulse = (Real)0.0; return; } JVector n = p2 - p1; - if (n.LengthSquared() != 0.0f) n.Normalize(); + if (n.LengthSquared() != (Real)0.0) n.Normalize(); var jacobian = new Span(Unsafe.AsPointer(ref data.J0), 4); - jacobian[0] = -1.0f * n; - jacobian[1] = -1.0f * (r1 % n); - jacobian[2] = 1.0f * n; + jacobian[0] = -(Real)1.0 * n; + jacobian[1] = -(Real)1.0 * (r1 % n); + jacobian[2] = (Real)1.0 * n; jacobian[3] = r2 % n; data.EffectiveMass = body1.InverseMass + @@ -225,7 +226,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.EffectiveMass += data.Softness * idt; - data.EffectiveMass = 1.0f / data.EffectiveMass; + data.EffectiveMass = (Real)1.0 / data.EffectiveMass; data.Bias = error * data.BiasFactor * idt; @@ -236,21 +237,21 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) body2.AngularVelocity += JVector.Transform(data.AccumulatedImpulse * jacobian[3], body2.InverseInertiaWorld); } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; } - public float Impulse => handle.Data.AccumulatedImpulse; + public Real Impulse => handle.Data.AccumulatedImpulse; - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref DistanceLimitData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -260,27 +261,27 @@ public static void Iterate(ref ConstraintData constraint, float idt) var jacobian = new Span(Unsafe.AsPointer(ref data.J0), 4); - float jv = + Real jv = body1.Velocity * jacobian[0] + body1.AngularVelocity * jacobian[1] + body2.Velocity * jacobian[2] + body2.AngularVelocity * jacobian[3]; - float softnessScalar = data.AccumulatedImpulse * data.Softness * idt; + Real softnessScalar = data.AccumulatedImpulse * data.Softness * idt; - float lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); + Real lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); - float oldacc = data.AccumulatedImpulse; + Real oldacc = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; if (data.Clamp == 1) { - data.AccumulatedImpulse = MathF.Min(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Min(data.AccumulatedImpulse, (Real)0.0); } else { - data.AccumulatedImpulse = MathF.Max(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Max(data.AccumulatedImpulse, (Real)0.0); } lambda = data.AccumulatedImpulse - oldacc; diff --git a/src/Jitter2/Dynamics/Constraints/FixedAngle.cs b/src/Jitter2/Dynamics/Constraints/FixedAngle.cs index 0964d7e4..dee18072 100644 --- a/src/Jitter2/Dynamics/Constraints/FixedAngle.cs +++ b/src/Jitter2/Dynamics/Constraints/FixedAngle.cs @@ -39,16 +39,16 @@ public struct FixedAngleData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; - public float MinAngle; - public float MaxAngle; + public Real MinAngle; + public Real MaxAngle; - public float BiasFactor; - public float Softness; + public Real BiasFactor; + public Real Softness; public JVector Axis; public JQuaternion Q0; @@ -78,8 +78,8 @@ public void Initialize() ref RigidBodyData body1 = ref data.Body1.Data; ref RigidBodyData body2 = ref data.Body2.Data; - data.Softness = 0.001f; - data.BiasFactor = 0.2f; + data.Softness = (Real)0.001; + data.BiasFactor = (Real)0.2; JQuaternion q1 = body1.Orientation; JQuaternion q2 = body2.Orientation; @@ -87,7 +87,7 @@ public void Initialize() data.Q0 = q2.Conjugate() * q1; } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref FixedAngleData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); @@ -105,10 +105,10 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.Jacobian = QMatrix.ProjectMultiplyLeftRight(data.Q0 * q1.Conjugate(), q2); - if (quat0.W < 0.0f) + if (quat0.W < (Real)0.0) { - error *= -1.0f; - data.Jacobian *= -1.0f; + error *= -(Real)1.0; + data.Jacobian *= -(Real)1.0; } data.EffectiveMass = JMatrix.Multiply(data.Jacobian, JMatrix.MultiplyTransposed(body1.InverseInertiaWorld + body2.InverseInertiaWorld, data.Jacobian)); @@ -125,13 +125,13 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) body2.AngularVelocity -= JVector.Transform(JVector.TransposedTransform(data.AccumulatedImpulse, data.Jacobian), body2.InverseInertiaWorld); } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; @@ -139,7 +139,7 @@ public float Bias public JVector Impulse => handle.Data.AccumulatedImpulse; - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref FixedAngleData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -147,7 +147,7 @@ public static void Iterate(ref ConstraintData constraint, float idt) JVector jv = JVector.Transform(body1.AngularVelocity - body2.AngularVelocity, data.Jacobian); JVector softness = data.AccumulatedImpulse * (data.Softness * idt); - JVector lambda = -1.0f * JVector.Transform(jv + data.Bias + softness, data.EffectiveMass); + JVector lambda = -(Real)1.0 * JVector.Transform(jv + data.Bias + softness, data.EffectiveMass); data.AccumulatedImpulse += lambda; diff --git a/src/Jitter2/Dynamics/Constraints/HingeAngle.cs b/src/Jitter2/Dynamics/Constraints/HingeAngle.cs index f2094cc3..95eb5c47 100644 --- a/src/Jitter2/Dynamics/Constraints/HingeAngle.cs +++ b/src/Jitter2/Dynamics/Constraints/HingeAngle.cs @@ -40,19 +40,19 @@ public struct HingeAngleData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; - public float MinAngle; - public float MaxAngle; + public Real MinAngle; + public Real MaxAngle; - public float BiasFactor; - public float LimitBias; + public Real BiasFactor; + public Real LimitBias; - public float LimitSoftness; - public float Softness; + public Real LimitSoftness; + public Real Softness; public JVector Axis; public JQuaternion Q0; @@ -86,13 +86,13 @@ public void Initialize(JVector axis, AngularLimit limit) ref RigidBodyData body1 = ref data.Body1.Data; ref RigidBodyData body2 = ref data.Body2.Data; - data.Softness = 0.001f; - data.LimitSoftness = 0.001f; - data.BiasFactor = 0.2f; - data.LimitBias = 0.1f; + data.Softness = (Real)0.001; + data.LimitSoftness = (Real)0.001; + data.BiasFactor = (Real)0.2; + data.LimitBias = (Real)0.1; - data.MinAngle = MathF.Sin((float)limit.From / 2.0f); - data.MaxAngle = MathF.Sin((float)limit.To / 2.0f); + data.MinAngle = MathR.Sin((Real)limit.From / (Real)2.0); + data.MaxAngle = MathR.Sin((Real)limit.To / (Real)2.0); data.Axis = JVector.TransposedTransform(axis, body2.Orientation); @@ -107,12 +107,12 @@ public AngularLimit Limit set { ref HingeAngleData data = ref handle.Data; - data.MinAngle = MathF.Sin((float)value.From / 2.0f); - data.MaxAngle = MathF.Sin((float)value.To / 2.0f); + data.MinAngle = MathR.Sin((Real)value.From / (Real)2.0); + data.MaxAngle = MathR.Sin((Real)value.To / (Real)2.0); } } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref HingeAngleData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); @@ -134,12 +134,12 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.Clamp = 0; - JMatrix m0 = (-1.0f / 2.0f) * QMatrix.ProjectMultiplyLeftRight(data.Q0 * q1.Conjugate(), q2); + JMatrix m0 = (-(Real)(1.0 / 2.0)) * QMatrix.ProjectMultiplyLeftRight(data.Q0 * q1.Conjugate(), q2); - if (quat0.W < 0.0f) + if (quat0.W < (Real)0.0) { - error *= -1.0f; - m0 *= -1.0f; + error *= -(Real)1.0; + m0 *= -(Real)1.0; } data.Jacobian.UnsafeGet(0) = JVector.TransposedTransform(p0, m0); @@ -152,8 +152,8 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.EffectiveMass.M22 += data.Softness * idt; data.EffectiveMass.M33 += data.LimitSoftness * idt; - float maxa = data.MaxAngle; - float mina = data.MinAngle; + Real maxa = data.MaxAngle; + Real mina = data.MinAngle; if (error.Z > maxa) { @@ -198,35 +198,35 @@ public JAngle Angle JQuaternion quat0 = data.Q0 * q1.Conjugate() * q2; - if (quat0.W < 0.0f) + if (quat0.W < (Real)0.0) { - quat0 *= -1.0f; + quat0 *= -(Real)1.0; } - float error = JVector.Dot(data.Axis, new JVector(quat0.X, quat0.Y, quat0.Z)); - return (JAngle)(2.0f * MathF.Asin(error)); + Real error = JVector.Dot(data.Axis, new JVector(quat0.X, quat0.Y, quat0.Z)); + return (JAngle)((Real)2.0 * MathR.Asin(error)); } } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float LimitSoftness + public Real LimitSoftness { get => handle.Data.LimitSoftness; set => handle.Data.LimitSoftness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; } - public float LimitBias + public Real LimitBias { get => handle.Data.LimitBias; set => handle.Data.LimitBias = value; @@ -234,7 +234,7 @@ public float LimitBias public JVector Impulse => handle.Data.AccumulatedImpulse; - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref HingeAngleData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -247,7 +247,7 @@ public static void Iterate(ref ConstraintData constraint, float idt) softness.Y *= data.Softness; softness.Z *= data.LimitSoftness; - JVector lambda = -1.0f * JVector.Transform(jv + data.Bias + softness, data.EffectiveMass); + JVector lambda = -(Real)1.0 * JVector.Transform(jv + data.Bias + softness, data.EffectiveMass); JVector origAcc = data.AccumulatedImpulse; @@ -255,11 +255,11 @@ public static void Iterate(ref ConstraintData constraint, float idt) if (data.Clamp == 1) { - data.AccumulatedImpulse.Z = MathF.Min(0, data.AccumulatedImpulse.Z); + data.AccumulatedImpulse.Z = MathR.Min(0, data.AccumulatedImpulse.Z); } else if (data.Clamp == 2) { - data.AccumulatedImpulse.Z = MathF.Max(0, data.AccumulatedImpulse.Z); + data.AccumulatedImpulse.Z = MathR.Max(0, data.AccumulatedImpulse.Z); } else { diff --git a/src/Jitter2/Dynamics/Constraints/Internal/QMatrix.cs b/src/Jitter2/Dynamics/Constraints/Internal/QMatrix.cs index 78d818b0..8a12323b 100644 --- a/src/Jitter2/Dynamics/Constraints/Internal/QMatrix.cs +++ b/src/Jitter2/Dynamics/Constraints/Internal/QMatrix.cs @@ -31,20 +31,20 @@ namespace Jitter2.Dynamics.Constraints; internal unsafe struct QMatrix { - private MemoryHelper.MemBlock64 mem; + private MemoryHelper.MemBlock16Real mem; - public float* Pointer => (float*)Unsafe.AsPointer(ref mem); + public Real* Pointer => (Real*)Unsafe.AsPointer(ref mem); - private static QMatrix Multiply(float* left, float* right) + private static QMatrix Multiply(Real* left, Real* right) { Unsafe.SkipInit(out QMatrix res); - float* result = res.Pointer; + Real* result = res.Pointer; for (int c = 0; c < 4; c++) { for (int r = 0; r < 4; r++) { - float* tt = &result[4 * c + r]; + Real* tt = &result[4 * c + r]; *tt = 0; for (int k = 0; k < 4; k++) { @@ -56,26 +56,9 @@ private static QMatrix Multiply(float* left, float* right) return res; } - public static JMatrix ProjectMultiplyLeftRight(in JQuaternion left, in JQuaternion right) - { - Unsafe.SkipInit(out JMatrix res); - - res.M11 = -left.X * right.X + left.W * right.W + left.Z * right.Z + left.Y * right.Y; - res.M12 = -left.X * right.Y + left.W * right.Z - left.Z * right.W - left.Y * right.X; - res.M13 = -left.X * right.Z - left.W * right.Y - left.Z * right.X + left.Y * right.W; - res.M21 = -left.Y * right.X + left.Z * right.W - left.W * right.Z - left.X * right.Y; - res.M22 = -left.Y * right.Y + left.Z * right.Z + left.W * right.W + left.X * right.X; - res.M23 = -left.Y * right.Z - left.Z * right.Y + left.W * right.X - left.X * right.W; - res.M31 = -left.Z * right.X - left.Y * right.W - left.X * right.Z + left.W * right.Y; - res.M32 = -left.Z * right.Y - left.Y * right.Z + left.X * right.W - left.W * right.X; - res.M33 = -left.Z * right.Z + left.Y * right.Y + left.X * right.X + left.W * right.W; - - return res; - } - public JMatrix Projection() { - float* m = Pointer; + Real* m = Pointer; return new JMatrix(m[0x5], m[0x9], m[0xD], m[0x6], m[0xA], m[0xE], @@ -85,7 +68,7 @@ public JMatrix Projection() public static QMatrix CreateLM(in JQuaternion quat) { Unsafe.SkipInit(out QMatrix result); - float* q = result.Pointer; + Real* q = result.Pointer; q[0x0] = +quat.W; q[0x4] = -quat.X; @@ -110,7 +93,7 @@ public static QMatrix CreateLM(in JQuaternion quat) public static QMatrix CreateRM(in JQuaternion quat) { Unsafe.SkipInit(out QMatrix result); - float* q = result.Pointer; + Real* q = result.Pointer; q[0x0] = +quat.W; q[0x4] = -quat.X; @@ -138,8 +121,25 @@ public static QMatrix Multiply(ref QMatrix left, ref QMatrix right) { fixed (QMatrix* rptr = &right) { - return Multiply((float*)lptr, (float*)rptr); + return Multiply((Real*)lptr, (Real*)rptr); } } } + + public static JMatrix ProjectMultiplyLeftRight(in JQuaternion left, in JQuaternion right) + { + Unsafe.SkipInit(out JMatrix res); + + res.M11 = -left.X * right.X + left.W * right.W + left.Z * right.Z + left.Y * right.Y; + res.M12 = -left.X * right.Y + left.W * right.Z - left.Z * right.W - left.Y * right.X; + res.M13 = -left.X * right.Z - left.W * right.Y - left.Z * right.X + left.Y * right.W; + res.M21 = -left.Y * right.X + left.Z * right.W - left.W * right.Z - left.X * right.Y; + res.M22 = -left.Y * right.Y + left.Z * right.Z + left.W * right.W + left.X * right.X; + res.M23 = -left.Y * right.Z - left.Z * right.Y + left.W * right.X - left.X * right.W; + res.M31 = -left.Z * right.X - left.Y * right.W - left.X * right.Z + left.W * right.Y; + res.M32 = -left.Z * right.Y - left.Y * right.Z + left.X * right.W - left.W * right.X; + res.M33 = -left.Z * right.Z + left.Y * right.Y + left.X * right.X + left.W * right.W; + + return res; + } } \ No newline at end of file diff --git a/src/Jitter2/Dynamics/Constraints/Limit.cs b/src/Jitter2/Dynamics/Constraints/Limit.cs index c2672d9d..29b2a7f9 100644 --- a/src/Jitter2/Dynamics/Constraints/Limit.cs +++ b/src/Jitter2/Dynamics/Constraints/Limit.cs @@ -32,12 +32,12 @@ public struct AngularLimit public JAngle To { get; set; } public static readonly AngularLimit Full = - new(JAngle.FromRadiant(-MathF.PI), JAngle.FromRadiant(MathF.PI)); + new(JAngle.FromRadiant(-MathR.PI), JAngle.FromRadiant(MathR.PI)); public static readonly AngularLimit Fixed = - new(JAngle.FromRadiant(+1e-6f), JAngle.FromRadiant(-1e-6f)); + new(JAngle.FromRadiant(+(Real)1e-6), JAngle.FromRadiant(-(Real)1e-6)); - public static AngularLimit FromDegree(float min, float max) + public static AngularLimit FromDegree(Real min, Real max) { return new AngularLimit(JAngle.FromDegree(min), JAngle.FromDegree(max)); } @@ -57,27 +57,27 @@ public readonly void Deconstruct(out JAngle limitMin, out JAngle limitMax) public struct LinearLimit { - public float From { get; set; } - public float To { get; set; } + public Real From { get; set; } + public Real To { get; set; } public static readonly LinearLimit Full = - new(float.NegativeInfinity, float.PositiveInfinity); + new(Real.NegativeInfinity, Real.PositiveInfinity); public static readonly LinearLimit Fixed = - new(1e-6f, -1e-6f); + new((Real)1e-6, -(Real)1e-6); - public LinearLimit(float from, float to) + public LinearLimit(Real from, Real to) { From = from; To = to; } - public static LinearLimit FromMinMax(float min, float max) + public static LinearLimit FromMinMax(Real min, Real max) { return new LinearLimit(min, max); } - public readonly void Deconstruct(out float limitMin, out float limitMax) + public readonly void Deconstruct(out Real limitMin, out Real limitMax) { limitMin = From; limitMax = To; diff --git a/src/Jitter2/Dynamics/Constraints/LinearMotor.cs b/src/Jitter2/Dynamics/Constraints/LinearMotor.cs index f55e657d..26b6409a 100644 --- a/src/Jitter2/Dynamics/Constraints/LinearMotor.cs +++ b/src/Jitter2/Dynamics/Constraints/LinearMotor.cs @@ -41,7 +41,7 @@ public struct LinearMotorData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -49,13 +49,13 @@ public struct LinearMotorData public JVector LocalAxis1; public JVector LocalAxis2; - public float Velocity; - public float MaxForce; - public float MaxLambda; + public Real Velocity; + public Real MaxForce; + public Real MaxLambda; - public float EffectiveMass; + public Real EffectiveMass; - public float AccumulatedImpulse; + public Real AccumulatedImpulse; } private JHandle handle; @@ -101,18 +101,18 @@ public void Initialize(JVector axis1, JVector axis2) data.Velocity = 0; } - public float TargetVelocity + public Real TargetVelocity { get => handle.Data.Velocity; set => handle.Data.Velocity = value; } - public float MaximumForce + public Real MaximumForce { get => handle.Data.MaxForce; set { - if (value < 0.0f) + if (value < (Real)0.0) { throw new ArgumentException("Maximum force must not be negative."); } @@ -121,9 +121,9 @@ public float MaximumForce } } - public float Impulse => handle.Data.AccumulatedImpulse; + public Real Impulse => handle.Data.AccumulatedImpulse; - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref LinearMotorData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); @@ -134,8 +134,8 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) JVector.Transform(data.LocalAxis2, body2.Orientation, out JVector j2); data.EffectiveMass = body1.InverseMass + body2.InverseMass; - data.EffectiveMass = 1.0f / data.EffectiveMass; - data.MaxLambda = (1.0f / idt) * data.MaxForce; + data.EffectiveMass = (Real)1.0 / data.EffectiveMass; + data.MaxLambda = ((Real)1.0 / idt) * data.MaxForce; body1.Velocity -= j1 * data.AccumulatedImpulse * body1.InverseMass; body2.Velocity += j2 * data.AccumulatedImpulse * body2.InverseMass; @@ -149,7 +149,7 @@ public override void DebugDraw(IDebugDrawer drawer) ref RigidBodyData body2 = ref data.Body2.Data; } - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref LinearMotorData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -158,11 +158,11 @@ public static void Iterate(ref ConstraintData constraint, float idt) JVector.Transform(data.LocalAxis1, body1.Orientation, out JVector j1); JVector.Transform(data.LocalAxis2, body2.Orientation, out JVector j2); - float jv = -j1 * body1.Velocity + j2 * body2.Velocity; + Real jv = -j1 * body1.Velocity + j2 * body2.Velocity; - float lambda = -(jv - data.Velocity) * data.EffectiveMass; + Real lambda = -(jv - data.Velocity) * data.EffectiveMass; - float olda = data.AccumulatedImpulse; + Real olda = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; diff --git a/src/Jitter2/Dynamics/Constraints/PointOnLine.cs b/src/Jitter2/Dynamics/Constraints/PointOnLine.cs index 2e2a04ac..451036e3 100644 --- a/src/Jitter2/Dynamics/Constraints/PointOnLine.cs +++ b/src/Jitter2/Dynamics/Constraints/PointOnLine.cs @@ -43,7 +43,7 @@ public struct PointOnLineData internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -53,17 +53,17 @@ public struct PointOnLineData public JVector LocalAnchor1; public JVector LocalAnchor2; - public float BiasFactor; - public float LimitBias; - public float Softness; - public float LimitSoftness; + public Real BiasFactor; + public Real LimitBias; + public Real Softness; + public Real LimitSoftness; public JMatrix EffectiveMass; public JVector AccumulatedImpulse; public JVector Bias; - public float Min; - public float Max; + public Real Min; + public Real Max; public ushort Clamp; @@ -109,15 +109,15 @@ public void Initialize(JVector axis, JVector anchor1, JVector anchor2, LinearLim JVector.ConjugatedTransform(axis, body1.Orientation, out data.LocalAxis); - data.BiasFactor = 0.01f; - data.Softness = 0.00001f; - data.LimitSoftness = 0.0001f; - data.LimitBias = 0.2f; + data.BiasFactor = (Real)0.01; + data.Softness = (Real)0.00001; + data.LimitSoftness = (Real)0.0001; + data.LimitBias = (Real)0.2; (data.Min, data.Max) = limit; } - public float Distance + public Real Distance { get { @@ -140,7 +140,7 @@ public float Distance } [System.Runtime.CompilerServices.SkipLocalsInit] - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref PointOnLineData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref data.Body1.Data; @@ -252,13 +252,13 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) body2.AngularVelocity += JVector.Transform(jacobian[3] * acc.X + jacobian[7] * acc.Y + jacobian[11] * acc.Z, body2.InverseInertiaWorld); } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; @@ -266,20 +266,20 @@ public float Bias public JVector Impulse => handle.Data.AccumulatedImpulse; - public float LimitSoftness + public Real LimitSoftness { get => handle.Data.LimitSoftness; set => handle.Data.LimitSoftness = value; } - public float LimitBias + public Real LimitBias { get => handle.Data.LimitBias; set => handle.Data.LimitBias = value; } [System.Runtime.CompilerServices.SkipLocalsInit] - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref PointOnLineData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -332,20 +332,20 @@ public static void Iterate(ref ConstraintData constraint, float idt) softnessVector.Y *= data.Softness; softnessVector.Z *= data.LimitSoftness; - JVector lambda = -1.0f * JVector.Transform(jv + data.Bias + softnessVector, data.EffectiveMass); + JVector lambda = -(Real)1.0 * JVector.Transform(jv + data.Bias + softnessVector, data.EffectiveMass); JVector origAcc = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; if ((data.Clamp & 1) != 0) - data.AccumulatedImpulse.Z = MathF.Min(data.AccumulatedImpulse.Z, 0.0f); + data.AccumulatedImpulse.Z = MathR.Min(data.AccumulatedImpulse.Z, (Real)0.0); else if ((data.Clamp & 2) != 0) - data.AccumulatedImpulse.Z = MathF.Max(data.AccumulatedImpulse.Z, 0.0f); + data.AccumulatedImpulse.Z = MathR.Max(data.AccumulatedImpulse.Z, (Real)0.0); else { - data.AccumulatedImpulse.Z = 0.0f; - origAcc.Z = 0.0f; + data.AccumulatedImpulse.Z = (Real)0.0; + origAcc.Z = (Real)0.0; } lambda = data.AccumulatedImpulse - origAcc; diff --git a/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs b/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs index d34c34b8..d72361a0 100644 --- a/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs +++ b/src/Jitter2/Dynamics/Constraints/PointOnPlane.cs @@ -43,7 +43,7 @@ public struct SliderData internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -53,19 +53,19 @@ public struct SliderData public JVector LocalAnchor1; public JVector LocalAnchor2; - public float BiasFactor; - public float Softness; + public Real BiasFactor; + public Real Softness; - public float EffectiveMass; - public float AccumulatedImpulse; - public float Bias; + public Real EffectiveMass; + public Real AccumulatedImpulse; + public Real Bias; - public float Min; - public float Max; + public Real Min; + public Real Max; public ushort Clamp; - public MemoryHelper.MemBlock48 J0; + public MemoryHelper.MemBlock12Real J0; } private JHandle handle; @@ -107,13 +107,13 @@ public void Initialize(JVector axis, JVector anchor1, JVector anchor2, LinearLim JVector.ConjugatedTransform(axis, body1.Orientation, out data.LocalAxis); - data.BiasFactor = 0.01f; - data.Softness = 0.00001f; + data.BiasFactor = (Real)0.01; + data.Softness = (Real)0.00001; (data.Min, data.Max) = limit; } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref SliderData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref data.Body1.Data; @@ -138,9 +138,9 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) jacobian[2] = axis; jacobian[3] = R2 % axis; - float error = JVector.Dot(U, axis); + Real error = JVector.Dot(U, axis); - data.EffectiveMass = 1.0f; + data.EffectiveMass = (Real)1.0; if (error > data.Max) { @@ -163,11 +163,11 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) JVector.Transform(jacobian[3], body2.InverseInertiaWorld) * jacobian[3]; data.EffectiveMass += (data.Softness * idt); - data.EffectiveMass = 1.0f / data.EffectiveMass; + data.EffectiveMass = (Real)1.0 / data.EffectiveMass; data.Bias = error * data.BiasFactor * idt; - float acc = data.AccumulatedImpulse; + Real acc = data.AccumulatedImpulse; body1.Velocity += body1.InverseMass * (jacobian[0] * acc); body1.AngularVelocity += JVector.Transform(jacobian[1] * acc, body1.InverseInertiaWorld); @@ -176,21 +176,21 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) body2.AngularVelocity += JVector.Transform(jacobian[3] * acc, body2.InverseInertiaWorld); } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; } - public float Impulse => handle.Data.AccumulatedImpulse; + public Real Impulse => handle.Data.AccumulatedImpulse; - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref SliderData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -200,24 +200,24 @@ public static void Iterate(ref ConstraintData constraint, float idt) var jacobian = new Span(Unsafe.AsPointer(ref data.J0), 4); - float jv = jacobian[0] * body1.Velocity + jacobian[1] * body1.AngularVelocity + jacobian[2] * body2.Velocity + + Real jv = jacobian[0] * body1.Velocity + jacobian[1] * body1.AngularVelocity + jacobian[2] * body2.Velocity + jacobian[3] * body2.AngularVelocity; - float softness = data.AccumulatedImpulse * data.Softness * idt; + Real softness = data.AccumulatedImpulse * data.Softness * idt; - float lambda = -1.0f * (jv + data.Bias + softness) * data.EffectiveMass; + Real lambda = -(Real)1.0 * (jv + data.Bias + softness) * data.EffectiveMass; - float origAcc = data.AccumulatedImpulse; + Real origAcc = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; if (data.Clamp == 1) { - data.AccumulatedImpulse = MathF.Min(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Min(data.AccumulatedImpulse, (Real)0.0); } else { - data.AccumulatedImpulse = MathF.Max(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Max(data.AccumulatedImpulse, (Real)0.0); } lambda = data.AccumulatedImpulse - origAcc; diff --git a/src/Jitter2/Dynamics/Constraints/TwistAngle.cs b/src/Jitter2/Dynamics/Constraints/TwistAngle.cs index a43419f0..2d5b6a7f 100644 --- a/src/Jitter2/Dynamics/Constraints/TwistAngle.cs +++ b/src/Jitter2/Dynamics/Constraints/TwistAngle.cs @@ -41,7 +41,7 @@ public struct TwistLimitData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -50,15 +50,15 @@ public struct TwistLimitData public JQuaternion Q0; - public float Angle1, Angle2; + public Real Angle1, Angle2; public ushort Clamp; - public float BiasFactor; - public float Softness; + public Real BiasFactor; + public Real Softness; - public float EffectiveMass; - public float AccumulatedImpulse; - public float Bias; + public Real EffectiveMass; + public Real AccumulatedImpulse; + public Real Bias; public JVector Jacobian; } @@ -85,14 +85,14 @@ public void Initialize(JVector axis1, JVector axis2, AngularLimit limit) ref RigidBodyData body1 = ref data.Body1.Data; ref RigidBodyData body2 = ref data.Body2.Data; - data.Softness = 0.0001f; - data.BiasFactor = 0.2f; + data.Softness = (Real)0.0001; + data.BiasFactor = (Real)0.2; axis1.Normalize(); axis2.Normalize(); - data.Angle1 = MathF.Sin((float)limit.From / 2.0f); - data.Angle2 = MathF.Sin((float)limit.To / 2.0f); + data.Angle1 = MathR.Sin((Real)limit.From / (Real)2.0); + data.Angle2 = MathR.Sin((Real)limit.To / (Real)2.0); data.B = JVector.TransposedTransform(axis2, body2.Orientation); @@ -107,8 +107,8 @@ public AngularLimit Limit set { ref TwistLimitData data = ref handle.Data; - data.Angle1 = MathF.Sin((float)value.From / 2.0f); - data.Angle2 = MathF.Sin((float)value.To / 2.0f); + data.Angle1 = MathR.Sin((Real)value.From / (Real)2.0); + data.Angle2 = MathR.Sin((Real)value.To / (Real)2.0); } } @@ -122,7 +122,7 @@ public void Initialize(JVector axis1, JVector axis2) Initialize(axis1, axis2, AngularLimit.Fixed); } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref TwistLimitData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); @@ -132,7 +132,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) JQuaternion q1 = body1.Orientation; JQuaternion q2 = body2.Orientation; - JMatrix m = (-1.0f / 2.0f) * QMatrix.ProjectMultiplyLeftRight(data.Q0 * q1.Conjugate(), q2); + JMatrix m = (-(Real)(1.0 / 2.0)) * QMatrix.ProjectMultiplyLeftRight(data.Q0 * q1.Conjugate(), q2); JQuaternion q = data.Q0 * q1.Conjugate() * q2; @@ -142,13 +142,13 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) data.EffectiveMass += (data.Softness * idt); - data.EffectiveMass = 1.0f / data.EffectiveMass; + data.EffectiveMass = (Real)1.0 / data.EffectiveMass; - float error = JVector.Dot(data.B, new JVector(q.X, q.Y, q.Z)); + Real error = JVector.Dot(data.B, new JVector(q.X, q.Y, q.Z)); - if (q.W < 0.0f) + if (q.W < (Real)0.0) { - error *= -1.0f; + error *= -(Real)1.0; data.Jacobian *= -1; } @@ -166,7 +166,7 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) } else { - data.AccumulatedImpulse = 0.0f; + data.AccumulatedImpulse = (Real)0.0; return; } @@ -186,29 +186,29 @@ public JAngle Angle JQuaternion quat0 = data.Q0 * q1.Conjugate() * q2; - if (quat0.W < 0.0f) + if (quat0.W < (Real)0.0) { - quat0 *= -1.0f; + quat0 *= -(Real)1.0; } - float error = JVector.Dot(data.B, new JVector(quat0.X, quat0.Y, quat0.Z)); - return (JAngle)(2.0f * MathF.Asin(error)); + Real error = JVector.Dot(data.B, new JVector(quat0.X, quat0.Y, quat0.Z)); + return (JAngle)((Real)2.0 * MathR.Asin(error)); } } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; } - public float Impulse => handle.Data.AccumulatedImpulse; + public Real Impulse => handle.Data.AccumulatedImpulse; public override void DebugDraw(IDebugDrawer drawer) { @@ -218,7 +218,7 @@ public override void DebugDraw(IDebugDrawer drawer) ref RigidBodyData body2 = ref data.Body2.Data; } - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref TwistLimitData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; @@ -226,22 +226,22 @@ public static void Iterate(ref ConstraintData constraint, float idt) if (data.Clamp == 0) return; - float jv = (body1.AngularVelocity - body2.AngularVelocity) * data.Jacobian; + Real jv = (body1.AngularVelocity - body2.AngularVelocity) * data.Jacobian; - float softnessScalar = data.AccumulatedImpulse * (data.Softness * idt); + Real softnessScalar = data.AccumulatedImpulse * (data.Softness * idt); - float lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); + Real lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); - float origAcc = data.AccumulatedImpulse; + Real origAcc = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; if (data.Clamp == 1) { - data.AccumulatedImpulse = MathF.Min(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Min(data.AccumulatedImpulse, (Real)0.0); } else { - data.AccumulatedImpulse = MathF.Max(data.AccumulatedImpulse, 0.0f); + data.AccumulatedImpulse = MathR.Max(data.AccumulatedImpulse, (Real)0.0); } lambda = data.AccumulatedImpulse - origAcc; diff --git a/src/Jitter2/Dynamics/Contact.cs b/src/Jitter2/Dynamics/Contact.cs index 864f06b9..68313275 100644 --- a/src/Jitter2/Dynamics/Contact.cs +++ b/src/Jitter2/Dynamics/Contact.cs @@ -59,7 +59,7 @@ public struct ContactData /// A sphere may slide down a ramp. Within one timestep Jitter may detect the collision, create the contact, /// solve the contact, integrate velocities and positions and then consider the contact as broken, since the /// movement orthogonal to the contact normal exceeds a threshold. This results in no intact contact before calling - /// and no intact contact after the call. However, the correspondig bit for the + /// and no intact contact after the call. However, the corresponding bit for the /// solver-phase will be set in this scenario. /// public uint UsageMask; @@ -69,8 +69,8 @@ public struct ContactData public ArbiterKey Key; - public float Restitution; - public float Friction; + public Real Restitution; + public Real Friction; public bool IsSpeculative; @@ -79,11 +79,11 @@ public struct ContactData public Contact Contact2; public Contact Contact3; - public unsafe void PrepareForIteration(float dt) + public unsafe void PrepareForIteration(Real dt) { var ptr = (ContactData*)Unsafe.AsPointer(ref this); - if (Vector128.IsHardwareAccelerated) + if (Vector.IsHardwareAccelerated) { if ((UsageMask & MaskContact0) != 0) Contact0.PrepareForIterationAccelerated(ptr, dt); if ((UsageMask & MaskContact1) != 0) Contact1.PrepareForIterationAccelerated(ptr, dt); @@ -103,7 +103,7 @@ public unsafe void Iterate(bool applyBias) { var ptr = (ContactData*)Unsafe.AsPointer(ref this); - if (Vector128.IsHardwareAccelerated) + if (Vector.IsHardwareAccelerated) { if ((UsageMask & MaskContact0) != 0) Contact0.IterateAccelerated(ptr, applyBias); if ((UsageMask & MaskContact1) != 0) Contact1.IterateAccelerated(ptr, applyBias); @@ -119,6 +119,12 @@ public unsafe void Iterate(bool applyBias) } } + /// + /// Gets a value indicating whether the current system supports hardware acceleration + /// for SIMD (Single Instruction, Multiple Data) operations. + /// + public static bool IsHardwareAccelerated => Vector.IsHardwareAccelerated; + public unsafe void UpdatePosition() { UsageMask &= MaskContactAll; @@ -152,8 +158,8 @@ public void Init(RigidBody body1, RigidBody body2) Body1 = body1.handle; Body2 = body2.handle; - Friction = MathF.Max(body1.Friction, body2.Friction); - Restitution = MathF.Max(body1.Restitution, body2.Restitution); + Friction = MathR.Max(body1.Friction, body2.Friction); + Restitution = MathR.Max(body1.Restitution, body2.Restitution); UsageMask = 0; } @@ -184,7 +190,7 @@ public void Init(RigidBody body1, RigidBody body2) /// /// Adds a new collision result to the contact manifold. Keeps at most four points. /// - public unsafe void AddContact(in JVector point1, in JVector point2, in JVector normal, float penetration) + public unsafe void AddContact(in JVector point1, in JVector point2, in JVector normal, Real penetration) { if ((UsageMask & MaskContactAll) == MaskContactAll) { @@ -197,13 +203,13 @@ public unsafe void AddContact(in JVector point1, in JVector point2, in JVector n // to an already existing point. Replace this point by the new one. Contact* closest = (Contact*)IntPtr.Zero; - float distanceSq = float.MaxValue; + Real distanceSq = Real.MaxValue; JVector relP1 = point1 - Body1.Data.Position; if ((UsageMask & MaskContact0) != 0) { - float distSq = (Contact0.RelativePosition1 - relP1).LengthSquared(); + Real distSq = (Contact0.RelativePosition1 - relP1).LengthSquared(); if (distSq < distanceSq) { distanceSq = distSq; @@ -213,7 +219,7 @@ public unsafe void AddContact(in JVector point1, in JVector point2, in JVector n if ((UsageMask & MaskContact1) != 0) { - float distSq = (Contact1.RelativePosition1 - relP1).LengthSquared(); + Real distSq = (Contact1.RelativePosition1 - relP1).LengthSquared(); if (distSq < distanceSq) { distanceSq = distSq; @@ -223,7 +229,7 @@ public unsafe void AddContact(in JVector point1, in JVector point2, in JVector n if ((UsageMask & MaskContact2) != 0) { - float distSq = (Contact2.RelativePosition1 - relP1).LengthSquared(); + Real distSq = (Contact2.RelativePosition1 - relP1).LengthSquared(); if (distSq < distanceSq) { distanceSq = distSq; @@ -233,7 +239,7 @@ public unsafe void AddContact(in JVector point1, in JVector point2, in JVector n if ((UsageMask & MaskContact3) != 0) { - float distSq = (Contact3.RelativePosition1 - relP1).LengthSquared(); + Real distSq = (Contact3.RelativePosition1 - relP1).LengthSquared(); if (distSq < distanceSq) { distanceSq = distSq; @@ -271,7 +277,7 @@ public unsafe void AddContact(in JVector point1, in JVector point2, in JVector n } } - private static float CalcArea4Points(in JVector p0, in JVector p1, in JVector p2, in JVector p3) + private static Real CalcArea4Points(in JVector p0, in JVector p1, in JVector p2, in JVector p3) { JVector a0 = p0 - p1; JVector a1 = p0 - p2; @@ -284,26 +290,26 @@ private static float CalcArea4Points(in JVector p0, in JVector p1, in JVector p2 JVector tmp1 = a1 % b1; JVector tmp2 = a2 % b2; - return MathF.Max(MathF.Max(tmp0.LengthSquared(), tmp1.LengthSquared()), tmp2.LengthSquared()); + return MathR.Max(MathR.Max(tmp0.LengthSquared(), tmp1.LengthSquared()), tmp2.LengthSquared()); } - private void SortCachedPoints(in JVector point1, in JVector point2, in JVector normal, float penetration) + private void SortCachedPoints(in JVector point1, in JVector point2, in JVector normal, Real penetration) { JVector.Subtract(point1, Body1.Data.Position, out JVector rp1); // calculate 4 possible cases areas, and take the biggest area // int maxPenetrationIndex = -1; - // float maxPenetration = penetration; + // Real maxPenetration = penetration; // always prefer the new point - const float epsilon = -0.0001f; + const Real epsilon = -(Real)0.0001; - float biggestArea = 0; + Real biggestArea = 0; ref Contact cref = ref Contact0; uint index = 0; - float clsq = CalcArea4Points(rp1, Contact1.RelativePosition1, Contact2.RelativePosition1, Contact3.RelativePosition1); + Real clsq = CalcArea4Points(rp1, Contact1.RelativePosition1, Contact2.RelativePosition1, Contact3.RelativePosition1); if (clsq > biggestArea + epsilon) { @@ -347,10 +353,10 @@ private void SortCachedPoints(in JVector point1, in JVector point2, in JVector n [StructLayout(LayoutKind.Explicit)] public struct Contact { - public const float MaximumBias = 100.0f; - public const float BiasFactor = 0.2f; - public const float AllowedPenetration = 0.01f; - public const float BreakThreshold = 0.02f; + public const Real MaximumBias = (Real)100.0; + public const Real BiasFactor = (Real)0.2; + public const Real AllowedPenetration = (Real)0.01; + public const Real BreakThreshold = (Real)0.02; [Flags] public enum Flags @@ -362,45 +368,45 @@ public enum Flags public Flags Flag; [FieldOffset(4)] - public float Bias; + public Real Bias; - [FieldOffset(8)] - public float PenaltyBias; + [FieldOffset(4+1*sizeof(Real))] + public Real PenaltyBias; - [FieldOffset(12)] - public float Penetration; + [FieldOffset(4+2*sizeof(Real))] + public Real Penetration; - [FieldOffset(16)] - internal Vector128 NormalTangentX; + [FieldOffset(4+3*sizeof(Real))] + internal VectorReal NormalTangentX; - [FieldOffset(28)] - internal Vector128 NormalTangentY; + [FieldOffset(4+6*sizeof(Real))] + internal VectorReal NormalTangentY; - [FieldOffset(40)] - internal Vector128 NormalTangentZ; + [FieldOffset(4+9*sizeof(Real))] + internal VectorReal NormalTangentZ; - [FieldOffset(52)] - internal Vector128 MassNormalTangent; + [FieldOffset(4+12*sizeof(Real))] + internal VectorReal MassNormalTangent; - [FieldOffset(64)] - internal Vector128 Accumulated; + [FieldOffset(4+15*sizeof(Real))] + internal VectorReal Accumulated; - [FieldOffset(80)] + [FieldOffset(4+19*sizeof(Real))] [ReferenceFrame(ReferenceFrame.Local)] internal JVector Position1; - [FieldOffset(92)] + [FieldOffset(4+22*sizeof(Real))] [ReferenceFrame(ReferenceFrame.Local)] internal JVector Position2; /// /// Position of the contact relative to the center of mass on the first body. /// - [FieldOffset(104)] + [FieldOffset(4+25*sizeof(Real))] [ReferenceFrame(ReferenceFrame.World)] public JVector RelativePosition1; /// /// Position of the contact relative to the center of mass on the second body. /// - [FieldOffset(116)] + [FieldOffset(4+28*sizeof(Real))] [ReferenceFrame(ReferenceFrame.World)] public JVector RelativePosition2; [ReferenceFrame(ReferenceFrame.World)] public JVector Normal => new JVector(NormalTangentX.GetElement(0), NormalTangentY.GetElement(0), NormalTangentZ.GetElement(0)); @@ -409,16 +415,16 @@ public enum Flags [ReferenceFrame(ReferenceFrame.World)] public JVector Tangent2 => new JVector(NormalTangentX.GetElement(2), NormalTangentY.GetElement(2), NormalTangentZ.GetElement(2)); - public float Impulse => Accumulated.GetElement(0); + public Real Impulse => Accumulated.GetElement(0); - public float TangentImpulse1 => Accumulated.GetElement(1); + public Real TangentImpulse1 => Accumulated.GetElement(1); - public float TangentImpulse2 => Accumulated.GetElement(2); + public Real TangentImpulse2 => Accumulated.GetElement(2); public void Initialize(ref RigidBodyData b1, ref RigidBodyData b2, in JVector point1, in JVector point2, in JVector n, - float penetration, bool newContact, float restitution) + Real penetration, bool newContact, Real restitution) { - Debug.Assert(Math.Abs(n.LengthSquared() - 1.0f) < 1e-3); + Debug.Assert(Math.Abs(n.LengthSquared() - (Real)1.0) < 1e-3); JVector.Subtract(point1, b1.Position, out RelativePosition1); JVector.Subtract(point2, b2.Position, out RelativePosition2); @@ -431,28 +437,28 @@ public void Initialize(ref RigidBodyData b1, ref RigidBodyData b2, in JVector po if (!newContact) return; Flag = Flags.NewContact; - Accumulated = Vector128.Create(0.0f); + Accumulated = Vector.Create((Real)0.0); JVector dv = b2.Velocity + b2.AngularVelocity % RelativePosition2; dv -= b1.Velocity + b1.AngularVelocity % RelativePosition1; - float relNormalVel = JVector.Dot(dv, n); + Real relNormalVel = JVector.Dot(dv, n); Bias = 0; // Fake restitution - if (relNormalVel < -1.0f) + if (relNormalVel < (Real)(-1.0)) { Bias = -restitution * relNormalVel; } var tangent1 = dv - n * relNormalVel; - float num = tangent1.LengthSquared(); + Real num = tangent1.LengthSquared(); - if (num > 1e-12f) + if (num > (Real)1e-12) { - num = 1.0f / MathF.Sqrt(num); + num = (Real)1.0 / MathR.Sqrt(num); tangent1 *= num; } else @@ -462,9 +468,9 @@ public void Initialize(ref RigidBodyData b1, ref RigidBodyData b2, in JVector po var tangent2 = tangent1 % n; - NormalTangentX = Vector128.Create(n.X, tangent1.X, tangent2.X, 0); - NormalTangentY = Vector128.Create(n.Y, tangent1.Y, tangent2.Y, 0); - NormalTangentZ = Vector128.Create(n.Z, tangent1.Z, tangent2.Z, 0); + NormalTangentX = Vector.Create(n.X, tangent1.X, tangent2.X, 0); + NormalTangentY = Vector.Create(n.Y, tangent1.Y, tangent2.Y, 0); + NormalTangentZ = Vector.Create(n.Z, tangent1.Z, tangent2.Z, 0); } public unsafe bool UpdatePosition(ContactData* cd) @@ -487,13 +493,13 @@ public unsafe bool UpdatePosition(ContactData* cd) Penetration = JVector.Dot(dist, n); - if (Penetration < -BreakThreshold * 0.1f) + if (Penetration < -BreakThreshold * (Real)0.1) { return false; } dist -= Penetration * n; - float tangentialOffsetSq = dist.LengthSquared(); + Real tangentialOffsetSq = dist.LengthSquared(); if (tangentialOffsetSq > BreakThreshold * BreakThreshold) { @@ -504,15 +510,15 @@ public unsafe bool UpdatePosition(ContactData* cd) } // Fallback for missing hardware acceleration - #region public unsafe void PrepareForIteration(ContactData* cd, float idt) - public unsafe void PrepareForIteration(ContactData* cd, float idt) + #region public unsafe void PrepareForIteration(ContactData* cd, Real idt) + public unsafe void PrepareForIteration(ContactData* cd, Real idt) { ref var b1 = ref cd->Body1.Data; ref var b2 = ref cd->Body2.Data; - float accumulatedNormalImpulse = Accumulated.GetElement(0); - float accumulatedTangentImpulse1 = Accumulated.GetElement(1); - float accumulatedTangentImpulse2 = Accumulated.GetElement(2); + Real accumulatedNormalImpulse = Accumulated.GetElement(0); + Real accumulatedTangentImpulse1 = Accumulated.GetElement(1); + Real accumulatedTangentImpulse2 = Accumulated.GetElement(2); var normal = new JVector(NormalTangentX.GetElement(0), NormalTangentY.GetElement(0), NormalTangentZ.GetElement(0)); var tangent1 = new JVector(NormalTangentX.GetElement(1), NormalTangentY.GetElement(1), NormalTangentZ.GetElement(1)); @@ -524,11 +530,11 @@ public unsafe void PrepareForIteration(ContactData* cd, float idt) JVector.Add(RelativePosition2, b2.Position, out JVector p2); JVector.Subtract(p1, p2, out JVector dist); - float inverseMass = b1.InverseMass + b2.InverseMass; + Real inverseMass = b1.InverseMass + b2.InverseMass; - float kTangent1 = inverseMass; - float kTangent2 = inverseMass; - float kNormal = inverseMass; + Real kTangent1 = inverseMass; + Real kTangent2 = inverseMass; + Real kNormal = inverseMass; if (!cd->IsSpeculative) { @@ -578,12 +584,12 @@ public unsafe void PrepareForIteration(ContactData* cd, float idt) accumulatedTangentImpulse2 * mTt2; } - float massTangent1 = 1.0f / kTangent1; - float massTangent2 = 1.0f / kTangent2; - float massNormal = 1.0f / kNormal; + Real massTangent1 = (Real)1.0 / kTangent1; + Real massTangent2 = (Real)1.0 / kTangent2; + Real massNormal = (Real)1.0 / kNormal; JVector mass = new JVector(massNormal, massTangent1, massTangent2); - Unsafe.CopyBlock(Unsafe.AsPointer(ref MassNormalTangent), Unsafe.AsPointer(ref mass), 12); + Unsafe.CopyBlock(Unsafe.AsPointer(ref MassNormalTangent), Unsafe.AsPointer(ref mass), 3*sizeof(Real)); if ((Flag & Flags.NewContact) == 0) { @@ -595,8 +601,8 @@ public unsafe void PrepareForIteration(ContactData* cd, float idt) Bias = Penetration * idt; } - PenaltyBias = BiasFactor * idt * Math.Max(0.0f, Penetration - AllowedPenetration); - PenaltyBias = Math.Clamp(PenaltyBias, 0.0f, MaximumBias); + PenaltyBias = BiasFactor * idt * Math.Max((Real)0.0, Penetration - AllowedPenetration); + PenaltyBias = Math.Clamp(PenaltyBias, (Real)0.0, MaximumBias); JVector impulse = normal * accumulatedNormalImpulse + tangent1 * accumulatedTangentImpulse1 + @@ -616,12 +622,12 @@ public unsafe void Iterate(ContactData* cd, bool applyBias) ref var b1 = ref cd->Body1.Data; ref var b2 = ref cd->Body2.Data; - float massNormal = MassNormalTangent.GetElement(0); - float massTangent1 = MassNormalTangent.GetElement(1); - float massTangent2 = MassNormalTangent.GetElement(2); - float accumulatedNormalImpulse = Accumulated.GetElement(0); - float accumulatedTangentImpulse1 = Accumulated.GetElement(1); - float accumulatedTangentImpulse2 = Accumulated.GetElement(2); + Real massNormal = MassNormalTangent.GetElement(0); + Real massTangent1 = MassNormalTangent.GetElement(1); + Real massTangent2 = MassNormalTangent.GetElement(2); + Real accumulatedNormalImpulse = Accumulated.GetElement(0); + Real accumulatedTangentImpulse1 = Accumulated.GetElement(1); + Real accumulatedTangentImpulse2 = Accumulated.GetElement(2); var normal = new JVector(NormalTangentX.GetElement(0), NormalTangentY.GetElement(0), NormalTangentZ.GetElement(0)); var tangent1 = new JVector(NormalTangentX.GetElement(1), NormalTangentY.GetElement(1), NormalTangentZ.GetElement(1)); @@ -630,36 +636,36 @@ public unsafe void Iterate(ContactData* cd, bool applyBias) JVector dv = b2.Velocity + b2.AngularVelocity % RelativePosition2; dv -= b1.Velocity + b1.AngularVelocity % RelativePosition1; - float vn = JVector.Dot(normal, dv); - float vt1 = JVector.Dot(tangent1, dv); - float vt2 = JVector.Dot(tangent2, dv); + Real vn = JVector.Dot(normal, dv); + Real vt1 = JVector.Dot(tangent1, dv); + Real vt2 = JVector.Dot(tangent2, dv); - float normalImpulse = -vn; + Real normalImpulse = -vn; - if (applyBias) normalImpulse += MathF.Max(PenaltyBias, Bias); + if (applyBias) normalImpulse += MathR.Max(PenaltyBias, Bias); else normalImpulse += Bias; normalImpulse *= massNormal; - float oldNormalImpulse = accumulatedNormalImpulse; - accumulatedNormalImpulse = MathF.Max(oldNormalImpulse + normalImpulse, 0.0f); + Real oldNormalImpulse = accumulatedNormalImpulse; + accumulatedNormalImpulse = MathR.Max(oldNormalImpulse + normalImpulse, (Real)0.0); normalImpulse = accumulatedNormalImpulse - oldNormalImpulse; - float maxTangentImpulse = cd->Friction * accumulatedNormalImpulse; - float tangentImpulse1 = massTangent1 * -vt1; - float tangentImpulse2 = massTangent2 * -vt2; + Real maxTangentImpulse = cd->Friction * accumulatedNormalImpulse; + Real tangentImpulse1 = massTangent1 * -vt1; + Real tangentImpulse2 = massTangent2 * -vt2; - float oldTangentImpulse1 = accumulatedTangentImpulse1; + Real oldTangentImpulse1 = accumulatedTangentImpulse1; accumulatedTangentImpulse1 = oldTangentImpulse1 + tangentImpulse1; accumulatedTangentImpulse1 = Math.Clamp(accumulatedTangentImpulse1, -maxTangentImpulse, maxTangentImpulse); tangentImpulse1 = accumulatedTangentImpulse1 - oldTangentImpulse1; - float oldTangentImpulse2 = accumulatedTangentImpulse2; + Real oldTangentImpulse2 = accumulatedTangentImpulse2; accumulatedTangentImpulse2 = oldTangentImpulse2 + tangentImpulse2; accumulatedTangentImpulse2 = Math.Clamp(accumulatedTangentImpulse2, -maxTangentImpulse, maxTangentImpulse); tangentImpulse2 = accumulatedTangentImpulse2 - oldTangentImpulse2; - Accumulated = Vector128.Create(accumulatedNormalImpulse, accumulatedTangentImpulse1, accumulatedTangentImpulse2, 0); + Accumulated = Vector.Create(accumulatedNormalImpulse, accumulatedTangentImpulse1, accumulatedTangentImpulse2, 0); if (!cd->IsSpeculative) { @@ -693,12 +699,12 @@ public unsafe void Iterate(ContactData* cd, bool applyBias) #endregion [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static float GetSum3(Vector128 vector) + private static Real GetSum3(VectorReal vector) { return vector.GetElement(0) + vector.GetElement(1) + vector.GetElement(2); } - public unsafe void PrepareForIterationAccelerated(ContactData* cd, float idt) + public unsafe void PrepareForIterationAccelerated(ContactData* cd, Real idt) { ref var b1 = ref cd->Body1.Data; ref var b2 = ref cd->Body2.Data; @@ -706,7 +712,7 @@ public unsafe void PrepareForIterationAccelerated(ContactData* cd, float idt) JVector.Transform(Position1, b1.Orientation, out RelativePosition1); JVector.Transform(Position2, b2.Orientation, out RelativePosition2); - Vector128 kNormalTangent = Vector128.Create(b1.InverseMass + b2.InverseMass); + VectorReal kNormalTangent = Vector.Create(b1.InverseMass + b2.InverseMass); if (!cd->IsSpeculative) { @@ -718,68 +724,68 @@ public unsafe void PrepareForIterationAccelerated(ContactData* cd, float idt) Penetration = JVector.Dot(dist, n); - var rp1X = Vector128.Create(RelativePosition1.X); - var rp1Y = Vector128.Create(RelativePosition1.Y); - var rp1Z = Vector128.Create(RelativePosition1.Z); + var rp1X = Vector.Create(RelativePosition1.X); + var rp1Y = Vector.Create(RelativePosition1.Y); + var rp1Z = Vector.Create(RelativePosition1.Z); - var rp2X = Vector128.Create(RelativePosition2.X); - var rp2Y = Vector128.Create(RelativePosition2.Y); - var rp2Z = Vector128.Create(RelativePosition2.Z); + var rp2X = Vector.Create(RelativePosition2.X); + var rp2Y = Vector.Create(RelativePosition2.Y); + var rp2Z = Vector.Create(RelativePosition2.Z); - var rrx = Vector128.Subtract(Vector128.Multiply(rp1Y, NormalTangentZ), Vector128.Multiply(rp1Z, NormalTangentY)); - var rry = Vector128.Subtract(Vector128.Multiply(rp1Z, NormalTangentX), Vector128.Multiply(rp1X, NormalTangentZ)); - var rrz = Vector128.Subtract(Vector128.Multiply(rp1X, NormalTangentY), Vector128.Multiply(rp1Y, NormalTangentX)); + var rrx = Vector.Subtract(Vector.Multiply(rp1Y, NormalTangentZ), Vector.Multiply(rp1Z, NormalTangentY)); + var rry = Vector.Subtract(Vector.Multiply(rp1Z, NormalTangentX), Vector.Multiply(rp1X, NormalTangentZ)); + var rrz = Vector.Subtract(Vector.Multiply(rp1X, NormalTangentY), Vector.Multiply(rp1Y, NormalTangentX)); - var ixx = Vector128.Create(b1.InverseInertiaWorld.M11); - var ixy = Vector128.Create(b1.InverseInertiaWorld.M21); - var ixz = Vector128.Create(b1.InverseInertiaWorld.M31); - var iyy = Vector128.Create(b1.InverseInertiaWorld.M22); - var iyz = Vector128.Create(b1.InverseInertiaWorld.M23); - var izz = Vector128.Create(b1.InverseInertiaWorld.M33); + var ixx = Vector.Create(b1.InverseInertiaWorld.M11); + var ixy = Vector.Create(b1.InverseInertiaWorld.M21); + var ixz = Vector.Create(b1.InverseInertiaWorld.M31); + var iyy = Vector.Create(b1.InverseInertiaWorld.M22); + var iyz = Vector.Create(b1.InverseInertiaWorld.M23); + var izz = Vector.Create(b1.InverseInertiaWorld.M33); - var e1 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixx, rrx), Vector128.Multiply(ixy, rry)), Vector128.Multiply(ixz, rrz)); - var e2 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixy, rrx), Vector128.Multiply(iyy, rry)), Vector128.Multiply(iyz, rrz)); - var e3 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixz, rrx), Vector128.Multiply(iyz, rry)), Vector128.Multiply(izz, rrz)); + var e1 = Vector.Add(Vector.Add(Vector.Multiply(ixx, rrx), Vector.Multiply(ixy, rry)), Vector.Multiply(ixz, rrz)); + var e2 = Vector.Add(Vector.Add(Vector.Multiply(ixy, rrx), Vector.Multiply(iyy, rry)), Vector.Multiply(iyz, rrz)); + var e3 = Vector.Add(Vector.Add(Vector.Multiply(ixz, rrx), Vector.Multiply(iyz, rry)), Vector.Multiply(izz, rrz)); - rrx = Vector128.Subtract(Vector128.Multiply(rp2Y, NormalTangentZ), Vector128.Multiply(rp2Z, NormalTangentY)); - rry = Vector128.Subtract(Vector128.Multiply(rp2Z, NormalTangentX), Vector128.Multiply(rp2X, NormalTangentZ)); - rrz = Vector128.Subtract(Vector128.Multiply(rp2X, NormalTangentY), Vector128.Multiply(rp2Y, NormalTangentX)); + rrx = Vector.Subtract(Vector.Multiply(rp2Y, NormalTangentZ), Vector.Multiply(rp2Z, NormalTangentY)); + rry = Vector.Subtract(Vector.Multiply(rp2Z, NormalTangentX), Vector.Multiply(rp2X, NormalTangentZ)); + rrz = Vector.Subtract(Vector.Multiply(rp2X, NormalTangentY), Vector.Multiply(rp2Y, NormalTangentX)); - ixx = Vector128.Create(b2.InverseInertiaWorld.M11); - ixy = Vector128.Create(b2.InverseInertiaWorld.M21); - ixz = Vector128.Create(b2.InverseInertiaWorld.M31); - iyy = Vector128.Create(b2.InverseInertiaWorld.M22); - iyz = Vector128.Create(b2.InverseInertiaWorld.M23); - izz = Vector128.Create(b2.InverseInertiaWorld.M33); + ixx = Vector.Create(b2.InverseInertiaWorld.M11); + ixy = Vector.Create(b2.InverseInertiaWorld.M21); + ixz = Vector.Create(b2.InverseInertiaWorld.M31); + iyy = Vector.Create(b2.InverseInertiaWorld.M22); + iyz = Vector.Create(b2.InverseInertiaWorld.M23); + izz = Vector.Create(b2.InverseInertiaWorld.M33); - var f1 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixx, rrx), Vector128.Multiply(ixy, rry)), Vector128.Multiply(ixz, rrz)); - var f2 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixy, rrx), Vector128.Multiply(iyy, rry)), Vector128.Multiply(iyz, rrz)); - var f3 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixz, rrx), Vector128.Multiply(iyz, rry)), Vector128.Multiply(izz, rrz)); + var f1 = Vector.Add(Vector.Add(Vector.Multiply(ixx, rrx), Vector.Multiply(ixy, rry)), Vector.Multiply(ixz, rrz)); + var f2 = Vector.Add(Vector.Add(Vector.Multiply(ixy, rrx), Vector.Multiply(iyy, rry)), Vector.Multiply(iyz, rrz)); + var f3 = Vector.Add(Vector.Add(Vector.Multiply(ixz, rrx), Vector.Multiply(iyz, rry)), Vector.Multiply(izz, rrz)); - var ktnx = Vector128.Subtract(Vector128.Add(Vector128.Subtract(Vector128.Multiply(e2, rp1Z), Vector128.Multiply(e3, rp1Y)), Vector128.Multiply(f2, rp2Z)), Vector128.Multiply(f3, rp2Y)); - var ktny = Vector128.Subtract(Vector128.Add(Vector128.Subtract(Vector128.Multiply(e3, rp1X), Vector128.Multiply(e1, rp1Z)), Vector128.Multiply(f3, rp2X)), Vector128.Multiply(f1, rp2Z)); - var ktnz = Vector128.Subtract(Vector128.Add(Vector128.Subtract(Vector128.Multiply(e1, rp1Y), Vector128.Multiply(e2, rp1X)), Vector128.Multiply(f1, rp2Y)), Vector128.Multiply(f2, rp2X)); + var ktnx = Vector.Subtract(Vector.Add(Vector.Subtract(Vector.Multiply(e2, rp1Z), Vector.Multiply(e3, rp1Y)), Vector.Multiply(f2, rp2Z)), Vector.Multiply(f3, rp2Y)); + var ktny = Vector.Subtract(Vector.Add(Vector.Subtract(Vector.Multiply(e3, rp1X), Vector.Multiply(e1, rp1Z)), Vector.Multiply(f3, rp2X)), Vector.Multiply(f1, rp2Z)); + var ktnz = Vector.Subtract(Vector.Add(Vector.Subtract(Vector.Multiply(e1, rp1Y), Vector.Multiply(e2, rp1X)), Vector.Multiply(f1, rp2Y)), Vector.Multiply(f2, rp2X)); - var kres = Vector128.Add(Vector128.Add(Vector128.Multiply(NormalTangentX, ktnx), Vector128.Multiply(NormalTangentY, ktny)), Vector128.Multiply(NormalTangentZ, ktnz)); + var kres = Vector.Add(Vector.Add(Vector.Multiply(NormalTangentX, ktnx), Vector.Multiply(NormalTangentY, ktny)), Vector.Multiply(NormalTangentZ, ktnz)); - kNormalTangent = Vector128.Add(kNormalTangent, kres); + kNormalTangent = Vector.Add(kNormalTangent, kres); Unsafe.SkipInit(out JVector angularImpulse1); - angularImpulse1.X = GetSum3(Vector128.Multiply(Accumulated, e1)); - angularImpulse1.Y = GetSum3(Vector128.Multiply(Accumulated, e2)); - angularImpulse1.Z = GetSum3(Vector128.Multiply(Accumulated, e3)); + angularImpulse1.X = GetSum3(Vector.Multiply(Accumulated, e1)); + angularImpulse1.Y = GetSum3(Vector.Multiply(Accumulated, e2)); + angularImpulse1.Z = GetSum3(Vector.Multiply(Accumulated, e3)); Unsafe.SkipInit(out JVector angularImpulse2); - angularImpulse2.X = GetSum3(Vector128.Multiply(Accumulated, f1)); - angularImpulse2.Y = GetSum3(Vector128.Multiply(Accumulated, f2)); - angularImpulse2.Z = GetSum3(Vector128.Multiply(Accumulated, f3)); + angularImpulse2.X = GetSum3(Vector.Multiply(Accumulated, f1)); + angularImpulse2.Y = GetSum3(Vector.Multiply(Accumulated, f2)); + angularImpulse2.Z = GetSum3(Vector.Multiply(Accumulated, f3)); b1.AngularVelocity -= angularImpulse1; b2.AngularVelocity += angularImpulse2; } - var mnt = Vector128.Divide(Vector128.Create(1.0f), kNormalTangent); - Unsafe.CopyBlock(Unsafe.AsPointer(ref MassNormalTangent), Unsafe.AsPointer(ref mnt), 12); + var mnt = Vector.Divide(Vector.Create((Real)1.0), kNormalTangent); + Unsafe.CopyBlock(Unsafe.AsPointer(ref MassNormalTangent), Unsafe.AsPointer(ref mnt), 3*sizeof(Real)); if ((Flag & Flags.NewContact) == 0) { @@ -792,14 +798,14 @@ public unsafe void PrepareForIterationAccelerated(ContactData* cd, float idt) Bias = Penetration * idt; } - PenaltyBias = BiasFactor * idt * Math.Max(0.0f, Penetration - AllowedPenetration); - PenaltyBias = Math.Clamp(PenaltyBias, 0.0f, MaximumBias); + PenaltyBias = BiasFactor * idt * Math.Max((Real)0.0, Penetration - AllowedPenetration); + PenaltyBias = Math.Clamp(PenaltyBias, (Real)0.0, MaximumBias); // warm-starting, linear Unsafe.SkipInit(out JVector linearImpulse); - linearImpulse.X = GetSum3(Vector128.Multiply(Accumulated, NormalTangentX)); - linearImpulse.Y = GetSum3(Vector128.Multiply(Accumulated, NormalTangentY)); - linearImpulse.Z = GetSum3(Vector128.Multiply(Accumulated, NormalTangentZ)); + linearImpulse.X = GetSum3(Vector.Multiply(Accumulated, NormalTangentX)); + linearImpulse.Y = GetSum3(Vector.Multiply(Accumulated, NormalTangentY)); + linearImpulse.Z = GetSum3(Vector.Multiply(Accumulated, NormalTangentZ)); b1.Velocity -= b1.InverseMass * linearImpulse; b2.Velocity += b2.InverseMass * linearImpulse; @@ -815,81 +821,81 @@ public unsafe void IterateAccelerated(ContactData* cd, bool applyBias) JVector dv = b2.Velocity + b2.AngularVelocity % RelativePosition2; dv -= b1.Velocity + b1.AngularVelocity % RelativePosition1; - var vdots = Vector128.Add(Vector128.Add(Vector128.Multiply(NormalTangentX, Vector128.Create(dv.X)), Vector128.Multiply(NormalTangentY, Vector128.Create(dv.Y))), Vector128.Multiply(NormalTangentZ, Vector128.Create(dv.Z))); + var vdots = Vector.Add(Vector.Add(Vector.Multiply(NormalTangentX, Vector.Create(dv.X)), Vector.Multiply(NormalTangentY, Vector.Create(dv.Y))), Vector.Multiply(NormalTangentZ, Vector.Create(dv.Z))); - float bias = applyBias ? MathF.Max(PenaltyBias, Bias) : Bias; + Real bias = applyBias ? MathR.Max(PenaltyBias, Bias) : Bias; - var impulse = Vector128.Multiply(MassNormalTangent, (Vector128.Subtract(Vector128.Create(bias, 0, 0, 0), vdots))); + var impulse = Vector.Multiply(MassNormalTangent, (Vector.Subtract(Vector.Create(bias, 0, 0, 0), vdots))); var oldImpulse = Accumulated; - float maxTangentImpulse = cd->Friction * Accumulated.GetElement(0); + Real maxTangentImpulse = cd->Friction * Accumulated.GetElement(0); - Accumulated = Vector128.Add(oldImpulse, impulse); + Accumulated = Vector.Add(oldImpulse, impulse); - var minImpulse = Vector128.Create(0, -maxTangentImpulse, -maxTangentImpulse, 0); - var maxImpulse = Vector128.Create(float.MaxValue, maxTangentImpulse, maxTangentImpulse, 0); + var minImpulse = Vector.Create(0, -maxTangentImpulse, -maxTangentImpulse, 0); + var maxImpulse = Vector.Create(Real.MaxValue, maxTangentImpulse, maxTangentImpulse, 0); - Accumulated = Vector128.Min(Vector128.Max(Accumulated, minImpulse), maxImpulse); - impulse = Vector128.Subtract(Accumulated, oldImpulse); + Accumulated = Vector.Min(Vector.Max(Accumulated, minImpulse), maxImpulse); + impulse = Vector.Subtract(Accumulated, oldImpulse); if (!cd->IsSpeculative) { - var rp1X = Vector128.Create(RelativePosition1.X); - var rp1Y = Vector128.Create(RelativePosition1.Y); - var rp1Z = Vector128.Create(RelativePosition1.Z); - - var rp2X = Vector128.Create(RelativePosition2.X); - var rp2Y = Vector128.Create(RelativePosition2.Y); - var rp2Z = Vector128.Create(RelativePosition2.Z); - - var rrx = Vector128.Subtract(Vector128.Multiply(rp1Y, NormalTangentZ), Vector128.Multiply(rp1Z, NormalTangentY)); - var rry = Vector128.Subtract(Vector128.Multiply(rp1Z, NormalTangentX), Vector128.Multiply(rp1X, NormalTangentZ)); - var rrz = Vector128.Subtract(Vector128.Multiply(rp1X, NormalTangentY), Vector128.Multiply(rp1Y, NormalTangentX)); - - var ixx = Vector128.Create(b1.InverseInertiaWorld.M11); - var ixy = Vector128.Create(b1.InverseInertiaWorld.M21); - var ixz = Vector128.Create(b1.InverseInertiaWorld.M31); - var iyy = Vector128.Create(b1.InverseInertiaWorld.M22); - var iyz = Vector128.Create(b1.InverseInertiaWorld.M23); - var izz = Vector128.Create(b1.InverseInertiaWorld.M33); - - var e1 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixx, rrx), Vector128.Multiply(ixy, rry)), Vector128.Multiply(ixz, rrz)); - var e2 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixy, rrx), Vector128.Multiply(iyy, rry)), Vector128.Multiply(iyz, rrz)); - var e3 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixz, rrx), Vector128.Multiply(iyz, rry)), Vector128.Multiply(izz, rrz)); - - rrx = Vector128.Subtract(Vector128.Multiply(rp2Y, NormalTangentZ), Vector128.Multiply(rp2Z, NormalTangentY)); - rry = Vector128.Subtract(Vector128.Multiply(rp2Z, NormalTangentX), Vector128.Multiply(rp2X, NormalTangentZ)); - rrz = Vector128.Subtract(Vector128.Multiply(rp2X, NormalTangentY), Vector128.Multiply(rp2Y, NormalTangentX)); - - ixx = Vector128.Create(b2.InverseInertiaWorld.M11); - ixy = Vector128.Create(b2.InverseInertiaWorld.M21); - ixz = Vector128.Create(b2.InverseInertiaWorld.M31); - iyy = Vector128.Create(b2.InverseInertiaWorld.M22); - iyz = Vector128.Create(b2.InverseInertiaWorld.M23); - izz = Vector128.Create(b2.InverseInertiaWorld.M33); - - var f1 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixx, rrx), Vector128.Multiply(ixy, rry)), Vector128.Multiply(ixz, rrz)); - var f2 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixy, rrx), Vector128.Multiply(iyy, rry)), Vector128.Multiply(iyz, rrz)); - var f3 = Vector128.Add(Vector128.Add(Vector128.Multiply(ixz, rrx), Vector128.Multiply(iyz, rry)), Vector128.Multiply(izz, rrz)); + var rp1X = Vector.Create(RelativePosition1.X); + var rp1Y = Vector.Create(RelativePosition1.Y); + var rp1Z = Vector.Create(RelativePosition1.Z); + + var rp2X = Vector.Create(RelativePosition2.X); + var rp2Y = Vector.Create(RelativePosition2.Y); + var rp2Z = Vector.Create(RelativePosition2.Z); + + var rrx = Vector.Subtract(Vector.Multiply(rp1Y, NormalTangentZ), Vector.Multiply(rp1Z, NormalTangentY)); + var rry = Vector.Subtract(Vector.Multiply(rp1Z, NormalTangentX), Vector.Multiply(rp1X, NormalTangentZ)); + var rrz = Vector.Subtract(Vector.Multiply(rp1X, NormalTangentY), Vector.Multiply(rp1Y, NormalTangentX)); + + var ixx = Vector.Create(b1.InverseInertiaWorld.M11); + var ixy = Vector.Create(b1.InverseInertiaWorld.M21); + var ixz = Vector.Create(b1.InverseInertiaWorld.M31); + var iyy = Vector.Create(b1.InverseInertiaWorld.M22); + var iyz = Vector.Create(b1.InverseInertiaWorld.M23); + var izz = Vector.Create(b1.InverseInertiaWorld.M33); + + var e1 = Vector.Add(Vector.Add(Vector.Multiply(ixx, rrx), Vector.Multiply(ixy, rry)), Vector.Multiply(ixz, rrz)); + var e2 = Vector.Add(Vector.Add(Vector.Multiply(ixy, rrx), Vector.Multiply(iyy, rry)), Vector.Multiply(iyz, rrz)); + var e3 = Vector.Add(Vector.Add(Vector.Multiply(ixz, rrx), Vector.Multiply(iyz, rry)), Vector.Multiply(izz, rrz)); + + rrx = Vector.Subtract(Vector.Multiply(rp2Y, NormalTangentZ), Vector.Multiply(rp2Z, NormalTangentY)); + rry = Vector.Subtract(Vector.Multiply(rp2Z, NormalTangentX), Vector.Multiply(rp2X, NormalTangentZ)); + rrz = Vector.Subtract(Vector.Multiply(rp2X, NormalTangentY), Vector.Multiply(rp2Y, NormalTangentX)); + + ixx = Vector.Create(b2.InverseInertiaWorld.M11); + ixy = Vector.Create(b2.InverseInertiaWorld.M21); + ixz = Vector.Create(b2.InverseInertiaWorld.M31); + iyy = Vector.Create(b2.InverseInertiaWorld.M22); + iyz = Vector.Create(b2.InverseInertiaWorld.M23); + izz = Vector.Create(b2.InverseInertiaWorld.M33); + + var f1 = Vector.Add(Vector.Add(Vector.Multiply(ixx, rrx), Vector.Multiply(ixy, rry)), Vector.Multiply(ixz, rrz)); + var f2 = Vector.Add(Vector.Add(Vector.Multiply(ixy, rrx), Vector.Multiply(iyy, rry)), Vector.Multiply(iyz, rrz)); + var f3 = Vector.Add(Vector.Add(Vector.Multiply(ixz, rrx), Vector.Multiply(iyz, rry)), Vector.Multiply(izz, rrz)); Unsafe.SkipInit(out JVector angularImpulse1); - angularImpulse1.X = GetSum3(Vector128.Multiply(impulse, e1)); - angularImpulse1.Y = GetSum3(Vector128.Multiply(impulse, e2)); - angularImpulse1.Z = GetSum3(Vector128.Multiply(impulse, e3)); + angularImpulse1.X = GetSum3(Vector.Multiply(impulse, e1)); + angularImpulse1.Y = GetSum3(Vector.Multiply(impulse, e2)); + angularImpulse1.Z = GetSum3(Vector.Multiply(impulse, e3)); Unsafe.SkipInit(out JVector angularImpulse2); - angularImpulse2.X = GetSum3(Vector128.Multiply(impulse, f1)); - angularImpulse2.Y = GetSum3(Vector128.Multiply(impulse, f2)); - angularImpulse2.Z = GetSum3(Vector128.Multiply(impulse, f3)); + angularImpulse2.X = GetSum3(Vector.Multiply(impulse, f1)); + angularImpulse2.Y = GetSum3(Vector.Multiply(impulse, f2)); + angularImpulse2.Z = GetSum3(Vector.Multiply(impulse, f3)); b1.AngularVelocity -= angularImpulse1; b2.AngularVelocity += angularImpulse2; } Unsafe.SkipInit(out JVector linearImpulse); - linearImpulse.X = GetSum3(Vector128.Multiply(impulse, NormalTangentX)); - linearImpulse.Y = GetSum3(Vector128.Multiply(impulse, NormalTangentY)); - linearImpulse.Z = GetSum3(Vector128.Multiply(impulse, NormalTangentZ)); + linearImpulse.X = GetSum3(Vector.Multiply(impulse, NormalTangentX)); + linearImpulse.Y = GetSum3(Vector.Multiply(impulse, NormalTangentY)); + linearImpulse.Z = GetSum3(Vector.Multiply(impulse, NormalTangentZ)); b1.Velocity -= b1.InverseMass * linearImpulse; b2.Velocity += b2.InverseMass * linearImpulse; diff --git a/src/Jitter2/Dynamics/RigidBody.cs b/src/Jitter2/Dynamics/RigidBody.cs index 0214a4ec..bde6e034 100644 --- a/src/Jitter2/Dynamics/RigidBody.cs +++ b/src/Jitter2/Dynamics/RigidBody.cs @@ -49,7 +49,7 @@ public struct RigidBodyData public JQuaternion Orientation; public JMatrix InverseInertiaWorld; - public float InverseMass; + public Real InverseMass; public bool IsActive; public bool IsStatic; @@ -65,8 +65,8 @@ public sealed class RigidBody : IListIndex, IDebugDrawable public readonly ulong RigidBodyId; - private float restitution = 0.0f; - private float friction = 0.2f; + private Real restitution = (Real)0.0; + private Real friction = (Real)0.2; /// /// Due to performance considerations, the data used to simulate this body (e.g., velocity or position) @@ -147,17 +147,17 @@ internal void RaiseEndCollide(Arbiter arbiter) internal int islandMarker; - internal float sleepTime = 0.0f; + internal Real sleepTime = (Real)0.0; - internal float inactiveThresholdLinearSq = 0.1f; - internal float inactiveThresholdAngularSq = 0.1f; - internal float deactivationTimeThreshold = 1.0f; + internal Real inactiveThresholdLinearSq = (Real)0.1; + internal Real inactiveThresholdAngularSq = (Real)0.1; + internal Real deactivationTimeThreshold = (Real)1.0; - internal float linearDampingMultiplier = 0.998f; - internal float angularDampingMultiplier = 0.995f; + internal Real linearDampingMultiplier = (Real)0.998; + internal Real angularDampingMultiplier = (Real)0.995; internal JMatrix inverseInertia = JMatrix.Identity; - internal float inverseMass = 1.0f; + internal Real inverseMass = (Real)1.0; /// /// Gets or sets the friction coefficient for this object. @@ -171,12 +171,12 @@ internal void RaiseEndCollide(Arbiter arbiter) /// /// Thrown if the value is not between 0 and 1. /// - public float Friction + public Real Friction { get => friction; set { - if (value < 0.0f || value > 1.0f) + if (value < (Real)0.0 || value > (Real)1.0) { throw new ArgumentOutOfRangeException(nameof(value), "Friction must be between 0 and 1."); @@ -198,12 +198,12 @@ public float Friction /// /// Thrown if the value is not between 0 and 1. /// - public float Restitution + public Real Restitution { get => restitution; set { - if (value < 0.0f || value > 1.0f) + if (value < (Real)0.0 || value > (Real)1.0) { throw new ArgumentOutOfRangeException(nameof(value), "Restitution must be between 0 and 1."); @@ -251,7 +251,7 @@ internal RigidBody(JHandle handle, World world) public TimeSpan DeactivationTime { get => TimeSpan.FromSeconds(deactivationTimeThreshold); - set => deactivationTimeThreshold = (float)value.TotalSeconds; + set => deactivationTimeThreshold = (Real)value.TotalSeconds; } /// @@ -259,9 +259,9 @@ public TimeSpan DeactivationTime /// remain below the specified values for the duration of , the body is deactivated. /// The threshold values are given in rad/s and length units/s, respectively. /// - public (float angular, float linear) DeactivationThreshold + public (Real angular, Real linear) DeactivationThreshold { - get => (MathF.Sqrt(inactiveThresholdAngularSq), MathF.Sqrt(inactiveThresholdLinearSq)); + get => (MathR.Sqrt(inactiveThresholdAngularSq), MathR.Sqrt(inactiveThresholdLinearSq)); set { if (value.linear < 0 || value.angular < 0) @@ -279,27 +279,27 @@ public TimeSpan DeactivationTime /// Gets or sets the damping factors for linear and angular motion. /// A damping factor of 0 means the body is not damped, while 1 brings /// the body to a halt immediately. Damping is applied when calling - /// . Jitter multiplies the respective + /// . Jitter multiplies the respective /// velocity each step by 1 minus the damping factor. Note that the values /// are not scaled by time; a smaller time-step in - /// results in increased damping. + /// results in increased damping. /// /// /// The damping factors should be within the range [0, 1]. /// - public (float linear, float angular) Damping + public (Real linear, Real angular) Damping { - get => (1.0f - linearDampingMultiplier, 1.0f - angularDampingMultiplier); + get => ((Real)1.0 - linearDampingMultiplier, (Real)1.0 - angularDampingMultiplier); set { - if (value.linear < 0.0f || value.linear > 1.0f || value.angular < 0.0f || value.angular > 1.0f) + if (value.linear < (Real)0.0 || value.linear > (Real)1.0 || value.angular < (Real)0.0 || value.angular > (Real)1.0) { throw new ArgumentOutOfRangeException(nameof(value), "Damping multiplier has to be within [0, 1]."); } - linearDampingMultiplier = 1.0f - value.linear; - angularDampingMultiplier = 1.0f - value.angular; + linearDampingMultiplier = (Real)1.0 - value.linear; + angularDampingMultiplier = (Real)1.0 - value.angular; } } @@ -311,7 +311,7 @@ public override int GetHashCode() private void SetDefaultMassInertia() { inverseInertia = JMatrix.Identity; - Data.InverseMass = 1.0f; + Data.InverseMass = (Real)1.0; UpdateWorldInertia(); } @@ -374,7 +374,7 @@ private void UpdateWorldInertia() if (Data.IsStatic) { Data.InverseInertiaWorld = JMatrix.Zero; - Data.InverseMass = 0.0f; + Data.InverseMass = (Real)0.0; } else { @@ -470,19 +470,19 @@ public void AddShape(RigidBodyShape shape, bool setMassInertia = true) } /// - /// Represents the force to be applied to the body during the next call to . + /// Represents the force to be applied to the body during the next call to . /// This value is automatically reset to zero after the call. /// public JVector Force { get; set; } /// - /// Represents the torque to be applied to the body during the next call to . + /// Represents the torque to be applied to the body during the next call to . /// This value is automatically reset to zero after the call. /// public JVector Torque { get; set; } /// - /// Applies a force to the rigid body, thereby altering its velocity. This force is effective for a single frame only and is reset to zero during the next call to . + /// Applies a force to the rigid body, thereby altering its velocity. This force is effective for a single frame only and is reset to zero during the next call to . /// /// The force to be applied. public void AddForce(in JVector force) @@ -492,7 +492,7 @@ public void AddForce(in JVector force) /// /// Applies a force to the rigid body, altering its velocity. This force is applied for a single frame only and is - /// reset to zero with the subsequent call to . + /// reset to zero with the subsequent call to . /// /// The force to be applied. /// The position where the force will be applied. @@ -606,12 +606,12 @@ public void SetMassInertia() if (shapes.Count == 0) { inverseInertia = JMatrix.Identity; - Data.InverseMass = 1.0f; + Data.InverseMass = (Real)1.0; return; } JMatrix inertia = JMatrix.Zero; - float mass = 0.0f; + Real mass = (Real)0.0; for (int i = 0; i < shapes.Count; i++) { @@ -628,7 +628,7 @@ public void SetMassInertia() "RigidBody.AddShape, call AddShape with setMassInertia set to false."); } - inverseMass = 1.0f / mass; + inverseMass = (Real)1.0 / mass; UpdateWorldInertia(); } @@ -636,9 +636,9 @@ public void SetMassInertia() /// /// Sets a new mass value and scales the inertia according to the ratio of the old mass to the new mass. /// - public void SetMassInertia(float mass) + public void SetMassInertia(Real mass) { - if (mass <= 0.0f) + if (mass <= (Real)0.0) { // we do not protect against NaN here, since it is the users responsibility // to not feed NaNs to the engine. @@ -646,8 +646,8 @@ public void SetMassInertia(float mass) } SetMassInertia(); - inverseInertia = JMatrix.Multiply(inverseInertia, 1.0f / (Data.InverseMass * mass)); - inverseMass = 1.0f / mass; + inverseInertia = JMatrix.Multiply(inverseInertia, (Real)1.0 / (Data.InverseMass * mass)); + inverseMass = (Real)1.0 / mass; UpdateWorldInertia(); } @@ -656,11 +656,11 @@ public void SetMassInertia(float mass) /// /// Set the inverse values. /// - public void SetMassInertia(in JMatrix inertia, float mass, bool setAsInverse = false) + public void SetMassInertia(in JMatrix inertia, Real mass, bool setAsInverse = false) { if (setAsInverse) { - if (float.IsInfinity(mass) || mass < 0.0f) + if (Real.IsInfinity(mass) || mass < (Real)0.0) { throw new ArgumentException("Inverse mass must be finite and not negative.", nameof(mass)); } @@ -670,7 +670,7 @@ public void SetMassInertia(in JMatrix inertia, float mass, bool setAsInverse = f } else { - if (mass <= 0.0f) + if (mass <= (Real)0.0) { throw new ArgumentException("Mass can not be zero or negative.", nameof(mass)); } @@ -680,7 +680,7 @@ public void SetMassInertia(in JMatrix inertia, float mass, bool setAsInverse = f throw new ArgumentException("Inertia matrix is not invertible.", nameof(inertia)); } - inverseMass = 1.0f / mass; + inverseMass = (Real)1.0 / mass; } UpdateWorldInertia(); @@ -715,10 +715,10 @@ public void DebugDraw(IDebugDrawer drawer) /// /// Gets the mass of the rigid body. To modify the mass, use - /// or - /// . + /// or + /// . /// - public float Mass => 1.0f / inverseMass; + public Real Mass => (Real)1.0 / inverseMass; int IListIndex.ListIndex { get; set; } = -1; } \ No newline at end of file diff --git a/src/Jitter2/Jitter2.csproj b/src/Jitter2/Jitter2.csproj index 08f826a3..5d2e871a 100644 --- a/src/Jitter2/Jitter2.csproj +++ b/src/Jitter2/Jitter2.csproj @@ -1,46 +1,58 @@ - - net7.0;net8.0;net9.0 - disable - enable - true - + + net7.0;net8.0;net9.0 + disable + enable + true + - - CS1591;CS1573 - + + CS1591;CS1573 + - - - Jitter Physics 2, the evolution of Jitter Physics, is an impulse-based dynamics engine with a semi-implicit Euler integrator. - It is a fast, simple, and dependency-free engine written in C# with a clear and user-friendly API. - - true - readme.md - https://jitterphysics.com - icon.png - physics engine;collision;csharp;dotnet - MIT - notgiven688 - https://github.com/notgiven688/jitterphysics2.git - git - Embedded - True - true - snupkg - + + + Jitter Physics 2, the evolution of Jitter Physics, is an impulse-based dynamics engine with a semi-implicit Euler integrator. + It is a fast, simple, and dependency-free engine written in C# with a clear and user-friendly API. + + true + readme.md + https://jitterphysics.com + icon.png + physics engine;collision;csharp;dotnet + MIT + notgiven688 + https://github.com/notgiven688/jitterphysics2.git + git + Embedded + true + true + snupkg + - - - - + + + + - - $(DefineConstants);RELEASE;TRACE - false - portable - true - + + + $(DefineConstants);RELEASE;TRACE + false + portable + true + + + + + $(DefineConstants);USE_DOUBLE_PRECISION + Jitter2.Double + + Jitter Physics 2 with double precision enabled. This is an impulse-based dynamics engine with a semi-implicit Euler integrator. + + Jitter2.Double + physics engine;collision;csharp;dotnet;double precision + diff --git a/src/Jitter2/LinearMath/JAngle.cs b/src/Jitter2/LinearMath/JAngle.cs index 21ca5116..2a4dcd8b 100644 --- a/src/Jitter2/LinearMath/JAngle.cs +++ b/src/Jitter2/LinearMath/JAngle.cs @@ -26,12 +26,12 @@ namespace Jitter2.LinearMath; /// -/// A 32-bit floating point variable representing an angle. This structure exists to eliminate +/// A 32-bit Realing point variable representing an angle. This structure exists to eliminate /// ambiguity between radians and degrees in the Jitter API. /// public struct JAngle : IEquatable { - public float Radiant { get; set; } + public Real Radiant { get; set; } public readonly override bool Equals(object? obj) { @@ -48,23 +48,23 @@ public readonly override int GetHashCode() return Radiant.GetHashCode(); } - public float Degree + public Real Degree { - readonly get => Radiant / MathF.PI * 180.0f; - set => Radiant = value / 180.0f * MathF.PI; + readonly get => Radiant / MathR.PI * (Real)180.0; + set => Radiant = value / (Real)180.0 * MathR.PI; } - public static JAngle FromRadiant(float rad) + public static JAngle FromRadiant(Real rad) { return new JAngle { Radiant = rad }; } - public static JAngle FromDegree(float deg) + public static JAngle FromDegree(Real deg) { return new JAngle { Degree = deg }; } - public static explicit operator JAngle(float angle) + public static explicit operator JAngle(Real angle) { return FromRadiant(angle); } @@ -86,35 +86,35 @@ public static explicit operator JAngle(float angle) public static bool operator ==(JAngle l, JAngle r) { - return (float)l == (float)r; + return (Real)l == (Real)r; } public static bool operator !=(JAngle l, JAngle r) { - return (float)l != (float)r; + return (Real)l != (Real)r; } public static bool operator <(JAngle l, JAngle r) { - return (float)l < (float)r; + return (Real)l < (Real)r; } public static bool operator >(JAngle l, JAngle r) { - return (float)l > (float)r; + return (Real)l > (Real)r; } public static bool operator >=(JAngle l, JAngle r) { - return (float)l >= (float)r; + return (Real)l >= (Real)r; } public static bool operator <=(JAngle l, JAngle r) { - return (float)l <= (float)r; + return (Real)l <= (Real)r; } - public static explicit operator float(JAngle angle) + public static explicit operator Real(JAngle angle) { return angle.Radiant; } diff --git a/src/Jitter2/LinearMath/JBBox.cs b/src/Jitter2/LinearMath/JBBox.cs index 7acf5e2e..917a09a6 100644 --- a/src/Jitter2/LinearMath/JBBox.cs +++ b/src/Jitter2/LinearMath/JBBox.cs @@ -28,7 +28,7 @@ namespace Jitter2.LinearMath; /// public struct JBBox { - public const float Epsilon = 1e-12f; + public const Real Epsilon = (Real)1e-12; public enum ContainmentType { @@ -46,10 +46,10 @@ public enum ContainmentType static JBBox() { - LargeBox.Min = new JVector(float.MinValue); - LargeBox.Max = new JVector(float.MaxValue); - SmallBox.Min = new JVector(float.MaxValue); - SmallBox.Max = new JVector(float.MinValue); + LargeBox.Min = new JVector(Real.MinValue); + LargeBox.Max = new JVector(Real.MaxValue); + SmallBox.Min = new JVector(Real.MaxValue); + SmallBox.Max = new JVector(Real.MinValue); } public JBBox(JVector min, JVector max) @@ -64,14 +64,14 @@ internal void InverseTransform(ref JVector position, ref JMatrix orientation) JVector.Subtract(Min, position, out Min); JVector.Add(Max, Min, out JVector center); - center.X *= 0.5f; - center.Y *= 0.5f; - center.Z *= 0.5f; + center.X *= (Real)0.5; + center.Y *= (Real)0.5; + center.Z *= (Real)0.5; JVector.Subtract(Max, Min, out JVector halfExtents); - halfExtents.X *= 0.5f; - halfExtents.Y *= 0.5f; - halfExtents.Z *= 0.5f; + halfExtents.X *= (Real)0.5; + halfExtents.Y *= (Real)0.5; + halfExtents.Z *= (Real)0.5; JVector.TransposedTransform(center, orientation, out center); @@ -84,8 +84,8 @@ internal void InverseTransform(ref JVector position, ref JMatrix orientation) public void Transform(ref JMatrix orientation) { - JVector halfExtents = 0.5f * (Max - Min); - JVector center = 0.5f * (Max + Min); + JVector halfExtents = (Real)0.5 * (Max - Min); + JVector center = (Real)0.5 * (Max + Min); JVector.Transform(center, orientation, out center); @@ -96,13 +96,13 @@ public void Transform(ref JMatrix orientation) Min = center - halfExtents; } - private bool Intersect1D(float start, float dir, float min, float max, - ref float enter, ref float exit) + private bool Intersect1D(Real start, Real dir, Real min, Real max, + ref Real enter, ref Real exit) { if (dir * dir < Epsilon * Epsilon) return start >= min && start <= max; - float t0 = (min - start) / dir; - float t1 = (max - start) / dir; + Real t0 = (min - start) / dir; + Real t1 = (max - start) / dir; if (t0 > t1) { @@ -118,7 +118,7 @@ private bool Intersect1D(float start, float dir, float min, float max, public bool SegmentIntersect(in JVector origin, in JVector direction) { - float enter = 0.0f, exit = 1.0f; + Real enter = (Real)0.0, exit = (Real)1.0; if (!Intersect1D(origin.X, direction.X, Min.X, Max.X, ref enter, ref exit)) return false; @@ -134,7 +134,7 @@ public bool SegmentIntersect(in JVector origin, in JVector direction) public bool RayIntersect(in JVector origin, in JVector direction) { - float enter = 0.0f, exit = float.MaxValue; + Real enter = (Real)0.0, exit = Real.MaxValue; if (!Intersect1D(origin.X, direction.X, Min.X, Max.X, ref enter, ref exit)) return false; @@ -148,10 +148,10 @@ public bool RayIntersect(in JVector origin, in JVector direction) return true; } - public bool RayIntersect(in JVector origin, in JVector direction, out float enter) + public bool RayIntersect(in JVector origin, in JVector direction, out Real enter) { - enter = 0.0f; - float exit = float.MaxValue; + enter = (Real)0.0; + Real exit = Real.MaxValue; if (!Intersect1D(origin.X, direction.X, Min.X, Max.X, ref enter, ref exit)) return false; @@ -194,8 +194,8 @@ public void AddPoint(in JVector point) public static JBBox CreateFromPoints(JVector[] points) { - JVector vector3 = new JVector(float.MaxValue); - JVector vector2 = new JVector(float.MinValue); + JVector vector3 = new JVector(Real.MaxValue); + JVector vector2 = new JVector(Real.MinValue); for (int i = 0; i < points.Length; i++) { @@ -245,17 +245,17 @@ public static void CreateMerged(in JBBox original, in JBBox additional, out JBBo JVector.Max(original.Max, additional.Max, out result.Max); } - public readonly JVector Center => (Min + Max) * (1.0f / 2.0f); + public readonly JVector Center => (Min + Max) * ((Real)(1.0 / 2.0)); - public float GetVolume() + public Real GetVolume() { JVector len = Max - Min; return len.X * len.Y * len.Z; } - public float GetSurfaceArea() + public Real GetSurfaceArea() { JVector len = Max - Min; - return 2.0f * (len.X * len.Y + len.Y * len.Z + len.Z * len.X); + return (Real)2.0 * (len.X * len.Y + len.Y * len.Z + len.Z * len.X); } } \ No newline at end of file diff --git a/src/Jitter2/LinearMath/JMatrix.cs b/src/Jitter2/LinearMath/JMatrix.cs index f89ce6fa..03d0711d 100644 --- a/src/Jitter2/LinearMath/JMatrix.cs +++ b/src/Jitter2/LinearMath/JMatrix.cs @@ -28,20 +28,20 @@ namespace Jitter2.LinearMath; /// -/// 3x3 matrix of 32 bit float values in column major format. +/// Represents a three-by-three matrix with components of type . /// -[StructLayout(LayoutKind.Explicit, Size = 36)] +[StructLayout(LayoutKind.Explicit, Size = 9*sizeof(Real))] public struct JMatrix { - [FieldOffset(0)] public float M11; - [FieldOffset(4)] public float M21; - [FieldOffset(8)] public float M31; - [FieldOffset(12)] public float M12; - [FieldOffset(16)] public float M22; - [FieldOffset(20)] public float M32; - [FieldOffset(24)] public float M13; - [FieldOffset(28)] public float M23; - [FieldOffset(32)] public float M33; + [FieldOffset(0*sizeof(Real))] public Real M11; + [FieldOffset(1*sizeof(Real))] public Real M21; + [FieldOffset(2*sizeof(Real))] public Real M31; + [FieldOffset(3*sizeof(Real))] public Real M12; + [FieldOffset(4*sizeof(Real))] public Real M22; + [FieldOffset(5*sizeof(Real))] public Real M32; + [FieldOffset(6*sizeof(Real))] public Real M13; + [FieldOffset(7*sizeof(Real))] public Real M23; + [FieldOffset(8*sizeof(Real))] public Real M33; public static readonly JMatrix Identity; public static readonly JMatrix Zero; @@ -52,13 +52,13 @@ static JMatrix() Identity = new JMatrix { - M11 = 1.0f, - M22 = 1.0f, - M33 = 1.0f + M11 = (Real)1.0, + M22 = (Real)1.0, + M33 = (Real)1.0 }; } - public JMatrix(float m11, float m12, float m13, float m21, float m22, float m23, float m31, float m32, float m33) + public JMatrix(Real m11, Real m12, Real m13, Real m21, Real m22, Real m23, Real m31, Real m32, Real m33) { M11 = m11; M12 = m12; @@ -91,7 +91,7 @@ public unsafe ref JVector UnsafeGet(int index) [MethodImpl(MethodImplOptions.AggressiveInlining)] public unsafe JVector GetColumn(int index) { - fixed (float* ptr = &M11) + fixed (Real* ptr = &M11) { JVector* vptr = (JVector*)ptr; return vptr[index]; @@ -119,10 +119,10 @@ public static JMatrix TransposedMultiply(in JMatrix matrix1, in JMatrix matrix2) return result; } - public static JMatrix CreateRotationMatrix(JVector axis, float angle) + public static JMatrix CreateRotationMatrix(JVector axis, Real angle) { - float c = MathF.Cos(angle / 2.0f); - float s = MathF.Sin(angle / 2.0f); + Real c = MathR.Cos(angle / (Real)2.0); + Real s = MathR.Sin(angle / (Real)2.0); axis *= s; JQuaternion jq = new(axis.X, axis.Y, axis.Z, c); CreateFromQuaternion(in jq, out JMatrix result); @@ -132,15 +132,15 @@ public static JMatrix CreateRotationMatrix(JVector axis, float angle) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Multiply(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result) { - float num0 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M21 + matrix1.M13 * matrix2.M31; - float num1 = matrix1.M11 * matrix2.M12 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M32; - float num2 = matrix1.M11 * matrix2.M13 + matrix1.M12 * matrix2.M23 + matrix1.M13 * matrix2.M33; - float num3 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M23 * matrix2.M31; - float num4 = matrix1.M21 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M32; - float num5 = matrix1.M21 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M23 * matrix2.M33; - float num6 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M21 + matrix1.M33 * matrix2.M31; - float num7 = matrix1.M31 * matrix2.M12 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M32; - float num8 = matrix1.M31 * matrix2.M13 + matrix1.M32 * matrix2.M23 + matrix1.M33 * matrix2.M33; + Real num0 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M21 + matrix1.M13 * matrix2.M31; + Real num1 = matrix1.M11 * matrix2.M12 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M32; + Real num2 = matrix1.M11 * matrix2.M13 + matrix1.M12 * matrix2.M23 + matrix1.M13 * matrix2.M33; + Real num3 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M23 * matrix2.M31; + Real num4 = matrix1.M21 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M32; + Real num5 = matrix1.M21 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M23 * matrix2.M33; + Real num6 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M21 + matrix1.M33 * matrix2.M31; + Real num7 = matrix1.M31 * matrix2.M12 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M32; + Real num8 = matrix1.M31 * matrix2.M13 + matrix1.M32 * matrix2.M23 + matrix1.M33 * matrix2.M33; result.M11 = num0; result.M12 = num1; @@ -166,15 +166,15 @@ public static JMatrix Add(JMatrix matrix1, JMatrix matrix2) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void MultiplyTransposed(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result) { - float num0 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M12 + matrix1.M13 * matrix2.M13; - float num1 = matrix1.M11 * matrix2.M21 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M23; - float num2 = matrix1.M11 * matrix2.M31 + matrix1.M12 * matrix2.M32 + matrix1.M13 * matrix2.M33; - float num3 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M12 + matrix1.M23 * matrix2.M13; - float num4 = matrix1.M21 * matrix2.M21 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M23; - float num5 = matrix1.M21 * matrix2.M31 + matrix1.M22 * matrix2.M32 + matrix1.M23 * matrix2.M33; - float num6 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M12 + matrix1.M33 * matrix2.M13; - float num7 = matrix1.M31 * matrix2.M21 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M23; - float num8 = matrix1.M31 * matrix2.M31 + matrix1.M32 * matrix2.M32 + matrix1.M33 * matrix2.M33; + Real num0 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M12 + matrix1.M13 * matrix2.M13; + Real num1 = matrix1.M11 * matrix2.M21 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M23; + Real num2 = matrix1.M11 * matrix2.M31 + matrix1.M12 * matrix2.M32 + matrix1.M13 * matrix2.M33; + Real num3 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M12 + matrix1.M23 * matrix2.M13; + Real num4 = matrix1.M21 * matrix2.M21 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M23; + Real num5 = matrix1.M21 * matrix2.M31 + matrix1.M22 * matrix2.M32 + matrix1.M23 * matrix2.M33; + Real num6 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M12 + matrix1.M33 * matrix2.M13; + Real num7 = matrix1.M31 * matrix2.M21 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M23; + Real num8 = matrix1.M31 * matrix2.M31 + matrix1.M32 * matrix2.M32 + matrix1.M33 * matrix2.M33; result.M11 = num0; result.M12 = num1; @@ -187,12 +187,12 @@ public static void MultiplyTransposed(in JMatrix matrix1, in JMatrix matrix2, ou result.M33 = num8; } - public static JMatrix CreateRotationX(float radians) + public static JMatrix CreateRotationX(Real radians) { JMatrix result = Identity; - float c = (float)Math.Cos(radians); - float s = (float)Math.Sin(radians); + Real c = MathR.Cos(radians); + Real s = MathR.Sin(radians); // [ 1 0 0 ] // [ 0 c -s ] @@ -205,12 +205,12 @@ public static JMatrix CreateRotationX(float radians) return result; } - public static JMatrix CreateRotationY(float radians) + public static JMatrix CreateRotationY(Real radians) { JMatrix result = Identity; - float c = (float)Math.Cos(radians); - float s = (float)Math.Sin(radians); + Real c = MathR.Cos(radians); + Real s = MathR.Sin(radians); // [ c 0 s ] // [ 0 1 0 ] @@ -223,12 +223,12 @@ public static JMatrix CreateRotationY(float radians) return result; } - public static JMatrix CreateRotationZ(float radians) + public static JMatrix CreateRotationZ(Real radians) { JMatrix result = Identity; - float c = (float)Math.Cos(radians); - float s = (float)Math.Sin(radians); + Real c = MathR.Cos(radians); + Real s = MathR.Sin(radians); // [ c -s 0 ] // [ s c 0 ] @@ -260,7 +260,7 @@ public static JMatrix CreateScale(in JVector scale) /// Create a scaling matrix. /// /// - public static JMatrix CreateScale(float x, float y, float z) + public static JMatrix CreateScale(Real x, Real y, Real z) { return CreateScale(new JVector(x, y, z)); } @@ -271,15 +271,15 @@ public static JMatrix CreateScale(float x, float y, float z) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void TransposedMultiply(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result) { - float num0 = matrix1.M11 * matrix2.M11 + matrix1.M21 * matrix2.M21 + matrix1.M31 * matrix2.M31; - float num1 = matrix1.M11 * matrix2.M12 + matrix1.M21 * matrix2.M22 + matrix1.M31 * matrix2.M32; - float num2 = matrix1.M11 * matrix2.M13 + matrix1.M21 * matrix2.M23 + matrix1.M31 * matrix2.M33; - float num3 = matrix1.M12 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M32 * matrix2.M31; - float num4 = matrix1.M12 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M32 * matrix2.M32; - float num5 = matrix1.M12 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M32 * matrix2.M33; - float num6 = matrix1.M13 * matrix2.M11 + matrix1.M23 * matrix2.M21 + matrix1.M33 * matrix2.M31; - float num7 = matrix1.M13 * matrix2.M12 + matrix1.M23 * matrix2.M22 + matrix1.M33 * matrix2.M32; - float num8 = matrix1.M13 * matrix2.M13 + matrix1.M23 * matrix2.M23 + matrix1.M33 * matrix2.M33; + Real num0 = matrix1.M11 * matrix2.M11 + matrix1.M21 * matrix2.M21 + matrix1.M31 * matrix2.M31; + Real num1 = matrix1.M11 * matrix2.M12 + matrix1.M21 * matrix2.M22 + matrix1.M31 * matrix2.M32; + Real num2 = matrix1.M11 * matrix2.M13 + matrix1.M21 * matrix2.M23 + matrix1.M31 * matrix2.M33; + Real num3 = matrix1.M12 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M32 * matrix2.M31; + Real num4 = matrix1.M12 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M32 * matrix2.M32; + Real num5 = matrix1.M12 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M32 * matrix2.M33; + Real num6 = matrix1.M13 * matrix2.M11 + matrix1.M23 * matrix2.M21 + matrix1.M33 * matrix2.M31; + Real num7 = matrix1.M13 * matrix2.M12 + matrix1.M23 * matrix2.M22 + matrix1.M33 * matrix2.M32; + Real num8 = matrix1.M13 * matrix2.M13 + matrix1.M23 * matrix2.M23 + matrix1.M33 * matrix2.M33; result.M11 = num0; result.M12 = num1; @@ -321,7 +321,7 @@ public static void Subtract(in JMatrix matrix1, in JMatrix matrix2, out JMatrix } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float Determinant() + public readonly Real Determinant() { return M11 * M22 * M33 + M12 * M23 * M31 + M13 * M21 * M32 - M31 * M22 * M13 - M32 * M23 * M11 - M33 * M21 * M12; @@ -330,25 +330,25 @@ public readonly float Determinant() [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool Inverse(in JMatrix matrix, out JMatrix result) { - float idet = 1.0f / matrix.Determinant(); + Real idet = (Real)1.0 / matrix.Determinant(); - if (!float.IsNormal(idet)) + if (!Real.IsNormal(idet)) { result = new JMatrix(); return false; } - float num11 = matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32; - float num12 = matrix.M13 * matrix.M32 - matrix.M12 * matrix.M33; - float num13 = matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13; + Real num11 = matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32; + Real num12 = matrix.M13 * matrix.M32 - matrix.M12 * matrix.M33; + Real num13 = matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13; - float num21 = matrix.M23 * matrix.M31 - matrix.M33 * matrix.M21; - float num22 = matrix.M11 * matrix.M33 - matrix.M31 * matrix.M13; - float num23 = matrix.M13 * matrix.M21 - matrix.M23 * matrix.M11; + Real num21 = matrix.M23 * matrix.M31 - matrix.M33 * matrix.M21; + Real num22 = matrix.M11 * matrix.M33 - matrix.M31 * matrix.M13; + Real num23 = matrix.M13 * matrix.M21 - matrix.M23 * matrix.M11; - float num31 = matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22; - float num32 = matrix.M12 * matrix.M31 - matrix.M32 * matrix.M11; - float num33 = matrix.M11 * matrix.M22 - matrix.M21 * matrix.M12; + Real num31 = matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22; + Real num32 = matrix.M12 * matrix.M31 - matrix.M32 * matrix.M11; + Real num33 = matrix.M11 * matrix.M22 - matrix.M21 * matrix.M12; result.M11 = num11 * idet; result.M12 = num12 * idet; @@ -364,16 +364,16 @@ public static bool Inverse(in JMatrix matrix, out JMatrix result) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JMatrix Multiply(JMatrix matrix1, float scaleFactor) + public static JMatrix Multiply(JMatrix matrix1, Real scaleFactor) { Multiply(in matrix1, scaleFactor, out JMatrix result); return result; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Multiply(in JMatrix matrix1, float scaleFactor, out JMatrix result) + public static void Multiply(in JMatrix matrix1, Real scaleFactor, out JMatrix result) { - float num = scaleFactor; + Real num = scaleFactor; result.M11 = matrix1.M11 * num; result.M12 = matrix1.M12 * num; result.M13 = matrix1.M13 * num; @@ -409,20 +409,20 @@ public static void Absolute(in JMatrix matrix, out JMatrix result) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void CreateFromQuaternion(in JQuaternion quaternion, out JMatrix result) { - float r = quaternion.W; - float i = quaternion.X; - float j = quaternion.Y; - float k = quaternion.Z; + Real r = quaternion.W; + Real i = quaternion.X; + Real j = quaternion.Y; + Real k = quaternion.Z; - result.M11 = 1.0f - 2.0f * (j * j + k * k); - result.M12 = 2.0f * (i * j - k * r); - result.M13 = 2.0f * (i * k + j * r); - result.M21 = 2.0f * (i * j + k * r); - result.M22 = 1.0f - 2.0f * (i * i + k * k); - result.M23 = 2.0f * (j * k - i * r); - result.M31 = 2.0f * (i * k - j * r); - result.M32 = 2.0f * (j * k + i * r); - result.M33 = 1.0f - 2.0f * (i * i + j * j); + result.M11 = (Real)1.0 - (Real)2.0 * (j * j + k * k); + result.M12 = (Real)2.0 * (i * j - k * r); + result.M13 = (Real)2.0 * (i * k + j * r); + result.M21 = (Real)2.0 * (i * j + k * r); + result.M22 = (Real)1.0 - (Real)2.0 * (i * i + k * k); + result.M23 = (Real)2.0 * (j * k - i * r); + result.M31 = (Real)2.0 * (i * k - j * r); + result.M32 = (Real)2.0 * (j * k + i * r); + result.M33 = (Real)1.0 - (Real)2.0 * (i * i + j * j); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -472,20 +472,20 @@ private static void Transpose(in JMatrix matrix, out JMatrix result) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public float Trace() + public Real Trace() { return M11 + M22 + M33; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JMatrix operator *(float factor, in JMatrix matrix) + public static JMatrix operator *(Real factor, in JMatrix matrix) { Multiply(matrix, factor, out JMatrix result); return result; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JMatrix operator *(in JMatrix matrix, float factor) + public static JMatrix operator *(in JMatrix matrix, Real factor) { Multiply(matrix, factor, out JMatrix result); return result; diff --git a/src/Jitter2/LinearMath/JQuaternion.cs b/src/Jitter2/LinearMath/JQuaternion.cs index 3963d656..365531c6 100644 --- a/src/Jitter2/LinearMath/JQuaternion.cs +++ b/src/Jitter2/LinearMath/JQuaternion.cs @@ -31,10 +31,10 @@ namespace Jitter2.LinearMath; /// public struct JQuaternion { - public float X; - public float Y; - public float Z; - public float W; + public Real X; + public Real Y; + public Real Z; + public Real W; /// /// Gets the identity quaternion (0, 0, 0, 1). @@ -48,7 +48,7 @@ public struct JQuaternion /// The Y component. /// The Z component. /// The W component. - public JQuaternion(float x, float y, float z, float w) + public JQuaternion(Real x, Real y, Real z, Real w) { X = x; Y = y; @@ -61,7 +61,7 @@ public JQuaternion(float x, float y, float z, float w) /// /// The W component. /// The vector component. - public JQuaternion(float w, in JVector v) + public JQuaternion(Real w, in JVector v) { X = v.X; Y = v.Y; @@ -186,9 +186,9 @@ public readonly JVector GetBasisX() { JVector result; - result.X = 1.0f - 2.0f * (Y * Y + Z * Z); - result.Y = 2.0f * (X * Y + Z * W); - result.Z = 2.0f * (X * Z - Y * W); + result.X = (Real)1.0 - (Real)2.0 * (Y * Y + Z * Z); + result.Y = (Real)2.0 * (X * Y + Z * W); + result.Z = (Real)2.0 * (X * Z - Y * W); return result; } @@ -201,9 +201,9 @@ public readonly JVector GetBasisY() { JVector result; - result.X = 2.0f * (X * Y - Z * W); - result.Y = 1.0f - 2.0f * (X * X + Z * Z); - result.Z = 2.0f * (Y * Z + X * W); + result.X = (Real)2.0 * (X * Y - Z * W); + result.Y = (Real)1.0 - (Real)2.0 * (X * X + Z * Z); + result.Z = (Real)2.0 * (Y * Z + X * W); return result; } @@ -216,9 +216,9 @@ public readonly JVector GetBasisZ() { JVector result; - result.X = 2.0f * (X * Z + Y * W); - result.Y = 2.0f * (Y * Z - X * W); - result.Z = 1.0f - 2.0f * (X * X + Y * Y); + result.X = (Real)2.0 * (X * Z + Y * W); + result.Y = (Real)2.0 * (Y * Z - X * W); + result.Z = (Real)1.0 - (Real)2.0 * (X * X + Y * Y); return result; } @@ -228,10 +228,10 @@ public readonly JVector GetBasisZ() /// /// The angle of rotation in radians. /// A quaternion representing the rotation. - public static JQuaternion CreateRotationX(float radians) + public static JQuaternion CreateRotationX(Real radians) { - float halfAngle = radians * 0.5f; - (float sha, float cha) = MathF.SinCos(halfAngle); + Real halfAngle = radians * (Real)0.5; + (Real sha, Real cha) = MathR.SinCos(halfAngle); return new JQuaternion(sha, 0, 0, cha); } @@ -240,10 +240,10 @@ public static JQuaternion CreateRotationX(float radians) /// /// The angle of rotation in radians. /// A quaternion representing the rotation. - public static JQuaternion CreateRotationY(float radians) + public static JQuaternion CreateRotationY(Real radians) { - float halfAngle = radians * 0.5f; - (float sha, float cha) = MathF.SinCos(halfAngle); + Real halfAngle = radians * (Real)0.5; + (Real sha, Real cha) = MathR.SinCos(halfAngle); return new JQuaternion(0, sha, 0, cha); } @@ -252,10 +252,10 @@ public static JQuaternion CreateRotationY(float radians) /// /// The angle of rotation in radians. /// A quaternion representing the rotation. - public static JQuaternion CreateRotationZ(float radians) + public static JQuaternion CreateRotationZ(Real radians) { - float halfAngle = radians * 0.5f; - (float sha, float cha) = MathF.SinCos(halfAngle); + Real halfAngle = radians * (Real)0.5; + (Real sha, Real cha) = MathR.SinCos(halfAngle); return new JQuaternion(0, 0, sha, cha); } @@ -268,15 +268,15 @@ public static JQuaternion CreateRotationZ(float radians) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Multiply(in JQuaternion quaternion1, in JQuaternion quaternion2, out JQuaternion result) { - float r1 = quaternion1.W; - float i1 = quaternion1.X; - float j1 = quaternion1.Y; - float k1 = quaternion1.Z; + Real r1 = quaternion1.W; + Real i1 = quaternion1.X; + Real j1 = quaternion1.Y; + Real k1 = quaternion1.Z; - float r2 = quaternion2.W; - float i2 = quaternion2.X; - float j2 = quaternion2.Y; - float k2 = quaternion2.Z; + Real r2 = quaternion2.W; + Real i2 = quaternion2.X; + Real j2 = quaternion2.Y; + Real k2 = quaternion2.Z; result.W = r1 * r2 - (i1 * i2 + j1 * j2 + k1 * k2); result.X = r1 * i2 + r2 * i1 + j1 * k2 - k1 * j2; @@ -293,15 +293,15 @@ public static void Multiply(in JQuaternion quaternion1, in JQuaternion quaternio [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConjugateMultiply(in JQuaternion quaternion1, in JQuaternion quaternion2, out JQuaternion result) { - float r1 = quaternion1.W; - float i1 = -quaternion1.X; - float j1 = -quaternion1.Y; - float k1 = -quaternion1.Z; + Real r1 = quaternion1.W; + Real i1 = -quaternion1.X; + Real j1 = -quaternion1.Y; + Real k1 = -quaternion1.Z; - float r2 = quaternion2.W; - float i2 = quaternion2.X; - float j2 = quaternion2.Y; - float k2 = quaternion2.Z; + Real r2 = quaternion2.W; + Real i2 = quaternion2.X; + Real j2 = quaternion2.Y; + Real k2 = quaternion2.Z; result.W = r1 * r2 - (i1 * i2 + j1 * j2 + k1 * k2); result.X = r1 * i2 + r2 * i1 + j1 * k2 - k1 * j2; @@ -330,15 +330,15 @@ public static JQuaternion ConjugateMultiply(in JQuaternion quaternion1, in JQuat [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void MultiplyConjugate(in JQuaternion quaternion1, in JQuaternion quaternion2, out JQuaternion result) { - float r1 = quaternion1.W; - float i1 = quaternion1.X; - float j1 = quaternion1.Y; - float k1 = quaternion1.Z; + Real r1 = quaternion1.W; + Real i1 = quaternion1.X; + Real j1 = quaternion1.Y; + Real k1 = quaternion1.Z; - float r2 = quaternion2.W; - float i2 = -quaternion2.X; - float j2 = -quaternion2.Y; - float k2 = -quaternion2.Z; + Real r2 = quaternion2.W; + Real i2 = -quaternion2.X; + Real j2 = -quaternion2.Y; + Real k2 = -quaternion2.Z; result.W = r1 * r2 - (i1 * i2 + j1 * j2 + k1 * k2); result.X = r1 * i2 + r2 * i1 + j1 * k2 - k1 * j2; @@ -365,7 +365,7 @@ public static JQuaternion MultiplyConjugate(in JQuaternion quaternion1, in JQuat /// The scalar factor. /// The scaled quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JQuaternion Multiply(in JQuaternion quaternion1, float scaleFactor) + public static JQuaternion Multiply(in JQuaternion quaternion1, Real scaleFactor) { Multiply(in quaternion1, scaleFactor, out JQuaternion result); return result; @@ -378,7 +378,7 @@ public static JQuaternion Multiply(in JQuaternion quaternion1, float scaleFactor /// The scalar factor. /// When the method completes, contains the scaled quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Multiply(in JQuaternion quaternion1, float scaleFactor, out JQuaternion result) + public static void Multiply(in JQuaternion quaternion1, Real scaleFactor, out JQuaternion result) { result.W = quaternion1.W * scaleFactor; result.X = quaternion1.X * scaleFactor; @@ -391,9 +391,9 @@ public static void Multiply(in JQuaternion quaternion1, float scaleFactor, out J /// /// The length of the quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float Length() + public readonly Real Length() { - return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W); + return (Real)Math.Sqrt(X * X + Y * Y + Z * Z + W * W); } /// @@ -402,8 +402,8 @@ public readonly float Length() [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Normalize() { - float num2 = X * X + Y * Y + Z * Z + W * W; - float num = 1f / (float)Math.Sqrt(num2); + Real num2 = X * X + Y * Y + Z * Z + W * W; + Real num = (Real)1.0 / MathR.Sqrt(num2); X *= num; Y *= num; Z *= num; @@ -429,18 +429,18 @@ public static JQuaternion CreateFromMatrix(in JMatrix matrix) /// When the method completes, contains the quaternion representing the rotation. public static void CreateFromMatrix(in JMatrix matrix, out JQuaternion result) { - float t; + Real t; if (matrix.M33 < 0) { if (matrix.M11 > matrix.M22) { - t = 1.0f + matrix.M11 - matrix.M22 - matrix.M33; + t = (Real)1.0 + matrix.M11 - matrix.M22 - matrix.M33; result = new JQuaternion(t, matrix.M21 + matrix.M12, matrix.M31 + matrix.M13, matrix.M32 - matrix.M23); } else { - t = 1.0f - matrix.M11 + matrix.M22 - matrix.M33; + t = (Real)1.0 - matrix.M11 + matrix.M22 - matrix.M33; result = new JQuaternion(matrix.M21 + matrix.M12, t, matrix.M32 + matrix.M23, matrix.M13 - matrix.M31); } } @@ -448,17 +448,17 @@ public static void CreateFromMatrix(in JMatrix matrix, out JQuaternion result) { if (matrix.M11 < -matrix.M22) { - t = 1.0f - matrix.M11 - matrix.M22 + matrix.M33; + t = (Real)1.0 - matrix.M11 - matrix.M22 + matrix.M33; result = new JQuaternion(matrix.M13 + matrix.M31, matrix.M32 + matrix.M23, t, matrix.M21 - matrix.M12); } else { - t = 1.0f + matrix.M11 + matrix.M22 + matrix.M33; + t = (Real)1.0 + matrix.M11 + matrix.M22 + matrix.M33; result = new JQuaternion(matrix.M32 - matrix.M23, matrix.M13 - matrix.M31, matrix.M21 - matrix.M12, t); } } - t = (float)(0.5d / Math.Sqrt(t)); + t = (Real)(0.5d / Math.Sqrt(t)); result.X *= t; result.Y *= t; result.Z *= t; @@ -485,7 +485,7 @@ public static void CreateFromMatrix(in JMatrix matrix, out JQuaternion result) /// The quaternion to multiply. /// The scaled quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JQuaternion operator *(float value1, in JQuaternion value2) + public static JQuaternion operator *(Real value1, in JQuaternion value2) { Multiply(value2, value1, out JQuaternion result); return result; @@ -498,7 +498,7 @@ public static void CreateFromMatrix(in JMatrix matrix, out JQuaternion result) /// The scalar factor. /// The scaled quaternion. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JQuaternion operator *(in JQuaternion value1, float value2) + public static JQuaternion operator *(in JQuaternion value1, Real value2) { Multiply(value1, value2, out JQuaternion result); return result; diff --git a/src/Jitter2/LinearMath/JVector.cs b/src/Jitter2/LinearMath/JVector.cs index 2eb96e6c..311357d6 100644 --- a/src/Jitter2/LinearMath/JVector.cs +++ b/src/Jitter2/LinearMath/JVector.cs @@ -28,17 +28,17 @@ namespace Jitter2.LinearMath; /// -/// Represents a three-dimensional vector using three floating-point numbers. +/// Represents a three-dimensional vector with components of type . /// -[StructLayout(LayoutKind.Sequential, Size = 12)] +[StructLayout(LayoutKind.Explicit, Size = 3*sizeof(Real))] public struct JVector { internal static JVector InternalZero; internal static JVector Arbitrary; - public float X; - public float Y; - public float Z; + [FieldOffset(0*sizeof(Real))] public Real X; + [FieldOffset(1*sizeof(Real))] public Real Y; + [FieldOffset(2*sizeof(Real))] public Real Z; public static readonly JVector Zero; public static readonly JVector UnitX; @@ -55,27 +55,27 @@ static JVector() UnitX = new JVector(1, 0, 0); UnitY = new JVector(0, 1, 0); UnitZ = new JVector(0, 0, 1); - MinValue = new JVector(float.MinValue); - MaxValue = new JVector(float.MaxValue); + MinValue = new JVector(Real.MinValue); + MaxValue = new JVector(Real.MaxValue); Arbitrary = new JVector(1, 1, 1); InternalZero = Zero; } - public JVector(float x, float y, float z) + public JVector(Real x, Real y, Real z) { X = x; Y = y; Z = z; } - public void Set(float x, float y, float z) + public void Set(Real x, Real y, Real z) { X = x; Y = y; Z = z; } - public JVector(float xyz) + public JVector(Real xyz) { X = xyz; Y = xyz; @@ -83,24 +83,24 @@ public JVector(float xyz) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public unsafe ref float UnsafeGet(int index) + public unsafe ref Real UnsafeGet(int index) { - float* ptr = (float*)Unsafe.AsPointer(ref this); + Real* ptr = (Real*)Unsafe.AsPointer(ref this); return ref ptr[index]; } - public unsafe float this[int i] + public unsafe Real this[int i] { get { - fixed (float* ptr = &X) + fixed (Real* ptr = &X) { return ptr[i]; } } set { - fixed (float* ptr = &X) + fixed (Real* ptr = &X) { ptr[i] = value; } @@ -160,14 +160,14 @@ public static JVector Max(in JVector value1, in JVector value2) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static JVector Abs(in JVector value1) { - return new JVector(MathF.Abs(value1.X), MathF.Abs(value1.Y), MathF.Abs(value1.Z)); + return new JVector(MathR.Abs(value1.X), MathR.Abs(value1.Y), MathR.Abs(value1.Z)); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float MaxAbs(in JVector value1) + public static Real MaxAbs(in JVector value1) { JVector abs = Abs(value1); - return MathF.Max(MathF.Max(abs.X, abs.Y), abs.Z); + return MathR.Max(MathR.Max(abs.X, abs.Y), abs.Z); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -181,9 +181,9 @@ public static void Max(in JVector value1, in JVector value2, out JVector result) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void MakeZero() { - X = 0.0f; - Y = 0.0f; - Z = 0.0f; + X = (Real)0.0; + Y = (Real)0.0; + Z = (Real)0.0; } /// @@ -226,9 +226,9 @@ public static JVector TransposedTransform(in JVector vector, in JQuaternion quat [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Transform(in JVector vector, in JMatrix matrix, out JVector result) { - float num0 = vector.X * matrix.M11 + vector.Y * matrix.M12 + vector.Z * matrix.M13; - float num1 = vector.X * matrix.M21 + vector.Y * matrix.M22 + vector.Z * matrix.M23; - float num2 = vector.X * matrix.M31 + vector.Y * matrix.M32 + vector.Z * matrix.M33; + Real num0 = vector.X * matrix.M11 + vector.Y * matrix.M12 + vector.Z * matrix.M13; + Real num1 = vector.X * matrix.M21 + vector.Y * matrix.M22 + vector.Z * matrix.M23; + Real num2 = vector.X * matrix.M31 + vector.Y * matrix.M32 + vector.Z * matrix.M33; result.X = num0; result.Y = num1; @@ -241,9 +241,9 @@ public static void Transform(in JVector vector, in JMatrix matrix, out JVector r [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void TransposedTransform(in JVector vector, in JMatrix matrix, out JVector result) { - float num0 = vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31; - float num1 = vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32; - float num2 = vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33; + Real num0 = vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31; + Real num1 = vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32; + Real num2 = vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33; result.X = num0; result.Y = num1; @@ -256,13 +256,13 @@ public static void TransposedTransform(in JVector vector, in JMatrix matrix, out [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Transform(in JVector vector, in JQuaternion quaternion, out JVector result) { - float numx = 2.0f * (quaternion.Y * vector.Z - quaternion.Z * vector.Y); - float numy = 2.0f * (quaternion.Z * vector.X - quaternion.X * vector.Z); - float numz = 2.0f * (quaternion.X * vector.Y - quaternion.Y * vector.X); + Real numx = (Real)2.0 * (quaternion.Y * vector.Z - quaternion.Z * vector.Y); + Real numy = (Real)2.0 * (quaternion.Z * vector.X - quaternion.X * vector.Z); + Real numz = (Real)2.0 * (quaternion.X * vector.Y - quaternion.Y * vector.X); - float num00 = quaternion.Y * numz - quaternion.Z * numy; - float num11 = quaternion.Z * numx - quaternion.X * numz; - float num22 = quaternion.X * numy - quaternion.Y * numx; + Real num00 = quaternion.Y * numz - quaternion.Z * numy; + Real num11 = quaternion.Z * numx - quaternion.X * numz; + Real num22 = quaternion.X * numy - quaternion.Y * numx; result.X = vector.X + quaternion.W * numx + num00; result.Y = vector.Y + quaternion.W * numy + num11; @@ -275,13 +275,13 @@ public static void Transform(in JVector vector, in JQuaternion quaternion, out J [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void ConjugatedTransform(in JVector vector, in JQuaternion quaternion, out JVector result) { - float numx = 2.0f * (quaternion.Z * vector.Y - quaternion.Y * vector.Z); - float numy = 2.0f * (quaternion.X * vector.Z - quaternion.Z * vector.X); - float numz = 2.0f * (quaternion.Y * vector.X - quaternion.X * vector.Y); + Real numx = (Real)2.0 * (quaternion.Z * vector.Y - quaternion.Y * vector.Z); + Real numy = (Real)2.0 * (quaternion.X * vector.Z - quaternion.Z * vector.X); + Real numz = (Real)2.0 * (quaternion.Y * vector.X - quaternion.X * vector.Y); - float num00 = quaternion.Z * numy - quaternion.Y * numz; - float num11 = quaternion.X * numz - quaternion.Z * numx; - float num22 = quaternion.Y * numx - quaternion.X * numy; + Real num00 = quaternion.Z * numy - quaternion.Y * numz; + Real num11 = quaternion.X * numz - quaternion.Z * numx; + Real num22 = quaternion.Y * numx - quaternion.X * numy; result.X = vector.X + quaternion.W * numx + num00; result.Y = vector.Y + quaternion.W * numy + num11; @@ -308,7 +308,7 @@ public static JMatrix Outer(in JVector u, in JVector v) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float Dot(in JVector vector1, in JVector vector2) + public static Real Dot(in JVector vector1, in JVector vector2) { return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; } @@ -338,9 +338,9 @@ public static JVector Subtract(JVector value1, JVector value2) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Subtract(in JVector value1, in JVector value2, out JVector result) { - float num0 = value1.X - value2.X; - float num1 = value1.Y - value2.Y; - float num2 = value1.Z - value2.Z; + Real num0 = value1.X - value2.X; + Real num1 = value1.Y - value2.Y; + Real num2 = value1.Z - value2.Z; result.X = num0; result.Y = num1; @@ -357,9 +357,9 @@ public static JVector Cross(in JVector vector1, in JVector vector2) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Cross(in JVector vector1, in JVector vector2, out JVector result) { - float num0 = vector1.Y * vector2.Z - vector1.Z * vector2.Y; - float num1 = vector1.Z * vector2.X - vector1.X * vector2.Z; - float num2 = vector1.X * vector2.Y - vector1.Y * vector2.X; + Real num0 = vector1.Y * vector2.Z - vector1.Z * vector2.Y; + Real num1 = vector1.Z * vector2.X - vector1.X * vector2.Z; + Real num2 = vector1.X * vector2.Y - vector1.Y * vector2.X; result.X = num0; result.Y = num1; @@ -390,9 +390,9 @@ public static JVector Negate(in JVector value) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Negate(in JVector value, out JVector result) { - float num0 = -value.X; - float num1 = -value.Y; - float num2 = -value.Z; + Real num0 = -value.X; + Real num1 = -value.Y; + Real num2 = -value.Z; result.X = num0; result.Y = num1; @@ -409,8 +409,8 @@ public static JVector Normalize(in JVector value) [MethodImpl(MethodImplOptions.AggressiveInlining)] public void Normalize() { - float num2 = X * X + Y * Y + Z * Z; - float num = 1f / (float)Math.Sqrt(num2); + Real num2 = X * X + Y * Y + Z * Z; + Real num = (Real)1.0 / MathR.Sqrt(num2); X *= num; Y *= num; Z *= num; @@ -419,23 +419,23 @@ public void Normalize() [MethodImpl(MethodImplOptions.AggressiveInlining)] public static void Normalize(in JVector value, out JVector result) { - float num2 = value.X * value.X + value.Y * value.Y + value.Z * value.Z; - float num = 1f / (float)Math.Sqrt(num2); + Real num2 = value.X * value.X + value.Y * value.Y + value.Z * value.Z; + Real num = (Real)1.0 / MathR.Sqrt(num2); result.X = value.X * num; result.Y = value.Y * num; result.Z = value.Z * num; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float LengthSquared() + public readonly Real LengthSquared() { return X * X + Y * Y + Z * Z; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public readonly float Length() + public readonly Real Length() { - return MathF.Sqrt(X * X + Y * Y + Z * Z); + return MathR.Sqrt(X * X + Y * Y + Z * Z); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -445,14 +445,14 @@ public static void Swap(ref JVector vector1, ref JVector vector2) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JVector Multiply(in JVector value1, float scaleFactor) + public static JVector Multiply(in JVector value1, Real scaleFactor) { Multiply(value1, scaleFactor, out JVector result); return result; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public void Multiply(float factor) + public void Multiply(Real factor) { X *= factor; Y *= factor; @@ -460,7 +460,7 @@ public void Multiply(float factor) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static void Multiply(in JVector value1, float scaleFactor, out JVector result) + public static void Multiply(in JVector value1, Real scaleFactor, out JVector result) { result.X = value1.X * scaleFactor; result.Y = value1.Y * scaleFactor; @@ -481,13 +481,13 @@ public static void Multiply(in JVector value1, float scaleFactor, out JVector re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float operator *(in JVector vector1, in JVector vector2) + public static Real operator *(in JVector vector1, in JVector vector2) { return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JVector operator *(in JVector value1, float value2) + public static JVector operator *(in JVector value1, Real value2) { JVector result; result.X = value1.X * value2; @@ -497,7 +497,7 @@ public static void Multiply(in JVector value1, float scaleFactor, out JVector re } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static JVector operator *(float value1, in JVector value2) + public static JVector operator *(Real value1, in JVector value2) { JVector result; result.X = value2.X * value1; @@ -519,7 +519,7 @@ public static void Multiply(in JVector value1, float scaleFactor, out JVector re [MethodImpl(MethodImplOptions.AggressiveInlining)] public static JVector operator -(in JVector left) { - return Multiply(left, -1.0f); + return Multiply(left, -(Real)1.0); } [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Jitter2/LinearMath/MathHelper.cs b/src/Jitter2/LinearMath/MathHelper.cs index 37bf2779..debd24e6 100644 --- a/src/Jitter2/LinearMath/MathHelper.cs +++ b/src/Jitter2/LinearMath/MathHelper.cs @@ -35,9 +35,9 @@ public static class MathHelper // Line 1: p1 + (p2 - p1)*mua // Line 2: p3 + (p4 - p3)*mub - public static bool LineLineIntersect(in JVector p1, in JVector p2, in JVector p3, in JVector P4, out JVector Pa, out JVector Pb, out float mua, out float mub) + public static bool LineLineIntersect(in JVector p1, in JVector p2, in JVector p3, in JVector P4, out JVector Pa, out JVector Pb, out Real mua, out Real mub) { - const float Epsilon = 1e-12f; + const Real Epsilon = (Real)1e-12; Pa = Pb = JVector.Zero; mua = mub = 0; @@ -46,16 +46,16 @@ public static bool LineLineIntersect(in JVector p1, in JVector p2, in JVector p3 JVector p43 = P4 - p3; JVector p21 = p2 - p1; - float d1343 = p13 * p43; - float d4321 = p43 * p21; - float d1321 = p13 * p21; - float d4343 = p43 * p43; - float d2121 = p21 * p21; + Real d1343 = p13 * p43; + Real d4321 = p43 * p21; + Real d1321 = p13 * p21; + Real d4343 = p43 * p43; + Real d2121 = p21 * p21; - float denom = d2121 * d4343 - d4321 * d4321; + Real denom = d2121 * d4343 - d4321 * d4321; if (Math.Abs(denom) < Epsilon) return false; - float numer = d1343 * d4321 - d1321 * d4343; + Real numer = d1343 * d4321 - d1321 * d4343; mua = numer / denom; mub = (d1343 + d4321 * mua) / d4343; @@ -71,7 +71,7 @@ public static bool LineLineIntersect(in JVector p1, in JVector p2, in JVector p3 /// /// Checks if matrix is a pure rotation matrix. /// - public static bool IsRotationMatrix(in JMatrix matrix, float epsilon = 1e-06f) + public static bool IsRotationMatrix(in JMatrix matrix, Real epsilon = (Real)1e-06) { JMatrix delta = JMatrix.MultiplyTransposed(matrix, matrix) - JMatrix.Identity; @@ -80,25 +80,25 @@ public static bool IsRotationMatrix(in JMatrix matrix, float epsilon = 1e-06f) return false; } - return MathF.Abs(matrix.Determinant() - 1.0f) < epsilon; + return MathR.Abs(matrix.Determinant() - (Real)1.0) < epsilon; } /// /// Checks if all entries of a vector are close to zero. /// - public static bool IsZero(in JVector vector, float epsilon = 1e-6f) + public static bool IsZero(in JVector vector, Real epsilon = (Real)1e-6) { - float x = MathF.Abs(vector.X); - float y = MathF.Abs(vector.Y); - float z = MathF.Abs(vector.Z); + Real x = MathR.Abs(vector.X); + Real y = MathR.Abs(vector.Y); + Real z = MathR.Abs(vector.Z); - return MathF.Max(x, MathF.Max(y, z)) < epsilon; + return MathR.Max(x, MathR.Max(y, z)) < epsilon; } /// /// Checks if all entries of a matrix are close to zero. /// - public static bool UnsafeIsZero(ref JMatrix matrix, float epsilon = 1e-6f) + public static bool UnsafeIsZero(ref JMatrix matrix, Real epsilon = (Real)1e-6) { if (!IsZero(matrix.UnsafeGet(0), epsilon)) return false; if (!IsZero(matrix.UnsafeGet(1), epsilon)) return false; @@ -112,7 +112,7 @@ public static bool UnsafeIsZero(ref JMatrix matrix, float epsilon = 1e-6f) /// The number of Jacobi iterations. public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2) { - float phi, cp, sp; + Real phi, cp, sp; Unsafe.SkipInit(out JMatrix r); JMatrix rotation = JMatrix.Identity; @@ -120,10 +120,10 @@ public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2) for (int i = 0; i < sweeps; i++) { // M32 - if (MathF.Abs(m.M23) > 1e-6f) + if (MathR.Abs(m.M23) > (Real)1e-6) { - phi = MathF.Atan2(1, (m.M33 - m.M22) / (2.0f * m.M23)) / 2.0f; - (sp, cp) = MathF.SinCos(phi); + phi = MathR.Atan2(1, (m.M33 - m.M22) / ((Real)2.0 * m.M23)) / (Real)2.0; + (sp, cp) = MathR.SinCos(phi); r = new JMatrix(1, 0, 0, 0, cp, sp, 0, -sp, cp); JMatrix.Multiply(m, r, out m); JMatrix.TransposedMultiply(r, m, out m); @@ -131,10 +131,10 @@ public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2) } // M21 - if (MathF.Abs(m.M21) > 1e-6f) + if (MathR.Abs(m.M21) > (Real)1e-6) { - phi = MathF.Atan2(1, (m.M22 - m.M11) / (2.0f * m.M21)) / 2.0f; - (sp, cp) = MathF.SinCos(phi); + phi = MathR.Atan2(1, (m.M22 - m.M11) / ((Real)2.0 * m.M21)) / (Real)2.0; + (sp, cp) = MathR.SinCos(phi); r = new JMatrix(cp, sp, 0, -sp, cp, 0, 0, 0, 1); JMatrix.Multiply(m, r, out m); JMatrix.TransposedMultiply(r, m, out m); @@ -142,10 +142,10 @@ public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2) } // M31 - if (MathF.Abs(m.M31) > 1e-6f) + if (MathR.Abs(m.M31) > (Real)1e-6) { - phi = MathF.Atan2(1, (m.M33 - m.M11) / (2.0f * m.M31)) / 2.0f; - (sp, cp) = MathF.SinCos(phi); + phi = MathR.Atan2(1, (m.M33 - m.M11) / ((Real)2.0 * m.M31)) / (Real)2.0; + (sp, cp) = MathR.SinCos(phi); r = new JMatrix(cp, 0, sp, 0, 1, 0, -sp, 0, cp); JMatrix.Multiply(m, r, out m); JMatrix.TransposedMultiply(r, m, out m); @@ -153,9 +153,9 @@ public static JMatrix InverseSquareRoot(JMatrix m, int sweeps = 2) } } - JMatrix d = new JMatrix(1.0f / MathF.Sqrt(m.M11), 0, 0, - 0, 1.0f / MathF.Sqrt(m.M22), 0, - 0, 0, 1.0f / MathF.Sqrt(m.M33)); + JMatrix d = new JMatrix((Real)1.0 / MathR.Sqrt(m.M11), 0, 0, + 0, (Real)1.0 / MathR.Sqrt(m.M22), 0, + 0, 0, (Real)1.0 / MathR.Sqrt(m.M33)); return rotation * d * JMatrix.Transpose(rotation); } @@ -172,9 +172,9 @@ public static JVector CreateOrthonormal(in JVector vec) Debug.Assert(!CloseToZero(vec)); - float xa = Math.Abs(vec.X); - float ya = Math.Abs(vec.Y); - float za = Math.Abs(vec.Z); + Real xa = Math.Abs(vec.X); + Real ya = Math.Abs(vec.Y); + Real za = Math.Abs(vec.Z); if ((xa > ya && xa > za) || (ya > xa && ya > za)) { @@ -191,7 +191,7 @@ public static JVector CreateOrthonormal(in JVector vec) result.Normalize(); - Debug.Assert(MathF.Abs(JVector.Dot(result, vec)) < 1e-6f); + Debug.Assert(MathR.Abs(JVector.Dot(result, vec)) < (Real)1e-6); return result; } @@ -202,7 +202,7 @@ public static JVector CreateOrthonormal(in JVector vec) /// /// The input matrix to check for an orthonormal basis. /// True if the columns of the matrix form an orthonormal basis; otherwise, false. - public static bool CheckOrthonormalBasis(in JMatrix matrix, float epsilon = 1e-6f) + public static bool CheckOrthonormalBasis(in JMatrix matrix, Real epsilon = (Real)1e-6) { JMatrix delta = JMatrix.MultiplyTransposed(matrix, matrix) - JMatrix.Identity; return UnsafeIsZero(ref delta, epsilon); @@ -215,7 +215,7 @@ public static bool CheckOrthonormalBasis(in JMatrix matrix, float epsilon = 1e-6 /// A threshold value below which the squared magnitude of the vector /// is considered to be zero or close to zero. /// True if the vector is close to zero; otherwise, false. - public static bool CloseToZero(in JVector v, float epsilonSq = 1e-16f) + public static bool CloseToZero(in JVector v, Real epsilonSq = (Real)1e-16) { return v.LengthSquared() < epsilonSq; } diff --git a/src/Jitter2/Precision.cs b/src/Jitter2/Precision.cs new file mode 100644 index 00000000..275b0608 --- /dev/null +++ b/src/Jitter2/Precision.cs @@ -0,0 +1,62 @@ +/* + * Copyright (c) Thorben Linneweber and others + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE + * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION + * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +// Uncomment here to build Jitter using double precision +// -------------------------------------------------------- +// #define USE_DOUBLE_PRECISION +// -------------------------------------------------------- +// Or use command line option, e.g. +// dotnet build -c Release -p:DoublePrecision=true + +#if USE_DOUBLE_PRECISION + +global using Real = System.Double; +global using MathR = System.Math; +global using Vector = System.Runtime.Intrinsics.Vector256; +global using VectorReal = System.Runtime.Intrinsics.Vector256; + +#else + +global using Real = System.Single; +global using MathR = System.MathF; +global using Vector = System.Runtime.Intrinsics.Vector128; +global using VectorReal = System.Runtime.Intrinsics.Vector128; + +#endif + +namespace Jitter2; +public static class Precision +{ + #if USE_DOUBLE_PRECISION + public const int ConstraintSizeFull = 512; + public const int ConstraintSizeSmall = 256; + #else + public const int ConstraintSizeFull = 256; + public const int ConstraintSizeSmall = 128; + #endif + + /// + /// Gets a value indicating whether the engine is configured to use double-precision floating-point numbers. + /// + public static bool IsDoublePrecision => sizeof(Real) == sizeof(double); +} \ No newline at end of file diff --git a/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs b/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs index 5c0acc48..63f8375b 100644 --- a/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs +++ b/src/Jitter2/SoftBodies/BroadPhaseCollisionFilter.cs @@ -52,7 +52,7 @@ public bool Filter(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) bool colliding = NarrowPhase.MPREPA(i1, i2, JQuaternion.Identity, JVector.Zero, - out JVector pA, out JVector pB, out JVector normal, out float penetration); + out JVector pA, out JVector pB, out JVector normal, out Real penetration); if (!colliding) return false; @@ -72,7 +72,7 @@ public bool Filter(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) if (!i1.SoftBody.IsActive && !rb.Data.IsActive) return false; bool colliding = NarrowPhase.MPREPA(i1, (proxyB as RigidBodyShape)!, rb.Orientation, rb.Position, - out JVector pA, out JVector pB, out JVector normal, out float penetration); + out JVector pA, out JVector pB, out JVector normal, out Real penetration); if (!colliding) return false; @@ -91,7 +91,7 @@ public bool Filter(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) if (!i2.SoftBody.IsActive && !ra.Data.IsActive) return false; bool colliding = NarrowPhase.MPREPA(i2, (proxyA as RigidBodyShape)!, ra.Orientation, ra.Position, - out JVector pA, out JVector pB, out JVector normal, out float penetration); + out JVector pA, out JVector pB, out JVector normal, out Real penetration); if (!colliding) return false; diff --git a/src/Jitter2/SoftBodies/SoftBody.cs b/src/Jitter2/SoftBodies/SoftBody.cs index c4fc9eb0..ea0ea34d 100644 --- a/src/Jitter2/SoftBodies/SoftBody.cs +++ b/src/Jitter2/SoftBodies/SoftBody.cs @@ -65,7 +65,7 @@ public void Destroy() private bool active = true; - protected virtual void WorldOnPostStep(float dt) + protected virtual void WorldOnPostStep(Real dt) { if (IsActive == active) return; active = IsActive; diff --git a/src/Jitter2/SoftBodies/SoftBodyShape.cs b/src/Jitter2/SoftBodies/SoftBodyShape.cs index 1afeee22..cdf80515 100644 --- a/src/Jitter2/SoftBodies/SoftBodyShape.cs +++ b/src/Jitter2/SoftBodies/SoftBodyShape.cs @@ -33,7 +33,7 @@ public abstract class SoftBodyShape : Shape public abstract RigidBody GetClosest(in JVector pos); public SoftBody SoftBody { get; internal init; } = null!; - public override bool RayCast(in JVector origin, in JVector direction, out JVector normal, out float lambda) + public override bool RayCast(in JVector origin, in JVector direction, out JVector normal, out Real lambda) { return NarrowPhase.RayCast(this, origin, direction, out lambda, out normal); } diff --git a/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs b/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs index 26144ff3..d90b13f6 100644 --- a/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs +++ b/src/Jitter2/SoftBodies/SoftBodyTetrahedron.cs @@ -53,7 +53,7 @@ public override JVector Velocity vel += Vertices[i].Velocity; } - vel *= 0.25f; + vel *= (Real)0.25; return vel; } @@ -61,12 +61,12 @@ public override JVector Velocity public override RigidBody GetClosest(in JVector pos) { - float dist = float.MaxValue; + Real dist = Real.MaxValue; int closest = 0; for (int i = 0; i < 4; i++) { - float len = (pos - Vertices[i].Position).LengthSquared(); + Real len = (pos - Vertices[i].Position).LengthSquared(); if (len < dist) { dist = len; @@ -79,12 +79,12 @@ public override RigidBody GetClosest(in JVector pos) public override void SupportMap(in JVector direction, out JVector result) { - float maxDot = float.MinValue; + Real maxDot = Real.MinValue; int furthest = 0; for (int i = 0; i < 4; i++) { - float dot = JVector.Dot(direction, Vertices[i].Position); + Real dot = JVector.Dot(direction, Vertices[i].Position); if (dot > maxDot) { maxDot = dot; @@ -97,13 +97,13 @@ public override void SupportMap(in JVector direction, out JVector result) public override void GetCenter(out JVector point) { - point = 0.25f * (Vertices[0].Position + Vertices[1].Position + + point = (Real)0.25 * (Vertices[0].Position + Vertices[1].Position + Vertices[2].Position + Vertices[3].Position); } - public override void UpdateWorldBoundingBox(float dt = 0.0f) + public override void UpdateWorldBoundingBox(Real dt = (Real)0.0) { - const float extraMargin = 0.01f; + const Real extraMargin = (Real)0.01; JBBox box = JBBox.SmallBox; box.AddPoint(Vertices[0].Position); diff --git a/src/Jitter2/SoftBodies/SoftBodyTriangle.cs b/src/Jitter2/SoftBodies/SoftBodyTriangle.cs index d8a06d76..74619371 100644 --- a/src/Jitter2/SoftBodies/SoftBodyTriangle.cs +++ b/src/Jitter2/SoftBodies/SoftBodyTriangle.cs @@ -37,12 +37,12 @@ public class SoftBodyTriangle : SoftBodyShape public RigidBody Vertex2 => v2; public RigidBody Vertex3 => v3; - private float halfThickness = 0.05f; + private Real halfThickness = (Real)0.05; - public float Thickness + public Real Thickness { - get => halfThickness * 2.0f; - set => halfThickness = value * 0.5f; + get => halfThickness * (Real)2.0; + set => halfThickness = value * (Real)0.5; } public SoftBodyTriangle(SoftBody body, RigidBody v1, RigidBody v2, RigidBody v3) @@ -55,13 +55,13 @@ public SoftBodyTriangle(SoftBody body, RigidBody v1, RigidBody v2, RigidBody v3) UpdateWorldBoundingBox(); } - public override JVector Velocity => 1.0f / 3.0f * (v1.Data.Velocity + v2.Data.Velocity + v3.Data.Velocity); + public override JVector Velocity => (Real)(1.0 / 3.0) * (v1.Data.Velocity + v2.Data.Velocity + v3.Data.Velocity); public override RigidBody GetClosest(in JVector pos) { - float len1 = (pos - v1.Position).LengthSquared(); - float len2 = (pos - v2.Position).LengthSquared(); - float len3 = (pos - v3.Position).LengthSquared(); + Real len1 = (pos - v1.Position).LengthSquared(); + Real len2 = (pos - v2.Position).LengthSquared(); + Real len3 = (pos - v3.Position).LengthSquared(); if (len1 < len2 && len1 < len3) { @@ -76,9 +76,9 @@ public override RigidBody GetClosest(in JVector pos) return v3; } - public override void UpdateWorldBoundingBox(float dt = 0.0f) + public override void UpdateWorldBoundingBox(Real dt = (Real)0.0) { - float extraMargin = MathF.Max(halfThickness, 0.01f); + Real extraMargin = MathR.Max(halfThickness, (Real)0.01); JBBox box = JBBox.SmallBox; @@ -100,8 +100,8 @@ public override void SupportMap(in JVector direction, out JVector result) JVector b = v2.Position; JVector c = v3.Position; - float min = JVector.Dot(a, direction); - float dot = JVector.Dot(b, direction); + Real min = JVector.Dot(a, direction); + Real dot = JVector.Dot(b, direction); result = a; @@ -123,6 +123,6 @@ public override void SupportMap(in JVector direction, out JVector result) public override void GetCenter(out JVector point) { - point = (1.0f / 3.0f) * (Vertex1.Position + Vertex2.Position + Vertex3.Position); + point = ((Real)(1.0 / 3.0)) * (Vertex1.Position + Vertex2.Position + Vertex3.Position); } } \ No newline at end of file diff --git a/src/Jitter2/SoftBodies/SpringConstraint.cs b/src/Jitter2/SoftBodies/SpringConstraint.cs index 2763f596..cef6920a 100644 --- a/src/Jitter2/SoftBodies/SpringConstraint.cs +++ b/src/Jitter2/SoftBodies/SpringConstraint.cs @@ -43,7 +43,7 @@ public struct SpringData { internal int _internal; public delegate* Iterate; - public delegate* PrepareForIteration; + public delegate* PrepareForIteration; public JHandle Body1; public JHandle Body2; @@ -51,13 +51,13 @@ public struct SpringData public JVector LocalAnchor1; public JVector LocalAnchor2; - public float BiasFactor; - public float Softness; - public float Distance; + public Real BiasFactor; + public Real Softness; + public Real Distance; - public float EffectiveMass; - public float AccumulatedImpulse; - public float Bias; + public Real EffectiveMass; + public Real AccumulatedImpulse; + public Real Bias; public JVector Jacobian; } @@ -88,12 +88,12 @@ public void Initialize(JVector anchor1, JVector anchor2) JVector.Subtract(anchor1, body1.Position, out data.LocalAnchor1); JVector.Subtract(anchor2, body2.Position, out data.LocalAnchor2); - data.Softness = 0.001f; - data.BiasFactor = 0.2f; + data.Softness = (Real)0.001; + data.BiasFactor = (Real)0.2; data.Distance = (anchor2 - anchor1).Length(); } - public float Impulse + public Real Impulse { get { @@ -136,7 +136,7 @@ public JVector Anchor2 } } - public float TargetDistance + public Real TargetDistance { set { @@ -146,7 +146,7 @@ public float TargetDistance get => handle.Data.Distance; } - public float Distance + public Real Distance { get { @@ -166,7 +166,7 @@ public float Distance } } - public static void PrepareForIteration(ref ConstraintData constraint, float idt) + public static void PrepareForIteration(ref ConstraintData constraint, Real idt) { ref SpringData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref data.Body1.Data; @@ -180,15 +180,15 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) JVector.Subtract(p2, p1, out JVector dp); - float error = dp.Length() - data.Distance; + Real error = dp.Length() - data.Distance; JVector n = p2 - p1; - if (n.LengthSquared() != 0.0f) n.Normalize(); + if (n.LengthSquared() != (Real)0.0) n.Normalize(); data.Jacobian = n; data.EffectiveMass = body1.InverseMass + body2.InverseMass; data.EffectiveMass += data.Softness * idt; - data.EffectiveMass = 1.0f / data.EffectiveMass; + data.EffectiveMass = (Real)1.0 / data.EffectiveMass; data.Bias = error * data.BiasFactor * idt; @@ -196,31 +196,31 @@ public static void PrepareForIteration(ref ConstraintData constraint, float idt) body2.Velocity += body2.InverseMass * data.AccumulatedImpulse * data.Jacobian; } - public float Softness + public Real Softness { get => handle.Data.Softness; set => handle.Data.Softness = value; } - public float Bias + public Real Bias { get => handle.Data.BiasFactor; set => handle.Data.BiasFactor = value; } - public static void Iterate(ref ConstraintData constraint, float idt) + public static void Iterate(ref ConstraintData constraint, Real idt) { ref SpringData data = ref Unsafe.AsRef(Unsafe.AsPointer(ref constraint)); ref RigidBodyData body1 = ref constraint.Body1.Data; ref RigidBodyData body2 = ref constraint.Body2.Data; - float jv = (body2.Velocity - body1.Velocity) * data.Jacobian; + Real jv = (body2.Velocity - body1.Velocity) * data.Jacobian; - float softnessScalar = data.AccumulatedImpulse * data.Softness * idt; + Real softnessScalar = data.AccumulatedImpulse * data.Softness * idt; - float lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); + Real lambda = -data.EffectiveMass * (jv + data.Bias + softnessScalar); - float oldacc = data.AccumulatedImpulse; + Real oldacc = data.AccumulatedImpulse; data.AccumulatedImpulse += lambda; lambda = data.AccumulatedImpulse - oldacc; diff --git a/src/Jitter2/UnmanagedMemory/MemoryHelper.cs b/src/Jitter2/UnmanagedMemory/MemoryHelper.cs index 4600d190..84ff3703 100644 --- a/src/Jitter2/UnmanagedMemory/MemoryHelper.cs +++ b/src/Jitter2/UnmanagedMemory/MemoryHelper.cs @@ -28,54 +28,81 @@ namespace Jitter2.UnmanagedMemory; public static unsafe class MemoryHelper { /// - /// A block of 32 bytes of memory. + /// A memory block with a size equivalent to six instances of the type. /// - [StructLayout(LayoutKind.Sequential, Size = 16)] - public struct MemBlock16 - { - } + /// + /// The struct uses sequential layout and a fixed size to ensure consistent memory alignment and layout. + /// + [StructLayout(LayoutKind.Sequential, Size = 6 * sizeof(Real))] + public struct MemBlock6Real { } /// - /// A block of 32 bytes of memory. + /// A memory block with a size equivalent to nine instances of the type. /// - [StructLayout(LayoutKind.Sequential, Size = 32)] - public struct MemBlock32 - { - } + /// + /// The struct uses sequential layout and a fixed size to ensure consistent memory alignment and layout. + /// + [StructLayout(LayoutKind.Sequential, Size = 9 * sizeof(Real))] + public struct MemBlock9Real { } /// - /// A block of 48 bytes of memory. + /// A memory block with a size equivalent to twelve instances of the type. /// - [StructLayout(LayoutKind.Sequential, Size = 48)] - public struct MemBlock48 - { - } + /// + /// The struct uses sequential layout and a fixed size to ensure consistent memory alignment and layout. + /// + [StructLayout(LayoutKind.Sequential, Size = 12 * sizeof(Real))] + public struct MemBlock12Real { } /// - /// A block of 64 bytes of memory. + /// A memory block with a size equivalent to twelve instances of the type. /// - [StructLayout(LayoutKind.Sequential, Size = 64)] - public struct MemBlock64 - { - } + /// + /// The struct uses sequential layout and a fixed size to ensure consistent memory alignment and layout. + /// + [StructLayout(LayoutKind.Sequential, Size = 16 * sizeof(Real))] + public struct MemBlock16Real { } + /// + /// Allocates a block of unmanaged memory for an array of the specified type. + /// + /// The unmanaged type of the elements to allocate memory for. + /// The number of elements to allocate memory for. + /// A pointer to the allocated memory block. public static T* AllocateHeap(int num) where T : unmanaged { return (T*)AllocateHeap(num * sizeof(T)); } + /// + /// Frees a block of unmanaged memory previously allocated for an array of the specified type. + /// + /// The unmanaged type of the elements in the memory block. + /// A pointer to the memory block to free. public static void Free(T* ptr) where T : unmanaged { Free((void*)ptr); } + /// + /// Allocates a block of unmanaged memory of the specified length in bytes. + /// + /// The length of the memory block to allocate, in bytes. + /// A pointer to the allocated memory block. public static void* AllocateHeap(int len) => NativeMemory.Alloc((nuint)len); + + /// + /// Frees a block of unmanaged memory previously allocated. + /// + /// A pointer to the memory block to free. public static void Free(void* ptr) => NativeMemory.Free(ptr); /// /// Zeros out unmanaged memory. /// - public static void Memset(void* buffer, int len) + /// A pointer to the memory block to zero out. + /// The length of the memory block to zero out, in bytes. + public static void MemSet(void* buffer, int len) { for (int i = 0; i < len; i++) { diff --git a/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs b/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs index e1465073..6e8afb08 100644 --- a/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs +++ b/src/Jitter2/UnmanagedMemory/UnmanagedActiveList.cs @@ -244,7 +244,7 @@ public JHandle Allocate(bool active = false, bool clear = false) if (clear) { - MemoryHelper.Memset((byte*)handles[hdl] + sizeof(IntPtr), + MemoryHelper.MemSet((byte*)handles[hdl] + sizeof(IntPtr), sizeof(T) - sizeof(IntPtr)); } diff --git a/src/Jitter2/World.Detect.cs b/src/Jitter2/World.Detect.cs index 09b35f2f..de877d2a 100644 --- a/src/Jitter2/World.Detect.cs +++ b/src/Jitter2/World.Detect.cs @@ -49,7 +49,7 @@ private struct ConvexHullIntersection private void PushLeft(Span left, in JVector v) { - const float epsilon = 0.001f; + const Real epsilon = (Real)0.001; if (leftCount > 0) { @@ -66,7 +66,7 @@ private void PushLeft(Span left, in JVector v) private void PushRight(Span right, in JVector v) { - const float epsilon = 0.001f; + const Real epsilon = (Real)0.001; if (rightCount > 0) { @@ -90,7 +90,7 @@ public void Reset() [System.Runtime.CompilerServices.SkipLocalsInit] public void BuildManifold(RigidBodyShape shapeA, RigidBodyShape shapeB, - in JVector pA, in JVector pB, in JVector normal, float penetration) + in JVector pA, in JVector pB, in JVector normal, Real penetration) { manifoldData ??= new JVector[12]; Reset(); @@ -109,15 +109,15 @@ static void Support(RigidBodyShape shape, in JVector direction, out JVector v) Span left = stackalloc JVector[6]; Span right = stackalloc JVector[6]; - const float sqrt3Over2 = 0.8660254f; + const Real sqrt3Over2 = (Real)0.8660254; - Span hexagonVertices = stackalloc float[] - { 1f, 0f, 0.5f, sqrt3Over2, -0.5f, sqrt3Over2, -1f, 0f, -0.5f, -sqrt3Over2, 0.5f, -sqrt3Over2 }; + Span hexagonVertices = stackalloc Real[] + {(Real)1,(Real)0, (Real)0.5, sqrt3Over2, -(Real)0.5, sqrt3Over2, -1f,(Real)0, -(Real)0.5, -sqrt3Over2, (Real)0.5, -sqrt3Over2 }; for (int e = 0; e < 6; e++) { - JVector ptNormal = normal + hexagonVertices[2 * e + 0] * 0.01f * crossVector1 + - hexagonVertices[2 * e + 1] * 0.01f * crossVector2; + JVector ptNormal = normal + hexagonVertices[2 * e + 0] * (Real)0.01 * crossVector1 + + hexagonVertices[2 * e + 1] * (Real)0.01 * crossVector2; Support(shapeA, ptNormal, out JVector np1); PushLeft(left, np1); @@ -151,13 +151,13 @@ static void Support(RigidBodyShape shape, in JVector direction, out JVector v) JVector cr2 = (b - a) % (p - a); - sameSign = JVector.Dot(cr, cr2) > 1e-3f; + sameSign = JVector.Dot(cr, cr2) > (Real)1e-3; if (!sameSign) break; } if (sameSign) { - float diff = JVector.Dot(p - pA, normal); + Real diff = JVector.Dot(p - pA, normal); mB[manifoldCount] = p; mA[manifoldCount++] = p - diff * normal; @@ -186,13 +186,13 @@ static void Support(RigidBodyShape shape, in JVector direction, out JVector v) JVector cr2 = (b - a) % (p - a); - sameSign = JVector.Dot(cr, cr2) > 1e-3f; + sameSign = JVector.Dot(cr, cr2) > (Real)1e-3; if (!sameSign) break; } if (sameSign) { - float diff = JVector.Dot(p - pB, normal); + Real diff = JVector.Dot(p - pB, normal); mA[manifoldCount] = p; mB[manifoldCount++] = p - diff * normal; @@ -239,7 +239,7 @@ public InvalidCollisionTypeException(Type proxyA, Type proxyB) /// obstacle just touch after the next velocity integration). A value below 1 is preferred, as the leftover velocity /// might be sufficient to trigger another speculative contact in the next frame. /// - public float SpeculativeRelaxationFactor { get; set; } = 0.9f; + public Real SpeculativeRelaxationFactor { get; set; } = (Real)0.9; /// /// Speculative contacts are generated when the velocity towards an obstacle exceeds @@ -247,11 +247,11 @@ public InvalidCollisionTypeException(Type proxyA, Type proxyB) /// threshold should be set to approximately D / timestep, e.g., 100 for a unit cube and a /// timestep of 0.01s. /// - public float SpeculativeVelocityThreshold { get; set; } = 10f; + public Real SpeculativeVelocityThreshold { get; set; } =(Real)10; [MethodImpl(MethodImplOptions.AggressiveInlining)] public void RegisterContact(Arbiter arbiter, in JVector point1, in JVector point2, - in JVector normal, float penetration, bool speculative = false) + in JVector normal, Real penetration, bool speculative = false) { lock (arbiter) { @@ -263,7 +263,7 @@ public void RegisterContact(Arbiter arbiter, in JVector point1, in JVector point } public void RegisterContact(ulong id0, ulong id1, RigidBody body1, RigidBody body2, - in JVector point1, in JVector point2, in JVector normal, float penetration, bool speculative = false) + in JVector point1, in JVector point2, in JVector normal, Real penetration, bool speculative = false) { GetArbiter(id0, id1, body1, body2, out Arbiter arbiter); RegisterContact(arbiter, point1, point2, normal, penetration, speculative); @@ -296,7 +296,7 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) Unsafe.SkipInit(out JVector normal); Unsafe.SkipInit(out JVector pA); Unsafe.SkipInit(out JVector pB); - float penetration; + Real penetration; Debug.Assert(sA.RigidBody != sB.RigidBody); Debug.Assert(sA.RigidBody.World == this); @@ -320,7 +320,7 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) if (!success) return; - colliding = penetration >= 0.0f; + colliding = penetration >= (Real)0.0; } else { @@ -338,9 +338,9 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) bool success = NarrowPhase.Sweep(sA, sB, b1.Orientation, b2.Orientation, b1.Position, b2.Position,b1.Velocity, b2.Velocity, - out pA, out pB, out normal, out float toi); + out pA, out pB, out normal, out Real toi); - if (!success || toi > step_dt || toi == 0.0f) return; + if (!success || toi > step_dt || toi == (Real)0.0) return; penetration = normal * (pA - pB) * SpeculativeRelaxationFactor; @@ -401,8 +401,8 @@ private void Detect(IDynamicTreeProxy proxyA, IDynamicTreeProxy proxyB) JVector mfA = cvh.ManifoldA[e]; JVector mfB = cvh.ManifoldB[e]; - float nd = JVector.Dot(mfA - mfB, normal); - if (nd < 0.0f) continue; + Real nd = JVector.Dot(mfA - mfB, normal); + if (nd < (Real)0.0) continue; arbiter.Handle.Data.AddContact(mfA, mfB, normal, nd); } diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs index 6c18a22c..09d7190e 100644 --- a/src/Jitter2/World.Step.cs +++ b/src/Jitter2/World.Step.cs @@ -90,7 +90,7 @@ public enum Timings } /// - /// Contains timings for the stages of the last call to . + /// Contains timings for the stages of the last call to . /// Array elements correspond to the enums in . Can be used to identify /// bottlenecks. /// @@ -101,16 +101,16 @@ public enum Timings /// /// The duration of time to simulate. This should remain fixed and not exceed 1/60 of a second. /// Indicates whether multithreading should be utilized. The behavior of the engine can be modified using . - public void Step(float dt, bool multiThread = true) + public void Step(Real dt, bool multiThread = true) { AssertNullBody(); - if (dt < 0.0f) + if (dt < (Real)0.0) { throw new ArgumentException("Time step cannot be negative.", nameof(dt)); } - if (dt == 0.0f) return; // nothing to do + if (dt == (Real)0.0) return; // nothing to do long time = Stopwatch.GetTimestamp(); double invFrequency = 1.0d / Stopwatch.Frequency; @@ -188,7 +188,7 @@ void SetTime(Timings type) // substep_dt = +dt; // Integrate(multiThread); ForeachActiveBody(multiThread); - + SetTime(Timings.UpdateBodies); // Perform collision detection. @@ -260,7 +260,7 @@ private void UpdateBodiesCallback(Parallel.Batch batch) private void PrepareContactsCallback(Parallel.Batch batch) { - float istep_dt = 1.0f / step_dt; + Real istep_dt = (Real)1.0 / step_dt; var span = memContacts.Active[batch.Start..batch.End]; @@ -286,7 +286,7 @@ private void PrepareContactsCallback(Parallel.Batch batch) private unsafe void PrepareSmallConstraintsCallback(Parallel.Batch batch) { - float istep_dt = 1.0f / step_dt; + Real istep_dt = (Real)1.0 / step_dt; var span = memSmallConstraints.Active[batch.Start..batch.End]; @@ -308,7 +308,7 @@ private unsafe void PrepareSmallConstraintsCallback(Parallel.Batch batch) private unsafe void IterateSmallConstraintsCallback(Parallel.Batch batch) { - float istep_dt = 1.0f / step_dt; + Real istep_dt = (Real)1.0 / step_dt; var span = memSmallConstraints.Active[batch.Start..batch.End]; @@ -328,7 +328,7 @@ private unsafe void IterateSmallConstraintsCallback(Parallel.Batch batch) private unsafe void PrepareConstraintsCallback(Parallel.Batch batch) { - float istep_dt = 1.0f / step_dt; + Real istep_dt = (Real)1.0 / step_dt; var span = memConstraints.Active[batch.Start..batch.End]; @@ -350,7 +350,7 @@ private unsafe void PrepareConstraintsCallback(Parallel.Batch batch) private unsafe void IterateConstraintsCallback(Parallel.Batch batch) { - float istep_dt = 1.0f / step_dt; + Real istep_dt = (Real)1.0 / step_dt; var span = memConstraints.Active[batch.Start..batch.End]; @@ -423,7 +423,7 @@ private void AssertNullBody() { ref RigidBodyData rigidBody = ref NullBody.Data; Debug.Assert(rigidBody.IsStatic); - Debug.Assert(rigidBody.InverseMass == 0.0f); + Debug.Assert(rigidBody.InverseMass == (Real)0.0); Debug.Assert(MathHelper.UnsafeIsZero(ref rigidBody.InverseInertiaWorld)); } @@ -435,7 +435,7 @@ private void ForeachActiveBody(bool multiThread) if (body.IsStatic) { System.Diagnostics.Debug.Assert(MathHelper.UnsafeIsZero(ref body.Data.InverseInertiaWorld)); - System.Diagnostics.Debug.Assert(body.Data.InverseMass == 0.0f); + System.Diagnostics.Debug.Assert(body.Data.InverseMass == (Real)0.0); } } #endif @@ -599,24 +599,24 @@ private void IntegrateCallback(Parallel.Batch batch) rigidBody.Position += lvel * substep_dt; - float angle = avel.Length(); + Real angle = avel.Length(); JVector axis; - if (angle < 0.001f) + if (angle < (Real)0.001) { // use Taylor's expansions of sync function - // axis = body.angularVelocity * (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle); + // axis = body.angularVelocity * ((Real)0.5 * timestep - (timestep * timestep * timestep) * ((Real)0.020833333333) * angle * angle); JVector.Multiply(avel, - 0.5f * substep_dt - substep_dt * substep_dt * substep_dt * 0.020833333333f * angle * angle, + (Real)0.5 * substep_dt - substep_dt * substep_dt * substep_dt * (Real)0.020833333333 * angle * angle, out axis); } else { // sync(fAngle) = sin(c*fAngle)/t - JVector.Multiply(avel, (float)Math.Sin(0.5f * angle * substep_dt) / angle, out axis); + JVector.Multiply(avel, MathR.Sin((Real)0.5 * angle * substep_dt) / angle, out axis); } - JQuaternion dorn = new(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * substep_dt * 0.5f)); + JQuaternion dorn = new(axis.X, axis.Y, axis.Z, MathR.Cos(angle * substep_dt * (Real)0.5)); //JQuaternion.CreateFromMatrix(rigidBody.Orientation, out JQuaternion ornA); JQuaternion ornA = rigidBody.Orientation; diff --git a/src/Jitter2/World.cs b/src/Jitter2/World.cs index bffda35d..d74bdc6e 100644 --- a/src/Jitter2/World.cs +++ b/src/Jitter2/World.cs @@ -91,7 +91,7 @@ public SpanData(World world) private readonly UnmanagedActiveList memConstraints; private readonly UnmanagedActiveList memSmallConstraints; - public delegate void WorldStep(float dt); + public delegate void WorldStep(Real dt); // Post- and Pre-step public event WorldStep? PreStep; @@ -135,9 +135,9 @@ public static (ulong min, ulong max) RequestId(int count) /// /// Defines the two available thread models. The model keeps the worker - /// threads active continuously, even when the is not in operation, which might + /// threads active continuously, even when the is not in operation, which might /// consume more CPU cycles and possibly affect the performance of other operations such as rendering. However, it ensures that the threads - /// remain 'warm' for the next invocation of . Conversely, the model allows + /// remain 'warm' for the next invocation of . Conversely, the model allows /// the worker threads to yield and undertake other tasks. /// public ThreadModelType ThreadModel { get; set; } = ThreadModelType.Regular; @@ -197,7 +197,7 @@ public static (ulong min, ulong max) RequestId(int count) } /// - /// The number of substeps for each call to . + /// The number of substeps for each call to . /// Substepping is deactivated when set to one. /// public int SubstepCount @@ -215,7 +215,7 @@ public int SubstepCount } } - private JVector gravity = new(0, -9.81f, 0); + private JVector gravity = new(0, -(Real)9.81, 0); /// /// Default gravity, see also . @@ -232,8 +232,8 @@ public JVector Gravity private volatile int velocityRelaxations = 4; private volatile int substeps = 1; - private volatile float substep_dt = 1.0f / 100.0f; - private volatile float step_dt = 1.0f / 100.0f; + private Real substep_dt = (Real)(1.0 / 100.0); + private Real step_dt = (Real)(1.0 / 100.0); /// /// Uses a slower alternative narrow phase collision detection method, instead @@ -392,7 +392,7 @@ internal void ActivateBodyNextStep(RigidBody body) internal void DeactivateBodyNextStep(RigidBody body) { - body.sleepTime = float.PositiveInfinity; + body.sleepTime = Real.PositiveInfinity; } /// diff --git a/src/JitterTests/AddRemoveTests.cs b/src/JitterTests/AddRemoveTests.cs index c05154cf..6e066ca8 100644 --- a/src/JitterTests/AddRemoveTests.cs +++ b/src/JitterTests/AddRemoveTests.cs @@ -29,7 +29,7 @@ public void RemoveStaticShape1() world.BroadPhaseFilter = new FilterOut(staticShape); - world.Step(0.01f); + world.Step((Real)0.01); Assert.That(world.DynamicTree.PotentialPairs.Count == 1); world.Clear(); @@ -47,7 +47,7 @@ public void RemoveStaticShape0() world.BroadPhaseFilter = new FilterOut(staticShape); - world.Step(0.01f); + world.Step((Real)0.01); Assert.That(world.DynamicTree.PotentialPairs.Count == 1); world.Remove(body); @@ -57,6 +57,8 @@ public void RemoveStaticShape0() [TestCase] public void AddRemoveBodies() { + void TinyStep() => world.Step((Real)1e-12); + var bA = world.CreateRigidBody(); bA.AddShape(new SphereShape()); var bB = world.CreateRigidBody(); @@ -69,23 +71,23 @@ public void AddRemoveBodies() bD.AddShape(new SphereShape()); bD.AddShape(new SphereShape()); Assert.That(world.DynamicTree.PotentialPairs.Count == 9); - world.Step(1e-12f); + TinyStep(); Assert.That(world.DynamicTree.PotentialPairs.Count == 9); world.Remove(bB); Assert.That(world.DynamicTree.PotentialPairs.Count == 5); - world.Step(1e-12f); + TinyStep(); bD.RemoveShape(bD.Shapes[0]); Assert.That(world.DynamicTree.PotentialPairs.Count == 3); - world.Step(1e-12f); + TinyStep(); world.Remove(bD); Assert.That(world.DynamicTree.PotentialPairs.Count == 1); - world.Step(1e-12f); + TinyStep(); world.NullBody.AddShape(new SphereShape()); - world.Step(1e-12f); + TinyStep(); Assert.That(world.DynamicTree.PotentialPairs.Count == 3); - world.Step(1e-12f); + TinyStep(); world.Remove(world.NullBody); Assert.That(world.DynamicTree.PotentialPairs.Count == 1); - world.Step(1e-12f); + TinyStep(); } } \ No newline at end of file diff --git a/src/JitterTests/BoundingBoxTests.cs b/src/JitterTests/BoundingBoxTests.cs index 49feb9a7..65fa571a 100644 --- a/src/JitterTests/BoundingBoxTests.cs +++ b/src/JitterTests/BoundingBoxTests.cs @@ -14,10 +14,10 @@ private static void CheckBoundingBox(RigidBodyShape shape) ShapeHelper.CalculateBoundingBox(shape, ori, pos, out JBBox shr); shape.CalculateBoundingBox(ori, pos, out JBBox sbb); - float fraction = shr.GetVolume() / sbb.GetVolume(); + Real fraction = shr.GetVolume() / sbb.GetVolume(); - Assert.That(fraction - 1e-7f, Is.LessThan(1.0f)); - Assert.That(fraction, Is.GreaterThan(0.2f)); + Assert.That(fraction - (Real)1e-7, Is.LessThan((Real)1.0)); + Assert.That(fraction, Is.GreaterThan((Real)0.2)); } [TestCase] diff --git a/src/JitterTests/CollisionTests.cs b/src/JitterTests/CollisionTests.cs index f1e2ac57..0c4861b5 100644 --- a/src/JitterTests/CollisionTests.cs +++ b/src/JitterTests/CollisionTests.cs @@ -13,10 +13,10 @@ public void Setup() [TestCase] public void NoBodyWorldBoundingBox() { - const float boxSize = 10.0f; + const Real boxSize = (Real)10.0; BoxShape shape = new BoxShape(boxSize); - Assert.That(MathHelper.CloseToZero(shape.WorldBoundingBox.Max - shape.Size * 0.5f)); - Assert.That(MathHelper.CloseToZero(shape.WorldBoundingBox.Min + shape.Size * 0.5f)); + Assert.That(MathHelper.CloseToZero(shape.WorldBoundingBox.Max - shape.Size * (Real)0.5)); + Assert.That(MathHelper.CloseToZero(shape.WorldBoundingBox.Min + shape.Size * (Real)0.5)); } [TestCase] @@ -26,24 +26,24 @@ public void OverlapDistanceTest() SphereShape ss = new SphereShape(1); var overlap = NarrowPhase.Overlap(bs, ss, - JQuaternion.CreateRotationX(0.2f), JVector.UnitY * 3.0f); + JQuaternion.CreateRotationX((Real)0.2), JVector.UnitY * (Real)3.0); var separated = NarrowPhase.Distance(bs, ss, - JQuaternion.CreateRotationX(0.2f), JVector.UnitY * 3.0f, - out JVector pA, out JVector pB, out float dist); + JQuaternion.CreateRotationX((Real)0.2), JVector.UnitY * (Real)3.0, + out JVector pA, out JVector pB, out Real dist); Assert.That(!overlap); Assert.That(separated); - Assert.That(MathF.Abs(dist - 1.5f) < 1e-4f); - Assert.That(MathHelper.CloseToZero(pA - new JVector(0, 0.5f, 0), 1e-4f)); - Assert.That(MathHelper.CloseToZero(pB - new JVector(0, 2.0f, 0), 1e-4f)); + Assert.That(MathR.Abs(dist - (Real)1.5) < (Real)1e-4); + Assert.That(MathHelper.CloseToZero(pA - new JVector(0, (Real)0.5, 0), (Real)1e-4)); + Assert.That(MathHelper.CloseToZero(pB - new JVector(0, (Real)2.0, 0), (Real)1e-4)); overlap = NarrowPhase.Overlap(bs, ss, - JQuaternion.CreateRotationX(0.2f), JVector.UnitY * 0.5f); + JQuaternion.CreateRotationX((Real)0.2), JVector.UnitY * (Real)0.5); separated = NarrowPhase.Distance(bs, ss, - JQuaternion.CreateRotationX(0.2f), JVector.UnitY * 0.5f, + JQuaternion.CreateRotationX((Real)0.2), JVector.UnitY * (Real)0.5, out pA, out pB, out dist); Assert.That(overlap); @@ -52,190 +52,190 @@ public void OverlapDistanceTest() JVector delta = new JVector(10, 13, -22); overlap = NarrowPhase.Overlap(ss, ss, - JQuaternion.CreateRotationX(0.2f), delta); + JQuaternion.CreateRotationX((Real)0.2), delta); separated = NarrowPhase.Distance(ss, ss, - JQuaternion.CreateRotationX(0.2f), delta, + JQuaternion.CreateRotationX((Real)0.2), delta, out pA, out pB, out dist); Assert.That(!overlap); Assert.That(separated); - Assert.That(MathF.Abs(dist - delta.Length() + 2) < 1e-4f); - Assert.That(MathHelper.CloseToZero(pA - JVector.Normalize(delta), 1e-4f)); - Assert.That(MathHelper.CloseToZero(pB - delta + JVector.Normalize(delta), 1e-4f)); + Assert.That(MathR.Abs(dist - delta.Length() + 2) < (Real)1e-4); + Assert.That(MathHelper.CloseToZero(pA - JVector.Normalize(delta), (Real)1e-4)); + Assert.That(MathHelper.CloseToZero(pB - delta + JVector.Normalize(delta), (Real)1e-4)); } [TestCase] public void SphereRayCast() { - SphereShape ss = new SphereShape(1.2f); + SphereShape ss = new SphereShape((Real)1.2); - const float epsilon = 1e-12f; + const Real epsilon = (Real)1e-12; - bool hit = ss.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -JVector.UnitY, out JVector normal, out float lambda); + bool hit = ss.LocalRayCast(new JVector(0, (Real)1.2 + (Real)0.25, 0), -JVector.UnitY, out JVector normal, out Real lambda); Assert.That(hit); - Assert.That(MathF.Abs(lambda - 0.25f), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda - (Real)0.25), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); - hit = ss.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -2.0f * JVector.UnitY, out normal, out lambda); + hit = ss.LocalRayCast(new JVector(0, (Real)1.2 + (Real)0.25, 0), -(Real)2.0 * JVector.UnitY, out normal, out lambda); Assert.That(hit); - Assert.That(MathF.Abs(lambda - 0.125f), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda - (Real)0.125), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); - hit = ss.LocalRayCast(new JVector(0, 1.2f - 0.25f, 0), -JVector.UnitY, out normal, out lambda); + hit = ss.LocalRayCast(new JVector(0, (Real)1.2 - (Real)0.25, 0), -JVector.UnitY, out normal, out lambda); Assert.That(hit); - Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal)); - hit = ss.LocalRayCast(new JVector(0, -1.2f - 0.25f, 0), -JVector.UnitY * 1.1f, out normal, out lambda); + hit = ss.LocalRayCast(new JVector(0, -(Real)1.2 - (Real)0.25, 0), -JVector.UnitY * (Real)1.1, out normal, out lambda); Assert.That(!hit); - Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal)); } [TestCase] public void BoxRayCast() { - BoxShape bs = new BoxShape(1.2f * 2.0f); + BoxShape bs = new BoxShape((Real)1.2 * (Real)2.0); - const float epsilon = 1e-12f; + const Real epsilon = (Real)1e-12; - bool hit = bs.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -JVector.UnitY, out JVector normal, out float lambda); + bool hit = bs.LocalRayCast(new JVector(0, (Real)1.2 + (Real)0.25, 0), -JVector.UnitY, out JVector normal, out Real lambda); Assert.That(hit); - Assert.That(MathF.Abs(lambda - 0.25f), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda - (Real)0.25), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); - hit = bs.LocalRayCast(new JVector(0, 1.2f + 0.25f, 0), -2.0f * JVector.UnitY, out normal, out lambda); + hit = bs.LocalRayCast(new JVector(0, (Real)1.2 + (Real)0.25, 0), -(Real)2.0 * JVector.UnitY, out normal, out lambda); Assert.That(hit); - Assert.That(MathF.Abs(lambda - 0.125f), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda - (Real)0.125), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal - JVector.UnitY)); - hit = bs.LocalRayCast(new JVector(0, 1.2f - 0.25f, 0), -JVector.UnitY, out normal, out lambda); + hit = bs.LocalRayCast(new JVector(0, (Real)1.2 - (Real)0.25, 0), -JVector.UnitY, out normal, out lambda); Assert.That(hit); - Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal)); - hit = bs.LocalRayCast(new JVector(0, -1.2f - 0.25f, 0), -JVector.UnitY * 1.1f, out normal, out lambda); + hit = bs.LocalRayCast(new JVector(0, -(Real)1.2 - (Real)0.25, 0), -JVector.UnitY * (Real)1.1, out normal, out lambda); Assert.That(!hit); - Assert.That(MathF.Abs(lambda), Is.LessThan(epsilon)); + Assert.That(MathR.Abs(lambda), Is.LessThan(epsilon)); Assert.That(MathHelper.CloseToZero(normal)); } [TestCase] public void RayCast() { - const float radius = 4; + const Real radius = 4; JVector sp = new JVector(10, 11, 12); JVector op = new JVector(1, 2, 3); SphereShape s1 = new(radius); - bool hit = NarrowPhase.RayCast(s1, JQuaternion.CreateRotationX(0.32f), sp, - op, sp - op, out float lambda, out JVector normal); + bool hit = NarrowPhase.RayCast(s1, JQuaternion.CreateRotationX((Real)0.32), sp, + op, sp - op, out Real lambda, out JVector normal); JVector cn = JVector.Normalize(op - sp); // analytical normal JVector hp = op + (sp - op) * lambda; // hit point Assert.That(hit); - Assert.That(MathHelper.CloseToZero(normal - cn, 1e-6f)); + Assert.That(MathHelper.CloseToZero(normal - cn, (Real)1e-6)); - float distance = (hp - sp).Length(); - Assert.That(MathF.Abs(distance - radius), Is.LessThan(1e-4f)); + Real distance = (hp - sp).Length(); + Assert.That(MathR.Abs(distance - radius), Is.LessThan((Real)1e-4)); } [TestCase] public void SweepTest() { - var s1 = new SphereShape(0.5f); + var s1 = new SphereShape((Real)0.5); var s2 = new BoxShape(1); - var rot = JQuaternion.CreateRotationZ(MathF.PI / 4.0f); + var rot = JQuaternion.CreateRotationZ(MathR.PI / (Real)4.0); var sweep = JVector.Normalize(new JVector(1, 1, 0)); bool hit = NarrowPhase.Sweep(s1, s2, rot, rot, new JVector(1, 1, 3), new JVector(11, 11, 3), - sweep, -2.0f * sweep, - out JVector pA, out JVector pB, out JVector normal, out float lambda); + sweep, -(Real)2.0 * sweep, + out JVector pA, out JVector pB, out JVector normal, out Real lambda); Assert.That(hit); - float expectedlambda = (MathF.Sqrt(200.0f) - 1.0f) * (1.0f / 3.0f); + Real expectedlambda = (MathR.Sqrt((Real)200.0) - (Real)1.0) * ((Real)(1.0 / 3.0)); JVector expectedNormal = JVector.Normalize(new JVector(1, 1, 0)); - JVector expectedPoint = new JVector(1, 1, 3) + expectedNormal * (0.5f + expectedlambda); + JVector expectedPoint = new JVector(1, 1, 3) + expectedNormal * ((Real)0.5 + expectedlambda); JVector expectedPointA = expectedPoint - sweep * lambda; - JVector expectedPointB = expectedPoint + 2.0f * sweep * lambda; + JVector expectedPointB = expectedPoint + (Real)2.0 * sweep * lambda; - Assert.That((normal - expectedNormal).LengthSquared(), Is.LessThan(1e-4f)); - Assert.That((pA - expectedPointA).LengthSquared(), Is.LessThan(1e-4f)); - Assert.That((pB - expectedPointB).LengthSquared(), Is.LessThan(1e-4f)); - Assert.That(MathF.Abs(lambda - expectedlambda), Is.LessThan(1e-4f)); + Assert.That((normal - expectedNormal).LengthSquared(), Is.LessThan((Real)1e-4)); + Assert.That((pA - expectedPointA).LengthSquared(), Is.LessThan((Real)1e-4)); + Assert.That((pB - expectedPointB).LengthSquared(), Is.LessThan((Real)1e-4)); + Assert.That(MathR.Abs(lambda - expectedlambda), Is.LessThan((Real)1e-4)); } [TestCase] public void NormalDirection() { - SphereShape s1 = new(0.5f); - SphereShape s2 = new(0.5f); + SphereShape s1 = new((Real)0.5); + SphereShape s2 = new((Real)0.5); // ----------------------------------------------- - NarrowPhase.MPREPA(s1, s2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-0.25f, 0, 0), new JVector(+0.25f, 0, 0), - out JVector pointA, out JVector pointB, out JVector normal, out float penetration); + NarrowPhase.MPREPA(s1, s2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-(Real)0.25, 0, 0), new JVector(+(Real)0.25, 0, 0), + out JVector pointA, out JVector pointB, out JVector normal, out Real penetration); // pointA is on s1 and pointB is on s2 - Assert.That(pointA.X, Is.GreaterThan(0.0f)); - Assert.That(pointB.X, Is.LessThan(0.0f)); + Assert.That(pointA.X, Is.GreaterThan((Real)0.0)); + Assert.That(pointB.X, Is.LessThan((Real)0.0)); // the collision normal points from s2 to s1 - Assert.That(normal.X, Is.GreaterThan(0.0f)); + Assert.That(normal.X, Is.GreaterThan(0)); // the separation is negative - Assert.That(penetration, Is.GreaterThan(0.0f)); + Assert.That(penetration, Is.GreaterThan(0)); // ----------------------------------------------- - NarrowPhase.Collision(s1, s2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-0.25f, 0, 0), new JVector(+0.25f, 0, 0), + NarrowPhase.Collision(s1, s2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-(Real)0.25, 0, 0), new JVector(+(Real)0.25, 0, 0), out pointA, out pointB, out normal, out penetration); // pointA is on s1 and pointB is on s2 - Assert.That(pointA.X, Is.GreaterThan(0.0f)); - Assert.That(pointB.X, Is.LessThan(0.0f)); + Assert.That(pointA.X, Is.GreaterThan(0)); + Assert.That(pointB.X, Is.LessThan(0)); // the collision normal points from s2 to s1 - Assert.That(normal.X, Is.GreaterThan(0.0f)); + Assert.That(normal.X, Is.GreaterThan(0)); // the separation is negative - Assert.That(penetration, Is.GreaterThan(0.0f)); + Assert.That(penetration, Is.GreaterThan(0)); // ----------------------------------------------- BoxShape b1 = new(1); BoxShape b2 = new(1); - NarrowPhase.MPREPA(b1, b2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-0.25f, 0.1f, 0), new JVector(+0.25f, -0.1f, 0), + NarrowPhase.MPREPA(b1, b2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-(Real)0.25, (Real)0.1, 0), new JVector(+(Real)0.25, -(Real)0.1, 0), out pointA, out pointB, out normal, out penetration); // pointA is on s1 and pointB is on s2 - Assert.That(pointA.X, Is.GreaterThan(0.0f)); - Assert.That(pointB.X, Is.LessThan(0.0f)); + Assert.That(pointA.X, Is.GreaterThan(0)); + Assert.That(pointB.X, Is.LessThan(0)); // the collision normal points from s2 to s1 - Assert.That(normal.X, Is.GreaterThan(0.0f)); + Assert.That(normal.X, Is.GreaterThan(0)); // the penetration is positive - Assert.That(penetration, Is.GreaterThan(0.0f)); + Assert.That(penetration, Is.GreaterThan(0)); // ----------------------------------------------- - NarrowPhase.Collision(b1, b2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-2.25f, 0, 0), new JVector(+2.25f, 0, 0), + NarrowPhase.Collision(b1, b2, JQuaternion.Identity, JQuaternion.Identity, new JVector(-(Real)2.25, 0, 0), new JVector(+(Real)2.25, 0, 0), out pointA, out pointB, out normal, out penetration); // the collision normal points from s2 to s1 - Assert.That(normal.X, Is.GreaterThan(0.0f)); + Assert.That(normal.X, Is.GreaterThan(0)); // the penetration is negative - Assert.That(penetration, Is.LessThan(0.0f)); + Assert.That(penetration, Is.LessThan(0)); } } \ No newline at end of file diff --git a/src/JitterTests/Helper.cs b/src/JitterTests/Helper.cs index bddf40e0..4337e01a 100644 --- a/src/JitterTests/Helper.cs +++ b/src/JitterTests/Helper.cs @@ -2,7 +2,7 @@ namespace JitterTests; public static class Helper { - public static void AdvanceWorld(World world, int seconds, float dt, bool multiThread) + public static void AdvanceWorld(World world, int seconds, Real dt, bool multiThread) { int total = (int)(seconds / dt); for (int i = 0; i < total; i++) @@ -11,7 +11,7 @@ public static void AdvanceWorld(World world, int seconds, float dt, bool multiTh public static RigidBody BuildTower(World world, JVector pos, int size = 40) { - JQuaternion halfRotationStep = JQuaternion.CreateRotationY(MathF.PI * 2.0f / 64.0f); + JQuaternion halfRotationStep = JQuaternion.CreateRotationY(MathR.PI * (Real)(2.0 / 64.0)); JQuaternion fullRotationStep = halfRotationStep * halfRotationStep; JQuaternion orientation = JQuaternion.Identity; @@ -24,9 +24,9 @@ public static RigidBody BuildTower(World world, JVector pos, int size = 40) for (int i = 0; i < 32; i++) { JVector position = pos + JVector.Transform( - new JVector(0, 0.5f + e, 19.5f), orientation); + new JVector(0, (Real)0.5 + e, (Real)19.5), orientation); - var shape = new BoxShape(3f, 1, 0.5f); + var shape = new BoxShape(3f, 1, (Real)0.5); last = world.CreateRigidBody(); @@ -51,9 +51,9 @@ public static RigidBody BuildSimpleStack(World world, int size = 12) { last = world.CreateRigidBody(); //body.AddShape(new BoxShape(1)); - last.Position = new JVector(0, 0.5f + i * 0.99f, 0); + last.Position = new JVector(0, (Real)0.5 + i * (Real)0.99, 0); last.AddShape(new BoxShape(1)); - last.Damping = (0.002f, 0.002f); + last.Damping = ((Real)0.002, (Real)0.002); if (i == 0) last.IsStatic = true; } @@ -69,7 +69,7 @@ public static RigidBody BuildPyramidBox(World world, JVector position, int size for (int e = i; e < size; e++) { last = world.CreateRigidBody(); - last.Position = position + new JVector((e - i * 0.5f) * 1.01f, 0.5f + i * 1.0f, 0.0f); + last.Position = position + new JVector((e - i * (Real)0.5) * (Real)1.01, (Real)0.5 + i * (Real)1.0, (Real)0.0); var shape = new BoxShape(1); last.AddShape(shape); @@ -88,8 +88,8 @@ public static RigidBody BuildPyramidCylinder(World world, JVector position, int for (int e = i; e < size; e++) { last = world.CreateRigidBody(); - last.Position = position + new JVector((e - i * 0.5f) * 1.01f, 0.5f + i * 1.0f, 0.0f); - var shape = new CylinderShape(1.0f, 0.5f); + last.Position = position + new JVector((e - i * (Real)0.5) * (Real)1.01, (Real)0.5 + i * (Real)1.0, (Real)0.0); + var shape = new CylinderShape((Real)1.0, (Real)0.5); last.AddShape(shape); if (i == 0) last.IsStatic = true; diff --git a/src/JitterTests/InertiaTests.cs b/src/JitterTests/InertiaTests.cs index d8519db7..b59f2d72 100644 --- a/src/JitterTests/InertiaTests.cs +++ b/src/JitterTests/InertiaTests.cs @@ -3,70 +3,70 @@ namespace JitterTests; [TestFixture] public class InertiaTests { - private static void Check(RigidBodyShape shape, JMatrix inertia, JVector com, float mass) + private static void Check(RigidBodyShape shape, JMatrix inertia, JVector com, Real mass) { - shape.CalculateMassInertia(out JMatrix shapeInertia, out JVector shapeCom, out float shapeMass); + shape.CalculateMassInertia(out JMatrix shapeInertia, out JVector shapeCom, out Real shapeMass); JMatrix dInertia = shapeInertia - inertia; - Assert.That(MathHelper.IsZero(dInertia.UnsafeGet(0), 1e-3f)); - Assert.That(MathHelper.IsZero(dInertia.UnsafeGet(1), 1e-3f)); - Assert.That(MathHelper.IsZero(dInertia.UnsafeGet(2), 1e-3f)); + Assert.That(MathHelper.IsZero(dInertia.UnsafeGet(0), (Real)1e-3)); + Assert.That(MathHelper.IsZero(dInertia.UnsafeGet(1), (Real)1e-3)); + Assert.That(MathHelper.IsZero(dInertia.UnsafeGet(2), (Real)1e-3)); - float dmass = shapeMass - mass; - Assert.That(MathF.Abs(dmass), Is.LessThan(1e-3f)); + Real dmass = shapeMass - mass; + Assert.That(MathR.Abs(dmass), Is.LessThan((Real)1e-3)); JVector dcom = shapeCom - com; - Assert.That(MathHelper.IsZero(dcom, 1e-3f)); + Assert.That(MathHelper.IsZero(dcom, (Real)1e-3)); } [TestCase] public static void CapsuleInertia() { - var ts = new CapsuleShape(0.429f, 1.7237f); - ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out float mass, 8); + var ts = new CapsuleShape((Real)0.429, (Real)1.7237); + ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out Real mass, 8); Check(ts, inertia, com, mass); } [TestCase] public static void CylinderInertia() { - var ts = new CylinderShape(0.429f, 1.7237f); - ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out float mass, 8); + var ts = new CylinderShape((Real)0.429, (Real)1.7237); + ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out Real mass, 8); Check(ts, inertia, com, mass); } [TestCase] public static void ConeInertia() { - var ts = new ConeShape(0.429f, 1.7237f); - ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out float mass, 8); + var ts = new ConeShape((Real)0.429, (Real)1.7237); + ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out Real mass, 8); Check(ts, inertia, com, mass); } [TestCase] public static void BoxInertia() { - var ts = new BoxShape(0.429f, 1.7237f, 2.11383f); - ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out float mass, 8); + var ts = new BoxShape((Real)0.429, (Real)1.7237, (Real)2.11383); + ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out Real mass, 8); Check(ts, inertia, com, mass); } [TestCase] public static void SphereInertia() { - var ts = new SphereShape(0.429f); - ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out float mass, 8); + var ts = new SphereShape((Real)0.429); + ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out Real mass, 8); Check(ts, inertia, com, mass); } [TestCase] public static void TransformedInertia() { - var ss = new SphereShape(0.429f); - var translation = new JVector(2.847f, 3.432f, 1.234f); + var ss = new SphereShape((Real)0.429); + var translation = new JVector((Real)2.847, (Real)3.432, (Real)1.234); var ts = new TransformedShape(ss, translation); - ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out float mass, 8); + ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out Real mass, 8); Check(ts, inertia, com, mass); } @@ -75,10 +75,10 @@ public static void ConvexHullInertia() { List cvh = new List(); - JVector a = new JVector(0.234f, 1.23f, 3.54f); - JVector b = new JVector(7.788f, 0.23f, 8.14f); - JVector c = new JVector(2.234f, 8.23f, 8.14f); - JVector d = new JVector(6.234f, 3.23f, 9.04f); + JVector a = new JVector((Real)0.234, (Real)1.23, (Real)3.54); + JVector b = new JVector((Real)7.788, (Real)0.23, (Real)8.14); + JVector c = new JVector((Real)2.234, (Real)8.23, (Real)8.14); + JVector d = new JVector((Real)6.234, (Real)3.23, (Real)9.04); cvh.Add(new JTriangle(a, b, c)); cvh.Add(new JTriangle(a, b, d)); @@ -86,7 +86,7 @@ public static void ConvexHullInertia() cvh.Add(new JTriangle(a, c, d)); var ts = new ConvexHullShape(cvh); - ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out float mass, 8); + ShapeHelper.CalculateMassInertia(ts, out JMatrix inertia, out JVector com, out Real mass, 8); Check(ts, inertia, com, mass); } } \ No newline at end of file diff --git a/src/JitterTests/JitterTests.csproj b/src/JitterTests/JitterTests.csproj index fd4e0ba9..5454359f 100644 --- a/src/JitterTests/JitterTests.csproj +++ b/src/JitterTests/JitterTests.csproj @@ -20,4 +20,10 @@ + + + Precision.cs + + + diff --git a/src/JitterTests/MathTests.cs b/src/JitterTests/MathTests.cs index 475ec52a..159a8825 100644 --- a/src/JitterTests/MathTests.cs +++ b/src/JitterTests/MathTests.cs @@ -12,8 +12,8 @@ public void Setup() [TestCase] public static void QMatrixProjectMultiplyLeftRight() { - JQuaternion jq1 = new(0.2f, 0.3f, 0.4f, 0.5f); - JQuaternion jq2 = new(0.1f, 0.7f, 0.1f, 0.8f); + JQuaternion jq1 = new((Real)0.2, (Real)0.3, (Real)0.4, (Real)0.5); + JQuaternion jq2 = new((Real)0.1, (Real)0.7, (Real)0.1, (Real)0.8); var qm1 = QMatrix.CreateLM(jq1); var qm2 = QMatrix.CreateRM(jq2); @@ -22,9 +22,9 @@ public static void QMatrixProjectMultiplyLeftRight() JMatrix res2 = QMatrix.ProjectMultiplyLeftRight(jq1, jq2); JMatrix delta = res1 - res2; - Assert.That(JVector.MaxAbs(delta.GetColumn(0)), Is.LessThan(1e-06f)); - Assert.That(JVector.MaxAbs(delta.GetColumn(1)), Is.LessThan(1e-06f)); - Assert.That(JVector.MaxAbs(delta.GetColumn(2)), Is.LessThan(1e-06f)); + Assert.That(JVector.MaxAbs(delta.GetColumn(0)), Is.LessThan((Real)1e-06)); + Assert.That(JVector.MaxAbs(delta.GetColumn(1)), Is.LessThan((Real)1e-06)); + Assert.That(JVector.MaxAbs(delta.GetColumn(2)), Is.LessThan((Real)1e-06)); } [TestCase] @@ -34,21 +34,21 @@ public static void TransformTests() JVector b = JVector.UnitY; JVector c = JVector.UnitZ; - Assert.That((a - b % c).Length(), Is.LessThan(1e-06f)); - Assert.That((b - c % a).Length(), Is.LessThan(1e-06f)); - Assert.That((c - a % b).Length(), Is.LessThan(1e-06f)); + Assert.That((a - b % c).Length(), Is.LessThan((Real)1e-06)); + Assert.That((b - c % a).Length(), Is.LessThan((Real)1e-06)); + Assert.That((c - a % b).Length(), Is.LessThan((Real)1e-06)); - JMatrix ar = JMatrix.CreateRotationX(0.123f) * - JMatrix.CreateRotationY(0.321f) * - JMatrix.CreateRotationZ(0.213f); + JMatrix ar = JMatrix.CreateRotationX((Real)0.123) * + JMatrix.CreateRotationY((Real)0.321) * + JMatrix.CreateRotationZ((Real)0.213); JVector.Transform(a, ar, out a); JVector.Transform(b, ar, out b); JVector.Transform(c, ar, out c); - Assert.That((a - b % c).Length(), Is.LessThan(1e-06f)); - Assert.That((b - c % a).Length(), Is.LessThan(1e-06f)); - Assert.That((c - a % b).Length(), Is.LessThan(1e-06f)); + Assert.That((a - b % c).Length(), Is.LessThan((Real)1e-06)); + Assert.That((b - c % a).Length(), Is.LessThan((Real)1e-06)); + Assert.That((c - a % b).Length(), Is.LessThan((Real)1e-06)); JMatrix.Inverse(ar, out ar); @@ -56,30 +56,30 @@ public static void TransformTests() JVector.Transform(b, ar, out b); JVector.Transform(c, ar, out c); - Assert.That((a - JVector.UnitX).Length(), Is.LessThan(1e-06f)); - Assert.That((b - JVector.UnitY).Length(), Is.LessThan(1e-06f)); - Assert.That((c - JVector.UnitZ).Length(), Is.LessThan(1e-06f)); + Assert.That((a - JVector.UnitX).Length(), Is.LessThan((Real)1e-06)); + Assert.That((b - JVector.UnitY).Length(), Is.LessThan((Real)1e-06)); + Assert.That((c - JVector.UnitZ).Length(), Is.LessThan((Real)1e-06)); // --- // https://arxiv.org/abs/1801.07478 - float cos = (float)Math.Cos(0.321f / 2.0f); - float sin = (float)Math.Sin(0.321f / 2.0f); + float cos = (float)MathR.Cos((Real)(0.321 / 2.0)); + float sin = (float)MathR.Sin((Real)(0.321 / 2.0)); JQuaternion quat1 = new(sin, 0, 0, cos); - JQuaternion quat2 = JQuaternion.CreateFromMatrix(JMatrix.CreateRotationZ(0.321f)); + JQuaternion quat2 = JQuaternion.CreateFromMatrix(JMatrix.CreateRotationZ((Real)0.321)); JQuaternion quat = JQuaternion.Multiply(quat1, quat2); JQuaternion tv = new(1, 2, 3, 0); JQuaternion tmp = JQuaternion.Multiply(JQuaternion.Multiply(quat, tv), JQuaternion.Conjugate(quat)); JVector resQuaternion = new(tmp.X, tmp.Y, tmp.Z); JVector.Transform(new JVector(1, 2, 3), JMatrix.CreateFromQuaternion(quat), out JVector resMatrix1); - Assert.That((resMatrix1 - resQuaternion).Length(), Is.LessThan(1e-06f)); + Assert.That((resMatrix1 - resQuaternion).Length(), Is.LessThan((Real)1e-06)); - JMatrix rot1 = JMatrix.CreateRotationX(0.321f); - JMatrix rot2 = JMatrix.CreateRotationZ(0.321f); + JMatrix rot1 = JMatrix.CreateRotationX((Real)0.321); + JMatrix rot2 = JMatrix.CreateRotationZ((Real)0.321); JMatrix rot = JMatrix.Multiply(rot1, rot2); JVector.Transform(new JVector(1, 2, 3), rot, out JVector resMatrix2); - Assert.That((resMatrix2 - resQuaternion).Length(), Is.LessThan(1e-06f)); + Assert.That((resMatrix2 - resQuaternion).Length(), Is.LessThan((Real)1e-06)); } } \ No newline at end of file diff --git a/src/JitterTests/StackingTests.cs b/src/JitterTests/StackingTests.cs index 2b859640..b517f096 100644 --- a/src/JitterTests/StackingTests.cs +++ b/src/JitterTests/StackingTests.cs @@ -23,11 +23,11 @@ public void SimpleStack(bool fullEPA) world.UseFullEPASolver = fullEPA; - float stackHeight = last.Position.Y; - Helper.AdvanceWorld(world, 10, 1.0f / 100.0f, true); - float delta = Math.Abs(stackHeight - last.Position.Y); + Real stackHeight = last.Position.Y; + Helper.AdvanceWorld(world, 10, (Real)(1.0 / 100.0), true); + Real delta = MathR.Abs(stackHeight - last.Position.Y); - Assert.That(delta, Is.LessThan(1f)); + Assert.That(delta, Is.LessThan(1)); } [TestCase(0, 0, 0, true)] @@ -39,9 +39,9 @@ public void PyramidStack(int x, int y, int z, bool multiThread) RigidBody last = Helper.BuildPyramidBox(world, new JVector(x, y, z)); - float stackHeight = last.Position.Y; - Helper.AdvanceWorld(world, 10, 1.0f / 100.0f, multiThread); - float delta = Math.Abs(stackHeight - last.Position.Y); + Real stackHeight = last.Position.Y; + Helper.AdvanceWorld(world, 10, (Real)(1.0 / 100.0), multiThread); + Real delta = MathR.Abs(stackHeight - last.Position.Y); Assert.That(delta, Is.LessThan(1f)); } @@ -57,9 +57,9 @@ public void PyramidStackCylinder(int x, int y, int z, bool fullEPA, bool multiTh RigidBody last = Helper.BuildPyramidCylinder(world, new JVector(x, y, z)); - float stackHeight = last.Position.Y; - Helper.AdvanceWorld(world, 10, 1.0f / 100.0f, multiThread); - float delta = Math.Abs(stackHeight - last.Position.Y); + Real stackHeight = last.Position.Y; + Helper.AdvanceWorld(world, 10, (Real)(1.0 / 100.0), multiThread); + Real delta = MathR.Abs(stackHeight - last.Position.Y); Assert.That(delta, Is.LessThan(1f)); } @@ -73,9 +73,9 @@ public void TowerStack(bool fullEPA, bool multiThread) RigidBody last = Helper.BuildTower(world, JVector.Zero, 30); - float stackHeight = last.Position.Y; - Helper.AdvanceWorld(world, 10, 1.0f / 100.0f, multiThread); - float delta = Math.Abs(stackHeight - last.Position.Y); + Real stackHeight = last.Position.Y; + Helper.AdvanceWorld(world, 10, (Real)(1.0 / 100.0), multiThread); + Real delta = MathR.Abs(stackHeight - last.Position.Y); Assert.That(delta, Is.LessThan(1f)); }