< Summary

Information
Class: FixedMathSharp.Geometry.WideConvexPrismRelations
Assembly: FixedMathSharp
File(s): File 1: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.cs
File 2: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.Projection.cs
File 3: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.ProjectionArithmetic.cs
File 4: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.RigidCylinderPairs.cs
File 5: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.RigidCylinderPairs.Rounding.cs
File 6: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.RigidFiniteShapePairs.cs
File 7: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.Support.cs
File 8: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.WideCandidate.cs
File 9: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.WideCandidate.Rounding.cs
File 10: /home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.WideCylinderPairs.cs
Line coverage
100%
Covered lines: 3583
Uncovered lines: 0
Coverable lines: 3583
Total lines: 5292
Line coverage: 100%
Branch coverage
100%
Covered branches: 380
Total branches: 380
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
File 1: TryGetCenteredCapsuleContact(...)100%11100%
File 1: TryGetCenteredCylinderContact(...)100%11100%
File 1: TryGetCenteredConeContact(...)100%11100%
File 1: TryGetAnchoredContact(...)100%22100%
File 1: TryGetContact(...)100%2020100%
File 1: TryKeepAxis(...)100%1818100%
File 1: GetPrismProjection(...)100%1818100%
File 1: GetEndpointToPrismVertexAxis(...)100%11100%
File 1: GetScaledEndpointDifference(...)100%11100%
File 1: GetPlanarEdge(...)100%11100%
File 1: GetPlanarVertexAxis(...)100%11100%
File 1: Cross(...)100%11100%
File 1: SubtractProducts(...)100%11100%
File 1: GetDifferenceProjection(...)100%11100%
File 1: GetRotatedPlanarProjection(...)100%11100%
File 1: ScaleProjection(...)100%11100%
File 2: GetShapeProjectionDepths(...)100%2222100%
File 2: IsProjectionNonNegative(...)100%66100%
File 2: CompareProjectionDepths(...)100%11100%
File 2: CompareSignedProjectionDepths(...)100%66100%
File 2: GetRadialProjection(...)100%44100%
File 2: GetRoundedDepth(...)100%44100%
File 2: GetDepthApproximation(...)100%66100%
File 2: GetDiskDepthApproximation(...)100%22100%
File 2: GetCeilingSquareRoot(...)100%22100%
File 2: GetNearestEvenIncrement(...)100%11100%
File 2: CompareProjectionDepthToTwiceRaw(...)100%88100%
File 2: IsDiskRadicalAtLeast(...)100%11100%
File 2: GetAxisProjection(...)100%11100%
File 2: GetAxisSquared(...)100%11100%
File 2: GetAxisSquared(...)100%11100%
File 2: GetPlaneSquared(...)100%11100%
File 2: GetMagnitude(...)100%22100%
File 2: BuildLocalCapsuleRadicand(...)100%11100%
File 2: BuildThresholdCapsuleRadicand(...)100%11100%
File 2: BuildLocalDiskRadicand(...)100%11100%
File 2: BuildRationalSquare(...)100%11100%
File 2: BuildThresholdRadicand(...)100%11100%
File 2: CompareRadicalPairs(...)100%44100%
File 2: ComparePositiveRadicalDifference(...)100%44100%
File 3: BuildProduct(...)100%11100%
File 3: BuildProduct(...)100%11100%
File 3: BuildProduct(...)100%11100%
File 3: BuildProduct(...)100%11100%
File 3: BuildProduct(...)100%11100%
File 3: MultiplyBy(...)100%11100%
File 3: MultiplyBy(...)100%11100%
File 3: MultiplyBy(...)100%11100%
File 3: GetMagnitude(...)100%11100%
File 3: ShiftLeft(...)100%44100%
File 3: IsZero(...)100%11100%
File 4: TryGetCenteredFiniteCylindersContact(...)100%11100%
File 4: TryGetCenteredFiniteCylindersContact(...)100%1414100%
File 4: TryKeepWideCylinderCylinderAxis(...)100%22100%
File 4: TryKeepCylinderCylinderAxis(...)100%88100%
File 4: IsCylinderPairDepthNonNegative(...)100%22100%
File 4: CompareCylinderPairDepths(...)100%44100%
File 4: BuildCylinderPairLocalRadicands(...)100%11100%
File 4: BuildCylinderPairCrossRadicands(...)100%11100%
File 4: BuildCylinderPairRationalRadicand(...)100%11100%
File 4: BuildCylinderPairDiskRadicand(...)100%11100%
File 4: BuildCylinderPairCrossRationalRadicand(...)100%11100%
File 4: BuildCylinderPairCrossDiskRadicand(...)100%66100%
File 4: MultiplyCylinderPairBy(...)100%11100%
File 4: MultiplyCylinderPairBy(...)100%11100%
File 4: MultiplyCylinderPairBy(...)100%11100%
File 4: MultiplyCylinderPairBy(...)100%11100%
File 5: GetRoundedCylinderCylinderDepth(...)100%1212100%
File 5: TryGetCylinderPairDepthApproximation(...)100%66100%
File 5: GetRoundedCylinderPairDepthBySearch(...)100%66100%
File 5: CompareCylinderPairDepthToTwiceRaw(...)100%22100%
File 6: TryGetCenteredFiniteCylinderCapsuleContact(...)100%11100%
File 6: TryGetCenteredFiniteCylinderCapsuleContact(...)100%1414100%
File 6: TryKeepCylinderCapsuleAxis(...)100%88100%
File 6: GetCenterDifferenceAxis(...)100%11100%
File 6: IsCylinderCapsuleProjectionNonNegative(...)100%22100%
File 6: GetRoundedCylinderCapsuleDepth(...)100%1212100%
File 6: TryGetCylinderCapsuleDepthApproximation(...)100%66100%
File 6: GetRoundedCylinderCapsuleDepthBySearch(...)100%66100%
File 6: CompareCylinderCapsuleDepthToTwiceRaw(...)100%44100%
File 6: GetMagnitude576(...)100%22100%
File 6: TryKeepWideCylinderCapsuleAxis(...)100%22100%
File 7: .cctor()100%11100%
File 7: .ctor(...)100%11100%
File 7: get_IsZero()100%44100%
File 7: .ctor(...)100%11100%
File 7: ToWide()100%11100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: TryGetFastDepth(...)100%2828100%
File 7: CreateFirstDiskDepth()100%11100%
File 7: CreateSecondDiskDepth()100%11100%
File 7: IsFullDiskProjection(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: TryNarrow(...)100%22100%
File 7: TryNarrowComponent(...)100%1818100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: .ctor(...)100%11100%
File 7: MaterializeContact(...)100%22100%
File 7: GetMatchedPrismOffset(...)100%1212100%
File 7: GetShapeSupportAnchor(...)100%44100%
File 7: FromDirection(...)100%11100%
File 7: Compare(...)100%11100%
File 7: Negate(...)100%11100%
File 8: TryKeepWideCylinderCapsuleAxisFallback(...)100%44100%
File 8: BuildWideAxisSquared(...)100%11100%
File 8: BuildWideDot(...)100%11100%
File 8: BuildWideScaledAlignment(...)100%11100%
File 8: BuildWideScaledCenter(...)100%11100%
File 8: BuildWidePlaneSquared(...)100%11100%
File 8: IsWideCylinderCapsuleProjectionNonNegative(...)100%11100%
File 8: IsWideBaseProjectionNonNegative(...)100%11100%
File 8: CompareWideProjectionDepth(...)100%11100%
File 8: BuildWideRationalRadicand(...)100%11100%
File 8: BuildWideRationalRadicand(...)100%11100%
File 8: BuildWideDiskRadicand(...)100%11100%
File 8: BuildWideDiskRadicand(...)100%11100%
File 8: BuildWideCapsuleRadicand(...)100%11100%
File 8: BuildNarrowRationalRadicand(...)100%11100%
File 8: BuildNarrowDiskRadicand(...)100%11100%
File 8: GetWideRadialCoefficient(...)100%11100%
File 9: GetRoundedWideCylinderCapsuleDepth(...)100%66100%
File 9: CompareWideCylinderCapsuleDepthToTwiceRaw(...)100%66100%
File 9: BuildWideThresholdRadicand(...)100%11100%
File 9: GetWideCandidateNormal(...)100%11100%
File 9: GetShiftedSigned576(...)100%66100%
File 9: CopyPositiveRadicand(...)100%22100%
File 9: CombineWideSignedMagnitudes(...)100%1616100%
File 10: TryKeepWideCylinderCylinderAxisFallback(...)100%44100%
File 10: IsWideCylinderPairDepthNonNegative(...)100%11100%
File 10: CompareWideCylinderPairDepth(...)100%11100%
File 10: BuildWideCylinderPairLocalRadicands(...)100%11100%
File 10: BuildWideCylinderPairCrossRadicands(...)100%11100%
File 10: BuildWideCylinderPairRationalRadicand(...)100%11100%
File 10: BuildWideCylinderPairDiskRadicand(...)100%66100%
File 10: BuildWideCylinderPairCrossRationalRadicand(...)100%11100%
File 10: BuildWideCylinderPairCrossDiskRadicand(...)100%11100%
File 10: BuildNarrowCylinderPairCrossRationalRadicand(...)100%11100%
File 10: BuildNarrowCylinderPairCrossDiskRadicand(...)100%44100%
File 10: MultiplyWideCylinderPairBy(...)100%11100%
File 10: MultiplyWideCylinderPairBy(...)100%11100%
File 10: MultiplyWideCylinderPairBy(...)100%11100%
File 10: MultiplyWideCylinderPairBy(...)100%11100%
File 10: GetRoundedWideCylinderPairDepth(...)100%44100%
File 10: CompareWideCylinderPairDepthToTwiceRaw(...)100%22100%

File(s)

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <summary>
 13/// Owns full-domain finite-surface versus convex-prism contact construction.
 14/// </summary>
 15internal static partial class WideConvexPrismRelations
 16{
 17    internal static bool TryGetCenteredCapsuleContact(
 18        Vector3d center,
 19        FixedQuaternion rotation,
 20        Vector3d localAxisDirection,
 21        Fixed64 axisLength,
 22        Fixed64 radius,
 23        Vector3d prismOrigin,
 24        Fixed64 prismRotation,
 25        ReadOnlySpan<Vector2d> prismLocalOffsets,
 26        Fixed64 prismHalfThickness,
 27        out FixedContactAnchors contact) =>
 3428        TryGetAnchoredContact(
 3429            FiniteShapeKind.Capsule,
 3430            center,
 3431            rotation,
 3432            localAxisDirection,
 3433            axisLength,
 3434            radius,
 3435            prismOrigin,
 3436            prismRotation,
 3437            prismLocalOffsets,
 3438            prismHalfThickness,
 3439            out contact);
 40
 41    internal static bool TryGetCenteredCylinderContact(
 42        Vector3d center,
 43        FixedQuaternion rotation,
 44        Vector3d localAxisDirection,
 45        Fixed64 axisLength,
 46        Fixed64 radius,
 47        Vector3d prismOrigin,
 48        Fixed64 prismRotation,
 49        ReadOnlySpan<Vector2d> prismLocalOffsets,
 50        Fixed64 prismHalfThickness,
 51        out FixedContactAnchors contact) =>
 1052        TryGetAnchoredContact(
 1053            FiniteShapeKind.Cylinder,
 1054            center,
 1055            rotation,
 1056            localAxisDirection,
 1057            axisLength,
 1058            radius,
 1059            prismOrigin,
 1060            prismRotation,
 1061            prismLocalOffsets,
 1062            prismHalfThickness,
 1063            out contact);
 64
 65    internal static bool TryGetCenteredConeContact(
 66        Vector3d center,
 67        FixedQuaternion rotation,
 68        Vector3d localAxisDirection,
 69        Fixed64 height,
 70        Fixed64 radius,
 71        Vector3d prismOrigin,
 72        Fixed64 prismRotation,
 73        ReadOnlySpan<Vector2d> prismLocalOffsets,
 74        Fixed64 prismHalfThickness,
 75        out FixedContactAnchors contact) =>
 776        TryGetAnchoredContact(
 777            FiniteShapeKind.Cone,
 778            center,
 779            rotation,
 780            localAxisDirection,
 781            height,
 782            radius,
 783            prismOrigin,
 784            prismRotation,
 785            prismLocalOffsets,
 786            prismHalfThickness,
 787            out contact);
 88
 89    private static bool TryGetAnchoredContact(
 90        FiniteShapeKind shapeKind,
 91        Vector3d center,
 92        FixedQuaternion shapeRotation,
 93        Vector3d localAxisDirection,
 94        Fixed64 axisLength,
 95        Fixed64 radius,
 96        Vector3d prismOrigin,
 97        Fixed64 prismRotation,
 98        ReadOnlySpan<Vector2d> prismLocalOffsets,
 99        Fixed64 prismHalfThickness,
 100        out FixedContactAnchors contact)
 101    {
 51102        if (!TryGetContact(
 51103                shapeKind,
 51104                center,
 51105                shapeRotation,
 51106                localAxisDirection,
 51107                axisLength,
 51108                radius,
 51109                prismOrigin,
 51110                prismRotation,
 51111                prismLocalOffsets,
 51112                prismHalfThickness,
 51113                out Penetration penetration))
 114        {
 11115            contact = default;
 11116            return false;
 117        }
 118
 40119        contact = new FixedContactAnchors(
 40120            penetration.ShapeAnchor,
 40121            new FixedPointAnchor(
 40122                prismOrigin,
 40123                penetration.PrismOrientation,
 40124                penetration.PrismOffset),
 40125            penetration.Normal,
 40126            penetration.Depth,
 40127            penetration.DepthIsClamped);
 40128        return true;
 129    }
 130
 131    private static bool TryGetContact(
 132        FiniteShapeKind shapeKind,
 133        Vector3d center,
 134        FixedQuaternion shapeRotation,
 135        Vector3d localAxisDirection,
 136        Fixed64 axisLength,
 137        Fixed64 radius,
 138        Vector3d prismOrigin,
 139        Fixed64 prismRotation,
 140        ReadOnlySpan<Vector2d> prismLocalOffsets,
 141        Fixed64 prismHalfThickness,
 142        out Penetration contact)
 143    {
 51144        var best = default(PenetrationCandidate);
 51145        WideOrientedBox.GetRotatedLocalAxisNumerators(
 51146            shapeRotation,
 51147            localAxisDirection,
 51148            out Signed192 shapeAxisX,
 51149            out Signed192 shapeAxisY,
 51150            out Signed192 shapeAxisZ,
 51151            out Signed192 shapeRotationDenominator);
 51152        var rigidShapeAxis = new RigidAxis3(
 51153            shapeAxisX,
 51154            shapeAxisY,
 51155            shapeAxisZ,
 51156            shapeRotationDenominator);
 51157        Axis3 shapeAxis = rigidShapeAxis.ToWide();
 51158        Axis3 up = FromDirection(Vector3d.Up);
 51159        FixedQuaternion prismOrientation =
 51160            FixedQuaternion.FromAxisAngle(
 51161                Vector3d.Up,
 51162                -prismRotation);
 51163        if (!TryKeepAxis(
 51164                up,
 51165                rigidShapeAxis,
 51166                shapeKind,
 51167                center,
 51168                axisLength,
 51169                radius,
 51170                shapeRotation,
 51171                localAxisDirection,
 51172                prismOrigin,
 51173                prismRotation,
 51174                prismOrientation,
 51175                prismLocalOffsets,
 51176                prismHalfThickness,
 51177                ref best)
 51178            || !TryKeepAxis(
 51179                shapeAxis,
 51180                rigidShapeAxis,
 51181                shapeKind,
 51182                center,
 51183                axisLength,
 51184                radius,
 51185                shapeRotation,
 51186                localAxisDirection,
 51187                prismOrigin,
 51188                prismRotation,
 51189                prismOrientation,
 51190                prismLocalOffsets,
 51191                prismHalfThickness,
 51192                ref best)
 51193            || !TryKeepAxis(
 51194                Cross(shapeAxis, up),
 51195                rigidShapeAxis,
 51196                shapeKind,
 51197                center,
 51198                axisLength,
 51199                radius,
 51200                shapeRotation,
 51201                localAxisDirection,
 51202                prismOrigin,
 51203                prismRotation,
 51204                prismOrientation,
 51205                prismLocalOffsets,
 51206                prismHalfThickness,
 51207                ref best))
 208        {
 3209            contact = default;
 3210            return false;
 211        }
 212
 418213        for (int index = 0; index < prismLocalOffsets.Length; index++)
 214        {
 169215            Vector2d start = prismLocalOffsets[index];
 169216            Vector2d end = prismLocalOffsets[
 169217                index + 1 == prismLocalOffsets.Length ? 0 : index + 1];
 169218            Axis3 edge = GetPlanarEdge(
 169219                prismRotation,
 169220                start,
 169221                end);
 169222            Axis3 face = new(
 169223                edge.Z,
 169224                default,
 169225                WideArithmetic.Negate(edge.X));
 169226            Axis3 planarVertexAxis = GetPlanarVertexAxis(
 169227                center,
 169228                prismOrigin,
 169229                prismRotation,
 169230                start);
 231            // Incident vertex axes reject every face-separated convex input;
 232            // the face remains necessary only as an overlap-depth candidate.
 169233            _ = TryKeepAxis(
 169234                face,
 169235                rigidShapeAxis,
 169236                shapeKind,
 169237                center,
 169238                axisLength,
 169239                radius,
 169240                shapeRotation,
 169241                localAxisDirection,
 169242                prismOrigin,
 169243                prismRotation,
 169244                prismOrientation,
 169245                prismLocalOffsets,
 169246                prismHalfThickness,
 169247                ref best);
 169248            if (!TryKeepAxis(
 169249                    Cross(shapeAxis, edge),
 169250                    rigidShapeAxis,
 169251                    shapeKind,
 169252                    center,
 169253                    axisLength,
 169254                    radius,
 169255                    shapeRotation,
 169256                    localAxisDirection,
 169257                    prismOrigin,
 169258                    prismRotation,
 169259                    prismOrientation,
 169260                    prismLocalOffsets,
 169261                    prismHalfThickness,
 169262                    ref best)
 169263                || !TryKeepAxis(
 169264                    planarVertexAxis,
 169265                    rigidShapeAxis,
 169266                    shapeKind,
 169267                    center,
 169268                    axisLength,
 169269                    radius,
 169270                    shapeRotation,
 169271                    localAxisDirection,
 169272                    prismOrigin,
 169273                    prismRotation,
 169274                    prismOrientation,
 169275                    prismLocalOffsets,
 169276                    prismHalfThickness,
 169277                    ref best))
 278            {
 7279                contact = default;
 7280                return false;
 281            }
 282
 162283            for (int verticalSign = -1;
 484284                verticalSign <= 1;
 322285                verticalSign += 2)
 286            {
 323287                for (int endpointSign = -1;
 968288                    endpointSign <= 1;
 645289                    endpointSign += 2)
 290                {
 646291                    Axis3 endpointAxis = GetEndpointToPrismVertexAxis(
 646292                        center,
 646293                        rigidShapeAxis,
 646294                        axisLength,
 646295                        endpointSign,
 646296                        prismOrigin,
 646297                        prismRotation,
 646298                        start,
 646299                        prismHalfThickness,
 646300                        verticalSign);
 646301                    if (!TryKeepAxis(
 646302                            endpointAxis,
 646303                            rigidShapeAxis,
 646304                            shapeKind,
 646305                            center,
 646306                            axisLength,
 646307                            radius,
 646308                            shapeRotation,
 646309                            localAxisDirection,
 646310                            prismOrigin,
 646311                            prismRotation,
 646312                            prismOrientation,
 646313                            prismLocalOffsets,
 646314                            prismHalfThickness,
 646315                            ref best))
 316                    {
 1317                        contact = default;
 1318                        return false;
 319                    }
 320                }
 321            }
 322        }
 323
 40324        contact = MaterializeContact(
 40325            best,
 40326            shapeKind,
 40327            rigidShapeAxis,
 40328            center,
 40329            shapeRotation,
 40330            localAxisDirection,
 40331            axisLength,
 40332            radius,
 40333            prismOrigin,
 40334            prismOrientation,
 40335            prismLocalOffsets,
 40336            prismHalfThickness);
 40337        return true;
 338    }
 339
 340    private static bool TryKeepAxis(
 341        Axis3 axis,
 342        RigidAxis3 shapeAxis,
 343        FiniteShapeKind shapeKind,
 344        Vector3d center,
 345        Fixed64 axisLength,
 346        Fixed64 radius,
 347        FixedQuaternion shapeRotation,
 348        Vector3d localAxisDirection,
 349        Vector3d prismOrigin,
 350        Fixed64 prismRotation,
 351        FixedQuaternion prismOrientation,
 352        ReadOnlySpan<Vector2d> prismLocalOffsets,
 353        Fixed64 prismHalfThickness,
 354        ref PenetrationCandidate best)
 355    {
 1302356        if (axis.IsZero)
 43357            return true;
 358
 1259359        GetPrismProjection(
 1259360            center,
 1259361            prismOrigin,
 1259362            prismRotation,
 1259363            prismLocalOffsets,
 1259364            prismHalfThickness,
 1259365            axis,
 1259366            out Signed576 prismMinimum,
 1259367            out Signed576 prismMaximum,
 1259368            out Vector3d minimumPrismOffset,
 1259369            out Vector3d maximumPrismOffset,
 1259370            out bool minimumPlanarTie,
 1259371            out bool maximumPlanarTie);
 1259372        GetShapeProjectionDepths(
 1259373            shapeKind,
 1259374            axis,
 1259375            shapeAxis,
 1259376            axisLength,
 1259377            radius,
 1259378            prismMinimum,
 1259379            prismMaximum,
 1259380            out ProjectionDepth positiveDepth,
 1259381            out ProjectionDepth negativeDepth);
 1259382        if (!IsProjectionNonNegative(positiveDepth)
 1259383            || !IsProjectionNonNegative(negativeDepth))
 384        {
 11385            return false;
 386        }
 387
 388        bool usePositiveNormal;
 389        bool useMinimumPrismOffset;
 390        ProjectionDepth selectedDepth;
 391        ShapeSupportFeature supportFeature;
 392        bool planarTie;
 1248393        if (prismMinimum.Sign >= 0)
 394        {
 161395            usePositiveNormal = true;
 161396            useMinimumPrismOffset = true;
 161397            selectedDepth = positiveDepth;
 161398            supportFeature = positiveDepth.SupportFeature;
 161399            planarTie = minimumPlanarTie;
 400        }
 1087401        else if (prismMaximum.Sign <= 0)
 402        {
 44403            usePositiveNormal = false;
 44404            useMinimumPrismOffset = false;
 44405            selectedDepth = negativeDepth;
 44406            supportFeature = negativeDepth.SupportFeature;
 44407            planarTie = maximumPlanarTie;
 408        }
 1043409        else if (CompareProjectionDepths(
 1043410                     positiveDepth,
 1043411                     negativeDepth) >= 0)
 412        {
 690413            usePositiveNormal = true;
 690414            useMinimumPrismOffset = false;
 690415            selectedDepth = negativeDepth;
 690416            supportFeature = negativeDepth.SupportFeature;
 690417            planarTie = maximumPlanarTie;
 418        }
 419        else
 420        {
 353421            usePositiveNormal = false;
 353422            useMinimumPrismOffset = true;
 353423            selectedDepth = positiveDepth;
 353424            supportFeature = positiveDepth.SupportFeature;
 353425            planarTie = minimumPlanarTie;
 426        }
 427
 1248428        if (best.HasValue
 1248429            && CompareProjectionDepths(
 1248430                selectedDepth,
 1248431                best.ExactDepth) >= 0)
 432        {
 1159433            return true;
 434        }
 435
 89436        Vector3d prismOffset = useMinimumPrismOffset
 89437            ? minimumPrismOffset
 89438            : maximumPrismOffset;
 89439        best = new PenetrationCandidate(
 89440            axis,
 89441            !usePositiveNormal,
 89442            supportFeature,
 89443            prismOffset,
 89444            planarTie,
 89445            selectedDepth);
 89446        return true;
 447    }
 448
 449    private static void GetPrismProjection(
 450        Vector3d sourceOrigin,
 451        Vector3d prismOrigin,
 452        Fixed64 prismRotation,
 453        ReadOnlySpan<Vector2d> offsets,
 454        Fixed64 halfThickness,
 455        Axis3 axis,
 456        out Signed576 minimum,
 457        out Signed576 maximum,
 458        out Vector3d minimumOffset,
 459        out Vector3d maximumOffset,
 460        out bool minimumPlanarTie,
 461        out bool maximumPlanarTie)
 462    {
 1259463        Signed576 originProjection = ScaleProjection(
 1259464            GetDifferenceProjection(
 1259465                prismOrigin,
 1259466                sourceOrigin,
 1259467                axis));
 1259468        Fixed64 minimumY = axis.Y.Sign >= 0
 1259469            ? -halfThickness
 1259470            : halfThickness;
 1259471        Fixed64 maximumY = -minimumY;
 1259472        int minimumIndex = 0;
 1259473        int maximumIndex = 0;
 1259474        Signed576 minimumPlanar = GetRotatedPlanarProjection(
 1259475            offsets[0],
 1259476            prismRotation,
 1259477            axis);
 1259478        Signed576 maximumPlanar = minimumPlanar;
 1259479        minimumPlanarTie = false;
 1259480        maximumPlanarTie = false;
 10142481        for (int index = 1; index < offsets.Length; index++)
 482        {
 3812483            Signed576 projection = GetRotatedPlanarProjection(
 3812484                offsets[index],
 3812485                prismRotation,
 3812486                axis);
 3812487            int minimumComparison = Compare(projection, minimumPlanar);
 3812488            if (minimumComparison < 0)
 489            {
 1061490                minimumPlanar = projection;
 1061491                minimumIndex = index;
 1061492                minimumPlanarTie = false;
 493            }
 2751494            else if (minimumComparison == 0
 2751495                && offsets[index] != offsets[minimumIndex])
 496            {
 766497                minimumPlanarTie = true;
 498            }
 499
 3812500            int maximumComparison = Compare(projection, maximumPlanar);
 3812501            if (maximumComparison > 0)
 502            {
 1022503                maximumPlanar = projection;
 1022504                maximumIndex = index;
 1022505                maximumPlanarTie = false;
 506            }
 2790507            else if (maximumComparison == 0
 2790508                && offsets[index] != offsets[maximumIndex])
 509            {
 748510                maximumPlanarTie = true;
 511            }
 512        }
 513
 1259514        Signed576 verticalRadius = ScaleProjection(
 1259515            WideArithmetic.MultiplySigned320(
 1259516                axis.Y.Sign < 0 ? WideArithmetic.Negate(axis.Y) : axis.Y,
 1259517                Signed320.ExtendValue(Signed192.Raw(halfThickness))));
 1259518        minimum = WideArithmetic.SubtractSigned576(
 1259519            WideArithmetic.AddSigned576(originProjection, minimumPlanar),
 1259520            verticalRadius);
 1259521        maximum = WideArithmetic.AddSigned576(
 1259522            WideArithmetic.AddSigned576(originProjection, maximumPlanar),
 1259523            verticalRadius);
 1259524        Vector2d minimumPlanarOffset = offsets[minimumIndex];
 1259525        Vector2d maximumPlanarOffset = offsets[maximumIndex];
 1259526        minimumOffset = new Vector3d(
 1259527            minimumPlanarOffset.X,
 1259528            minimumY,
 1259529            minimumPlanarOffset.Y);
 1259530        maximumOffset = new Vector3d(
 1259531            maximumPlanarOffset.X,
 1259532            maximumY,
 1259533            maximumPlanarOffset.Y);
 1259534    }
 535
 536    private static Axis3 GetEndpointToPrismVertexAxis(
 537        Vector3d sourceCenter,
 538        RigidAxis3 sourceAxis,
 539        Fixed64 sourceLength,
 540        int endpointSign,
 541        Vector3d prismOrigin,
 542        Fixed64 prismRotation,
 543        Vector2d prismOffset,
 544        Fixed64 prismHalfThickness,
 545        int verticalSign)
 546    {
 646547        WideConvex2dRelations.GetRelativePointNumerators(
 646548            new Vector2d(prismOrigin.X, prismOrigin.Z),
 646549            prismRotation,
 646550            prismOffset,
 646551            new Vector2d(sourceCenter.X, sourceCenter.Z),
 646552            out Signed192 relativeX,
 646553            out Signed192 relativeZ);
 646554        Signed192 relativeY = Signed192.NarrowProven(
 646555            WideArithmetic.MultiplySigned192(
 646556                WideArithmetic.AddSigned192(
 646557                    WideArithmetic.SubtractSigned192(
 646558                        Signed192.Raw(prismOrigin.Y),
 646559                        Signed192.Raw(sourceCenter.Y)),
 646560                    Signed192.Raw(verticalSign * prismHalfThickness)),
 646561                Scale));
 646562        return new Axis3(
 646563            GetScaledEndpointDifference(
 646564                relativeX,
 646565                sourceAxis.X,
 646566                sourceAxis.RotationDenominator,
 646567                sourceLength,
 646568                endpointSign),
 646569            GetScaledEndpointDifference(
 646570                relativeY,
 646571                sourceAxis.Y,
 646572                sourceAxis.RotationDenominator,
 646573                sourceLength,
 646574                endpointSign),
 646575            GetScaledEndpointDifference(
 646576                relativeZ,
 646577                sourceAxis.Z,
 646578                sourceAxis.RotationDenominator,
 646579                sourceLength,
 646580                endpointSign));
 581    }
 582
 583    private static Signed320 GetScaledEndpointDifference(
 584        Signed192 relativeNumerator,
 585        Signed192 sourceAxisNumerator,
 586        Signed192 sourceRotationDenominator,
 587        Fixed64 sourceLength,
 588        int endpointSign)
 589    {
 1938590        Signed320 scaledRelative = WideArithmetic.MultiplySigned192(
 1938591            relativeNumerator,
 1938592            sourceRotationDenominator);
 1938593        return
 1938594        WideArithmetic.SubtractSigned320(
 1938595            WideArithmetic.AddSigned320(
 1938596                scaledRelative,
 1938597                scaledRelative),
 1938598            WideArithmetic.MultiplySigned192(
 1938599                sourceAxisNumerator,
 1938600                Signed192.Signed(
 1938601                    endpointSign * sourceLength.m_rawValue)));
 602    }
 603
 604    private static Axis3 GetPlanarEdge(
 605        Fixed64 rotation,
 606        Vector2d start,
 607        Vector2d end)
 608    {
 169609        WideConvex2dRelations.GetRelativePointNumerators(
 169610            Vector2d.Zero,
 169611            rotation,
 169612            end,
 169613            Vector2d.Zero,
 169614            out Signed192 endX,
 169615            out Signed192 endY);
 169616        WideConvex2dRelations.GetRelativePointNumerators(
 169617            Vector2d.Zero,
 169618            rotation,
 169619            start,
 169620            Vector2d.Zero,
 169621            out Signed192 startX,
 169622            out Signed192 startY);
 169623        return new Axis3(
 169624            Signed320.ExtendValue(
 169625                WideArithmetic.SubtractSigned192(endX, startX)),
 169626            default,
 169627            Signed320.ExtendValue(
 169628                WideArithmetic.SubtractSigned192(endY, startY)));
 629    }
 630
 631    private static Axis3 GetPlanarVertexAxis(
 632        Vector3d sourceCenter,
 633        Vector3d prismOrigin,
 634        Fixed64 prismRotation,
 635        Vector2d prismOffset)
 636    {
 169637        WideConvex2dRelations.GetRelativePointNumerators(
 169638            new Vector2d(prismOrigin.X, prismOrigin.Z),
 169639            prismRotation,
 169640            prismOffset,
 169641            new Vector2d(sourceCenter.X, sourceCenter.Z),
 169642            out Signed192 x,
 169643            out Signed192 z);
 169644        return new Axis3(
 169645            Signed320.ExtendValue(x),
 169646            default,
 169647            Signed320.ExtendValue(z));
 648    }
 649
 650    private static Axis3 Cross(Axis3 left, Axis3 right) =>
 1267651        new(
 1267652            SubtractProducts(left.Y, right.Z, left.Z, right.Y),
 1267653            SubtractProducts(left.Z, right.X, left.X, right.Z),
 1267654            SubtractProducts(left.X, right.Y, left.Y, right.X));
 655
 656    private static Signed320 SubtractProducts(
 657        Signed320 firstLeft,
 658        Signed320 firstRight,
 659        Signed320 secondLeft,
 660        Signed320 secondRight)
 661    {
 3801662        Signed576 value = WideArithmetic.SubtractSigned576(
 3801663            WideArithmetic.MultiplySigned320(firstLeft, firstRight),
 3801664            WideArithmetic.MultiplySigned320(secondLeft, secondRight));
 3801665        _ = Signed320.TryNarrowSigned(value, out Signed320 result);
 3801666        return result;
 667    }
 668
 669    private static Signed576 GetDifferenceProjection(
 670        Vector3d end,
 671        Vector3d start,
 672        Axis3 axis) =>
 5200673        WideArithmetic.AddSigned576(
 5200674            WideArithmetic.AddSigned576(
 5200675                WideArithmetic.MultiplySigned320(
 5200676                    Signed320.ExtendValue(
 5200677                        WideArithmetic.SubtractSigned192(
 5200678                            Signed192.Raw(end.X),
 5200679                            Signed192.Raw(start.X))),
 5200680                    axis.X),
 5200681                WideArithmetic.MultiplySigned320(
 5200682                    Signed320.ExtendValue(
 5200683                        WideArithmetic.SubtractSigned192(
 5200684                            Signed192.Raw(end.Y),
 5200685                            Signed192.Raw(start.Y))),
 5200686                    axis.Y)),
 5200687            WideArithmetic.MultiplySigned320(
 5200688                Signed320.ExtendValue(
 5200689                    WideArithmetic.SubtractSigned192(
 5200690                        Signed192.Raw(end.Z),
 5200691                        Signed192.Raw(start.Z))),
 5200692                axis.Z));
 693
 694    private static Signed576 GetRotatedPlanarProjection(
 695        Vector2d offset,
 696        Fixed64 rotation,
 697        Axis3 axis)
 698    {
 5071699        WideConvex2dRelations.GetRelativePointNumerators(
 5071700            Vector2d.Zero,
 5071701            rotation,
 5071702            offset,
 5071703            Vector2d.Zero,
 5071704            out Signed192 x,
 5071705            out Signed192 y);
 5071706        return WideArithmetic.AddSigned576(
 5071707            WideArithmetic.MultiplySigned320(
 5071708                Signed320.ExtendValue(x),
 5071709                axis.X),
 5071710            WideArithmetic.MultiplySigned320(
 5071711                Signed320.ExtendValue(y),
 5071712                axis.Z));
 713    }
 714
 715    private static Signed576 ScaleProjection(Signed576 projection) =>
 2518716        WideArithmetic.MultiplySigned576(projection, Scale);
 717}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.Projection.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.Projection.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Contains projection helpers for computing wide-precision separating-axis
 14/// projection depths of convex prism shapes (capsules, disks, cones) onto arbitrary axes.
 15/// </content>
 16internal static partial class WideConvexPrismRelations
 17{
 18    private static void GetShapeProjectionDepths(
 19        FiniteShapeKind shapeKind,
 20        Axis3 axis,
 21        RigidAxis3 shapeAxis,
 22        Fixed64 axisLength,
 23        Fixed64 radius,
 24        Signed576 prismMinimum,
 25        Signed576 prismMaximum,
 26        out ProjectionDepth positive,
 27        out ProjectionDepth negative)
 28    {
 125929        Signed576 alignment = GetAxisProjection(axis, shapeAxis);
 125930        Signed704 axial = WideArithmetic.MultiplySigned576ToSigned704(
 125931            alignment,
 125932            Signed320.ExtendValue(Signed192.Raw(axisLength)));
 125933        Signed192 commonWithoutScale = WideArithmetic.AddSigned192(
 125934            shapeAxis.RotationDenominator,
 125935            shapeAxis.RotationDenominator);
 125936        Signed320 commonWide = WideArithmetic.MultiplySigned192(
 125937            commonWithoutScale,
 125938            Scale);
 125939        _ = Signed192.TryNarrowSigned(
 125940            Signed576.ExtendValue(commonWide),
 125941            out Signed192 common);
 125942        Signed704 scaledMinimum =
 125943            WideArithmetic.MultiplySigned576ToSigned704(
 125944                prismMinimum,
 125945                Signed320.ExtendValue(commonWithoutScale));
 125946        Signed704 scaledMaximum =
 125947            WideArithmetic.MultiplySigned576ToSigned704(
 125948                prismMaximum,
 125949                Signed320.ExtendValue(commonWithoutScale));
 125950        Signed576 axisSquared = GetAxisSquared(axis);
 125951        Signed320 shapeAxisSquared = GetAxisSquared(shapeAxis);
 52
 125953        if (shapeKind != FiniteShapeKind.Cone)
 54        {
 107155            Signed704 axialMagnitude = GetMagnitude(axial);
 107156            RadialKind radialKind = shapeKind == FiniteShapeKind.Capsule
 107157                ? RadialKind.Capsule
 107158                : RadialKind.Disk;
 107159            Signed832 planeSquared = radialKind == RadialKind.Disk
 107160                ? GetPlaneSquared(
 107161                    axisSquared,
 107162                    shapeAxisSquared,
 107163                    alignment)
 107164                : default;
 107165            positive = new ProjectionDepth(
 107166                WideArithmetic.SubtractSigned704(
 107167                    axialMagnitude,
 107168                    scaledMinimum),
 107169                common,
 107170                radius,
 107171                radialKind,
 107172                axisSquared,
 107173                shapeAxisSquared,
 107174                planeSquared);
 107175            negative = new ProjectionDepth(
 107176                WideArithmetic.AddSigned704(
 107177                    axialMagnitude,
 107178                    scaledMaximum),
 107179                common,
 107180                radius,
 107181                radialKind,
 107182                axisSquared,
 107183                shapeAxisSquared,
 107184                planeSquared);
 107185            return;
 86        }
 87
 18888        Signed832 conePlaneSquared = GetPlaneSquared(
 18889            axisSquared,
 18890            shapeAxisSquared,
 18891            alignment);
 18892        Signed704 doubledAxial = WideArithmetic.AddSigned704(axial, axial);
 18893        bool positiveUsesBase = doubledAxial.Sign <= 0
 18894            || IsDiskRadicalAtLeast(
 18895                common,
 18896                radius,
 18897                conePlaneSquared,
 18898                shapeAxisSquared,
 18899                doubledAxial);
 188100        Signed704 negativeBaseThreshold =
 188101            WideArithmetic.SubtractSigned704(default, doubledAxial);
 188102        bool negativeUsesBase = negativeBaseThreshold.Sign <= 0
 188103            || IsDiskRadicalAtLeast(
 188104                common,
 188105                radius,
 188106                conePlaneSquared,
 188107                shapeAxisSquared,
 188108                negativeBaseThreshold);
 188109        positive = new ProjectionDepth(
 188110            WideArithmetic.SubtractSigned704(
 188111                positiveUsesBase
 188112                    ? WideArithmetic.SubtractSigned704(default, axial)
 188113                    : axial,
 188114                scaledMinimum),
 188115            common,
 188116            radius,
 188117            positiveUsesBase ? RadialKind.Disk : RadialKind.None,
 188118            axisSquared,
 188119            shapeAxisSquared,
 188120            conePlaneSquared,
 188121            positiveUsesBase ? ShapeSupportFeature.ConeBase : ShapeSupportFeature.ConeApex);
 188122        negative = new ProjectionDepth(
 188123            WideArithmetic.SubtractSigned704(
 188124                scaledMaximum,
 188125                negativeUsesBase
 188126                    ? WideArithmetic.SubtractSigned704(default, axial)
 188127                    : axial),
 188128            common,
 188129            radius,
 188130            negativeUsesBase ? RadialKind.Disk : RadialKind.None,
 188131            axisSquared,
 188132            shapeAxisSquared,
 188133            conePlaneSquared,
 188134            negativeUsesBase ? ShapeSupportFeature.ConeBase : ShapeSupportFeature.ConeApex);
 188135    }
 136
 137    private static bool IsProjectionNonNegative(in ProjectionDepth depth)
 138    {
 11005139        if (depth.Rational.Sign >= 0)
 5763140            return true;
 5242141        if (depth.RadialKind == RadialKind.None)
 24142            return false;
 143
 5218144        if (depth.RadialKind == RadialKind.Capsule)
 145        {
 706146            Span<ulong> left = stackalloc ulong[40];
 706147            Span<ulong> right = stackalloc ulong[40];
 706148            BuildLocalCapsuleRadicand(depth, left);
 706149            BuildRationalSquare(depth.Rational, right);
 706150            return WideArithmetic.CompareMagnitudeEqualLength(left, right) >= 0;
 151        }
 152
 4512153        return IsDiskRadicalAtLeast(
 4512154            depth.Common,
 4512155            depth.Radius,
 4512156            depth.PlaneSquared,
 4512157            depth.ShapeAxisSquared,
 4512158            GetMagnitude(depth.Rational));
 159    }
 160
 161    private static int CompareProjectionDepths(
 162        in ProjectionDepth left,
 163        in ProjectionDepth right)
 164    {
 4314165        Signed576 unit = Signed576.ExtendValue(
 4314166            Signed320.ExtendValue(
 4314167                Signed192.Signed(1L)));
 4314168        GetRadialProjection(
 4314169            left,
 4314170            unit,
 4314171            out Signed320 leftCoefficient,
 4314172            out Signed832 leftNumerator,
 4314173            out Signed576 leftDenominator);
 4314174        GetRadialProjection(
 4314175            right,
 4314176            unit,
 4314177            out Signed320 rightCoefficient,
 4314178            out Signed832 rightNumerator,
 4314179            out Signed576 rightDenominator);
 4314180        return WideArithmetic.CompareRadialProjectionDepths(
 4314181            left.Rational,
 4314182            leftCoefficient,
 4314183            leftNumerator,
 4314184            leftDenominator,
 4314185            left.AxisSquared,
 4314186            right.Rational,
 4314187            rightCoefficient,
 4314188            rightNumerator,
 4314189            rightDenominator,
 4314190            right.AxisSquared);
 191    }
 192
 193    private static int CompareSignedProjectionDepths(
 194        in ProjectionDepth left,
 195        in ProjectionDepth right)
 196    {
 197        // Cylinder-capsule admission adds the same capsule radius to every
 198        // candidate, so base projections retain their ordering even when
 199        // either base is negative. Squared depths preserve order only within
 200        // one sign partition.
 1351201        bool leftNonNegative = IsProjectionNonNegative(left);
 1351202        bool rightNonNegative = IsProjectionNonNegative(right);
 1351203        if (leftNonNegative != rightNonNegative)
 253204            return leftNonNegative ? 1 : -1;
 205
 1098206        int squaredComparison = CompareProjectionDepths(left, right);
 1098207        return leftNonNegative
 1098208            ? squaredComparison
 1098209            : -squaredComparison;
 210    }
 211
 212    private static void GetRadialProjection(
 213        in ProjectionDepth depth,
 214        Signed576 unit,
 215        out Signed320 coefficient,
 216        out Signed832 numerator,
 217        out Signed576 denominator)
 218    {
 8628219        if (depth.RadialKind == RadialKind.None)
 220        {
 930221            coefficient = default;
 930222            numerator = default;
 930223            denominator = unit;
 930224            return;
 225        }
 226
 7698227        coefficient = WideArithmetic.MultiplySigned192(
 7698228            depth.Common,
 7698229            Signed192.Raw(depth.Radius));
 7698230        if (depth.RadialKind == RadialKind.Capsule)
 231        {
 3691232            numerator = Signed832.ExtendValue(
 3691233                depth.AxisSquared);
 3691234            denominator = unit;
 235        }
 236        else
 237        {
 4007238            numerator = depth.PlaneSquared;
 4007239            denominator = Signed576.ExtendValue(
 4007240                depth.ShapeAxisSquared);
 241        }
 4007242    }
 243
 244    private static Fixed64 GetRoundedDepth(
 245        in ProjectionDepth depth,
 246        out bool isClamped)
 247    {
 40248        if (CompareProjectionDepthToTwiceRaw(
 40249                depth,
 40250                new Signed192(0UL, 0UL, unchecked((ulong)long.MaxValue << 1))) > 0)
 251        {
 2252            isClamped = true;
 2253            return Fixed64.MaxValue;
 254        }
 255
 38256        Fixed64 approximation = GetDepthApproximation(depth);
 257        // Sign-aware rational roots and one-sided disk roots keep this estimate
 258        // at or below the exact depth. The admitted Q32-scaled axes keep exact
 259        // upward correction within five raw units.
 260        while (true)
 261        {
 45262            Signed192 upperMidpoint = new(
 45263                0UL,
 45264                0UL,
 45265                unchecked((ulong)approximation.m_rawValue << 1) | 1UL);
 45266            int upperComparison =
 45267                CompareProjectionDepthToTwiceRaw(
 45268                    depth,
 45269                    upperMidpoint);
 45270            if ((approximation < Fixed64.MaxValue)
 45271                & (upperComparison
 45272                    + (approximation.m_rawValue & 1L) > 0))
 273            {
 7274                approximation = Fixed64.FromRaw(
 7275                    approximation.m_rawValue + 1L);
 7276                continue;
 277            }
 278
 38279            isClamped = false;
 38280            return approximation;
 281        }
 282    }
 283
 284    private static Fixed64 GetDepthApproximation(
 285        in ProjectionDepth depth)
 286    {
 38287        Signed704 axisSquared =
 38288            Signed704.ExtendValue(depth.AxisSquared);
 38289        Signed576 axisFloor =
 38290            WideArithmetic.GetFloorSquareRoot(axisSquared);
 38291        Signed576 axisCeiling =
 38292            GetCeilingSquareRoot(axisSquared, axisFloor);
 38293        Signed576 rationalAxisLength = depth.Rational.Sign < 0
 38294            ? axisFloor
 38295            : axisCeiling;
 38296        Signed576 rationalDenominator = WideArithmetic.MultiplySigned576(
 38297            rationalAxisLength,
 38298            depth.Common);
 38299        Fixed64 radial = depth.RadialKind switch
 38300        {
 26301            RadialKind.Capsule => depth.Radius,
 11302            RadialKind.Disk => GetDiskDepthApproximation(
 11303                depth,
 11304                axisCeiling),
 1305            _ => Fixed64.Zero,
 38306        };
 38307        Signed704 combinedNumerator = WideArithmetic.AddSigned704(
 38308            depth.Rational,
 38309            WideArithmetic.MultiplySigned576ToSigned704(
 38310                rationalDenominator,
 38311                Signed320.ExtendValue(Signed192.Raw(radial))));
 312        // Cancel rational and radial support before narrowing so individually
 313        // out-of-domain terms cannot discard a representable shallow depth.
 38314        return Fixed64.GetNonNegativeRawRatioFloor(
 38315            combinedNumerator,
 38316            Signed704.ExtendValue(rationalDenominator));
 317    }
 318
 319    private static Fixed64 GetDiskDepthApproximation(
 320        in ProjectionDepth depth,
 321        Signed576 axisCeiling)
 322    {
 1023323        Signed576 planeLength =
 1023324            WideArithmetic.GetFloorSquareRootOfProduct(
 1023325                depth.PlaneSquared,
 1023326                Signed192.Signed(1L));
 1023327        Signed192 shapeAxisFloor = WideArithmetic.GetFloorSquareRoot(
 1023328            depth.ShapeAxisSquared,
 1023329            out Signed192 shapeAxisRemainder);
 1023330        Signed192 shapeAxisCeiling = shapeAxisRemainder.IsZero
 1023331            ? shapeAxisFloor
 1023332            : WideArithmetic.AddSigned192(
 1023333                shapeAxisFloor,
 1023334                Signed192.Signed(1L));
 1023335        Signed576 numerator = WideArithmetic.MultiplySigned576(
 1023336            planeLength,
 1023337            Signed192.Raw(depth.Radius));
 1023338        Signed576 denominator = WideArithmetic.MultiplySigned576(
 1023339            axisCeiling,
 1023340            shapeAxisCeiling);
 1023341        return Fixed64.GetNonNegativeRawRatioFloor(
 1023342            Signed704.ExtendValue(numerator),
 1023343            Signed704.ExtendValue(denominator));
 344    }
 345
 346    private static Signed576 GetCeilingSquareRoot(
 347        Signed704 squared,
 348        Signed576 floor)
 349    {
 38350        _ = Signed320.TryNarrowSigned(
 38351            floor,
 38352            out Signed320 narrowFloor);
 38353        Signed704 floorSquared =
 38354            WideArithmetic.MultiplySigned576ToSigned704(
 38355                floor,
 38356                narrowFloor);
 38357        return WideArithmetic.SubtractSigned704(
 38358                squared,
 38359                floorSquared).IsZero
 38360            ? floor
 38361            : WideArithmetic.AddSigned576(
 38362                floor,
 38363                Signed576.ExtendValue(
 38364                    Signed320.ExtendValue(
 38365                        Signed192.Signed(1L))));
 366    }
 367
 368    private static ulong GetNearestEvenIncrement(
 369        int midpointComparison,
 370        ulong floor) =>
 164371        unchecked((ulong)(
 164372            (midpointComparison
 164373                + (int)(floor & 1UL)
 164374                + 1) >> 1));
 375
 376    private static int CompareProjectionDepthToTwiceRaw(
 377        in ProjectionDepth depth,
 378        Signed192 twiceRaw)
 379    {
 409380        Span<ulong> leftFirst = stackalloc ulong[40];
 409381        Span<ulong> leftSecond = stackalloc ulong[40];
 409382        Span<ulong> rightFirst = stackalloc ulong[40];
 409383        Span<ulong> rightSecond = stackalloc ulong[40];
 409384        leftFirst.Clear();
 409385        leftSecond.Clear();
 409386        rightFirst.Clear();
 409387        rightSecond.Clear();
 388
 409389        Span<ulong> rationalTerm = depth.Rational.Sign >= 0
 409390            ? leftFirst
 409391            : rightFirst;
 409392        BuildProduct(
 409393            depth.Rational,
 409394            depth.Rational,
 409395            depth.ShapeAxisSquared,
 409396            rationalTerm);
 409397        ShiftLeft(rationalTerm, 2);
 409398        if (depth.RadialKind != RadialKind.None)
 399        {
 406400            Span<ulong> radialTerm = IsZero(leftFirst)
 406401                ? leftFirst
 406402                : leftSecond;
 406403            if (depth.RadialKind == RadialKind.Capsule)
 56404                BuildThresholdCapsuleRadicand(depth, radialTerm);
 405            else
 350406                BuildLocalDiskRadicand(depth, radialTerm);
 406407            ShiftLeft(radialTerm, 2);
 408        }
 409
 409410        Signed320 thresholdCoefficient =
 409411            WideArithmetic.MultiplySigned192(depth.Common, twiceRaw);
 409412        BuildThresholdRadicand(
 409413            thresholdCoefficient,
 409414            depth.ShapeAxisSquared,
 409415            depth.AxisSquared,
 409416            rightSecond);
 409417        return CompareRadicalPairs(
 409418            leftFirst,
 409419            leftSecond,
 409420            rightFirst,
 409421            rightSecond);
 422    }
 423
 424    private static bool IsDiskRadicalAtLeast(
 425        Signed192 common,
 426        Fixed64 radius,
 427        Signed832 planeSquared,
 428        Signed320 shapeAxisSquared,
 429        Signed704 threshold)
 430    {
 4636431        Span<ulong> radial = stackalloc ulong[40];
 4636432        Span<ulong> rational = stackalloc ulong[40];
 4636433        Signed320 coefficient = WideArithmetic.MultiplySigned192(
 4636434            common,
 4636435            Signed192.Raw(radius));
 4636436        BuildProduct(coefficient, coefficient, planeSquared, radial);
 4636437        BuildProduct(threshold, threshold, shapeAxisSquared, rational);
 4636438        return WideArithmetic.CompareMagnitudeEqualLength(radial, rational) >= 0;
 439    }
 440
 441    private static Signed576 GetAxisProjection(
 442        Axis3 axis,
 443        RigidAxis3 direction) =>
 9181444        WideArithmetic.AddSigned576(
 9181445            WideArithmetic.AddSigned576(
 9181446                WideArithmetic.MultiplySigned320(
 9181447                    axis.X,
 9181448                    Signed320.ExtendValue(direction.X)),
 9181449                WideArithmetic.MultiplySigned320(
 9181450                    axis.Y,
 9181451                    Signed320.ExtendValue(direction.Y))),
 9181452            WideArithmetic.MultiplySigned320(
 9181453                axis.Z,
 9181454                Signed320.ExtendValue(direction.Z)));
 455
 456    private static Signed576 GetAxisSquared(Axis3 axis) =>
 5200457        WideArithmetic.AddSigned576(
 5200458            WideArithmetic.AddSigned576(
 5200459                WideArithmetic.MultiplySigned320(axis.X, axis.X),
 5200460                WideArithmetic.MultiplySigned320(axis.Y, axis.Y)),
 5200461            WideArithmetic.MultiplySigned320(axis.Z, axis.Z));
 462
 463    private static Signed320 GetAxisSquared(RigidAxis3 axis) =>
 7455464        WideArithmetic.AddSigned320(
 7455465            WideArithmetic.AddSigned320(
 7455466                WideArithmetic.MultiplySigned192(axis.X, axis.X),
 7455467                WideArithmetic.MultiplySigned192(axis.Y, axis.Y)),
 7455468            WideArithmetic.MultiplySigned192(axis.Z, axis.Z));
 469
 470    private static Signed832 GetPlaneSquared(
 471        Signed576 axisSquared,
 472        Signed320 shapeAxisSquared,
 473        Signed576 alignment) =>
 6203474        WideArithmetic.SubtractSigned832(
 6203475            WideArithmetic.MultiplyNonNegativeToSigned832(
 6203476                Signed832.ExtendValue(axisSquared),
 6203477                shapeAxisSquared),
 6203478            WideArithmetic.MultiplySigned576ToSigned832(
 6203479                alignment,
 6203480                alignment));
 481
 482    private static Signed704 GetMagnitude(Signed704 value) =>
 5583483        value.Sign >= 0
 5583484            ? value
 5583485            : WideArithmetic.SubtractSigned704(default, value);
 486
 487    private static void BuildLocalCapsuleRadicand(
 488        in ProjectionDepth depth,
 489        Span<ulong> result)
 490    {
 706491        Signed320 coefficient = WideArithmetic.MultiplySigned192(
 706492            depth.Common,
 706493            Signed192.Raw(depth.Radius));
 706494        BuildProduct(
 706495            coefficient,
 706496            coefficient,
 706497            depth.AxisSquared,
 706498            result);
 706499    }
 500
 501    private static void BuildThresholdCapsuleRadicand(
 502        in ProjectionDepth depth,
 503        Span<ulong> result)
 504    {
 732505        Signed320 coefficient = WideArithmetic.MultiplySigned192(
 732506            depth.Common,
 732507            Signed192.Raw(depth.Radius));
 732508        BuildProduct(
 732509            coefficient,
 732510            coefficient,
 732511            depth.ShapeAxisSquared,
 732512            depth.AxisSquared,
 732513            result);
 732514    }
 515
 516    private static void BuildLocalDiskRadicand(
 517        in ProjectionDepth depth,
 518        Span<ulong> result)
 519    {
 3012520        Signed320 coefficient = WideArithmetic.MultiplySigned192(
 3012521            depth.Common,
 3012522            Signed192.Raw(depth.Radius));
 3012523        BuildProduct(
 3012524            coefficient,
 3012525            coefficient,
 3012526            depth.PlaneSquared,
 3012527            result);
 3012528    }
 529
 530    private static void BuildRationalSquare(
 531        Signed704 rational,
 532        Span<ulong> result) =>
 706533        BuildProduct(rational, rational, result);
 534
 535    private static void BuildThresholdRadicand(
 536        Signed320 coefficient,
 537        Signed320 shapeAxisSquared,
 538        Signed576 axisSquared,
 539        Span<ulong> result) =>
 2395540        BuildProduct(
 2395541            coefficient,
 2395542            coefficient,
 2395543            shapeAxisSquared,
 2395544            axisSquared,
 2395545            result);
 546
 547    private static int CompareRadicalPairs(
 548        ReadOnlySpan<ulong> leftFirst,
 549        ReadOnlySpan<ulong> leftSecond,
 550        ReadOnlySpan<ulong> rightFirst,
 551        ReadOnlySpan<ulong> rightSecond)
 552    {
 6917553        int termWords = leftFirst.Length;
 6917554        int productWords = termWords * 2;
 6917555        Span<ulong> leftBase = stackalloc ulong[termWords];
 6917556        Span<ulong> rightBase = stackalloc ulong[termWords];
 6917557        WideArithmetic.AddEqualMagnitudes(leftFirst, leftSecond, leftBase);
 6917558        WideArithmetic.AddEqualMagnitudes(rightFirst, rightSecond, rightBase);
 6917559        int baseComparison = WideArithmetic.CompareMagnitudeEqualLength(leftBase, rightBase);
 6917560        Span<ulong> baseMagnitude = stackalloc ulong[termWords];
 6917561        if (baseComparison >= 0)
 3251562            WideArithmetic.SubtractEqualMagnitudes(leftBase, rightBase, baseMagnitude);
 563        else
 3666564            WideArithmetic.SubtractEqualMagnitudes(rightBase, leftBase, baseMagnitude);
 565
 6917566        Span<ulong> leftProduct = stackalloc ulong[productWords];
 6917567        Span<ulong> rightProduct = stackalloc ulong[productWords];
 6917568        WideArithmetic.MultiplyMagnitudes(leftFirst, leftSecond, leftProduct);
 6917569        WideArithmetic.MultiplyMagnitudes(rightFirst, rightSecond, rightProduct);
 6917570        if (baseComparison >= 0)
 571        {
 3251572            return ComparePositiveRadicalDifference(
 3251573                baseMagnitude,
 3251574                leftProduct,
 3251575                rightProduct);
 576        }
 577
 3666578        return -ComparePositiveRadicalDifference(
 3666579            baseMagnitude,
 3666580            rightProduct,
 3666581            leftProduct);
 582    }
 583
 584    private static int ComparePositiveRadicalDifference(
 585        ReadOnlySpan<ulong> positiveBase,
 586        ReadOnlySpan<ulong> sameSideProduct,
 587        ReadOnlySpan<ulong> oppositeSideProduct)
 588    {
 6917589        int productWords = sameSideProduct.Length;
 6917590        int squaredWords = productWords * 2;
 6917591        Span<ulong> baseSquared = stackalloc ulong[productWords];
 6917592        Span<ulong> fourSame = stackalloc ulong[productWords];
 6917593        Span<ulong> fourOpposite = stackalloc ulong[productWords];
 6917594        WideArithmetic.MultiplyMagnitudes(positiveBase, positiveBase, baseSquared);
 6917595        sameSideProduct.CopyTo(fourSame);
 6917596        oppositeSideProduct.CopyTo(fourOpposite);
 6917597        ShiftLeft(fourSame, 2);
 6917598        ShiftLeft(fourOpposite, 2);
 599
 6917600        Span<ulong> knownLeft = stackalloc ulong[productWords];
 6917601        WideArithmetic.AddEqualMagnitudes(baseSquared, fourSame, knownLeft);
 6917602        int knownComparison = WideArithmetic.CompareMagnitudeEqualLength(
 6917603            knownLeft,
 6917604            fourOpposite);
 6917605        if (knownComparison > 0)
 5100606            return 1;
 607
 1817608        Span<ulong> remainder = stackalloc ulong[productWords];
 1817609        if (knownComparison == 0)
 610        {
 69611            return Math.Sign(
 69612                WideArithmetic.GetActiveMagnitudeLength(positiveBase)
 69613                * WideArithmetic.GetActiveMagnitudeLength(sameSideProduct));
 614        }
 615
 1748616        WideArithmetic.SubtractEqualMagnitudes(fourOpposite, knownLeft, remainder);
 1748617        Span<ulong> crossSquared = stackalloc ulong[squaredWords];
 1748618        Span<ulong> remainderSquared = stackalloc ulong[squaredWords];
 1748619        WideArithmetic.MultiplyMagnitudes(baseSquared, sameSideProduct, crossSquared);
 1748620        ShiftLeft(crossSquared, 4);
 1748621        WideArithmetic.MultiplyMagnitudes(remainder, remainder, remainderSquared);
 1748622        return WideArithmetic.CompareMagnitudeEqualLength(crossSquared, remainderSquared);
 623    }
 624}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.ProjectionArithmetic.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.ProjectionArithmetic.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Wide-precision (multi-word) arithmetic helpers used to build and manipulate
 14/// magnitude spans for high-precision projection products, including
 15/// multiplication, addition, subtraction, shifting, and comparison of
 16/// arbitrary-length unsigned word arrays.
 17/// </content>
 18internal static partial class WideConvexPrismRelations
 19{
 20    private static void BuildProduct(
 21        Signed704 first,
 22        Signed704 second,
 23        Span<ulong> result)
 24    {
 841325        Span<ulong> firstWords = stackalloc ulong[11];
 841326        Span<ulong> secondWords = stackalloc ulong[11];
 841327        WideArithmetic.GetMagnitude(first, firstWords);
 841328        WideArithmetic.GetMagnitude(second, secondWords);
 841329        WideArithmetic.MultiplyMagnitudes(firstWords, secondWords, result);
 841330    }
 31
 32    private static void BuildProduct(
 33        Signed704 first,
 34        Signed704 second,
 35        Signed320 third,
 36        Span<ulong> result)
 37    {
 770738        BuildProduct(first, second, result);
 770739        MultiplyBy(result, third);
 770740    }
 41
 42    private static void BuildProduct(
 43        Signed320 first,
 44        Signed320 second,
 45        Signed576 third,
 46        Span<ulong> result)
 47    {
 70648        Span<ulong> firstWords = stackalloc ulong[5];
 70649        Span<ulong> secondWords = stackalloc ulong[5];
 70650        GetMagnitude(first, firstWords);
 70651        GetMagnitude(second, secondWords);
 70652        WideArithmetic.MultiplyMagnitudes(firstWords, secondWords, result);
 70653        MultiplyBy(result, third);
 70654    }
 55
 56    private static void BuildProduct(
 57        Signed320 first,
 58        Signed320 second,
 59        Signed832 third,
 60        Span<ulong> result)
 61    {
 764862        Span<ulong> firstWords = stackalloc ulong[5];
 764863        Span<ulong> secondWords = stackalloc ulong[5];
 764864        Span<ulong> thirdWords = stackalloc ulong[13];
 764865        GetMagnitude(first, firstWords);
 764866        GetMagnitude(second, secondWords);
 764867        WideArithmetic.GetMagnitude(third, thirdWords);
 764868        Span<ulong> intermediate = stackalloc ulong[40];
 764869        WideArithmetic.MultiplyMagnitudes(firstWords, secondWords, intermediate);
 764870        WideArithmetic.MultiplyMagnitudes(intermediate, thirdWords, result);
 764871    }
 72
 73    private static void BuildProduct(
 74        Signed320 first,
 75        Signed320 second,
 76        Signed320 third,
 77        Signed576 fourth,
 78        Span<ulong> result)
 79    {
 312780        Span<ulong> firstWords = stackalloc ulong[5];
 312781        Span<ulong> secondWords = stackalloc ulong[5];
 312782        GetMagnitude(first, firstWords);
 312783        GetMagnitude(second, secondWords);
 312784        WideArithmetic.MultiplyMagnitudes(firstWords, secondWords, result);
 312785        MultiplyBy(result, third);
 312786        MultiplyBy(result, fourth);
 312787    }
 88
 89    private static void MultiplyBy(
 90        Span<ulong> value,
 91        Signed320 factor)
 92    {
 1083493        Span<ulong> words = stackalloc ulong[5];
 1083494        GetMagnitude(factor, words);
 1083495        MultiplyBy(value, words);
 1083496    }
 97
 98    private static void MultiplyBy(
 99        Span<ulong> value,
 100        Signed576 factor)
 101    {
 3833102        Span<ulong> words = stackalloc ulong[9];
 3833103        WideArithmetic.GetMagnitude(factor, words);
 3833104        MultiplyBy(value, words);
 3833105    }
 106
 107    private static void MultiplyBy(
 108        Span<ulong> value,
 109        ReadOnlySpan<ulong> factor)
 110    {
 14667111        Span<ulong> product = stackalloc ulong[value.Length];
 14667112        WideArithmetic.MultiplyMagnitudes(value, factor, product);
 14667113        product.CopyTo(value);
 14667114    }
 115
 116    private static void GetMagnitude(
 117        Signed320 value,
 118        Span<ulong> result) =>
 109299119        WideArithmetic.GetMagnitude(
 109299120            value,
 109299121            out result[4],
 109299122            out result[3],
 109299123            out result[2],
 109299124            out result[1],
 109299125            out result[0]);
 126
 127    private static void ShiftLeft(Span<ulong> value, int bits)
 128    {
 274886129        for (int iteration = 0; iteration < bits; iteration++)
 130        {
 92794131            ulong carry = 0UL;
 33229268132            for (int index = 0; index < value.Length; index++)
 133            {
 16521840134                ulong nextCarry = value[index] >> 63;
 16521840135                value[index] = (value[index] << 1) | carry;
 16521840136                carry = nextCarry;
 137            }
 138        }
 44649139    }
 140
 141    private static bool IsZero(ReadOnlySpan<ulong> value) =>
 14264142        WideArithmetic.GetActiveMagnitudeLength(value) == 0;
 143}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.RigidCylinderPairs.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.RigidCylinderPairs.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Contains routines for computing contact information between pairs of rigid, finite cylinders.
 14/// </content>
 15internal static partial class WideConvexPrismRelations
 16{
 17    internal static bool TryGetCenteredFiniteCylindersContact(
 18        Vector3d firstCenter,
 19        FixedQuaternion firstRotation,
 20        Vector3d firstLocalAxis,
 21        Fixed64 firstLength,
 22        Fixed64 firstRadius,
 23        Vector3d secondCenter,
 24        FixedQuaternion secondRotation,
 25        Vector3d secondLocalAxis,
 26        Fixed64 secondLength,
 27        Fixed64 secondRadius,
 28        out FixedContactAnchors contact) =>
 11729        TryGetCenteredFiniteCylindersContact(
 11730            firstCenter,
 11731            firstRotation,
 11732            firstLocalAxis,
 11733            firstLength,
 11734            firstRadius,
 11735            secondCenter,
 11736            secondRotation,
 11737            secondLocalAxis,
 11738            secondLength,
 11739            secondRadius,
 11740            out contact,
 11741            out _,
 11742            out _);
 43
 44    internal static bool TryGetCenteredFiniteCylindersContact(
 45        Vector3d firstCenter,
 46        FixedQuaternion firstRotation,
 47        Vector3d firstLocalAxis,
 48        Fixed64 firstLength,
 49        Fixed64 firstRadius,
 50        Vector3d secondCenter,
 51        FixedQuaternion secondRotation,
 52        Vector3d secondLocalAxis,
 53        Fixed64 secondLength,
 54        Fixed64 secondRadius,
 55        out FixedContactAnchors contact,
 56        out bool usedWideCandidate,
 57        out bool usedMultiRadicalRanking)
 58    {
 64059        usedWideCandidate = false;
 64060        usedMultiRadicalRanking = false;
 64061        WideOrientedBox.GetRotatedLocalAxisNumerators(
 64062            firstRotation,
 64063            firstLocalAxis,
 64064            out Signed192 firstAxisX,
 64065            out Signed192 firstAxisY,
 64066            out Signed192 firstAxisZ,
 64067            out Signed192 firstDenominator);
 64068        WideOrientedBox.GetRotatedLocalAxisNumerators(
 64069            secondRotation,
 64070            secondLocalAxis,
 64071            out Signed192 secondAxisX,
 64072            out Signed192 secondAxisY,
 64073            out Signed192 secondAxisZ,
 64074            out Signed192 secondDenominator);
 64075        var firstAxis = new RigidAxis3(
 64076            firstAxisX,
 64077            firstAxisY,
 64078            firstAxisZ,
 64079            firstDenominator);
 64080        var secondAxis = new RigidAxis3(
 64081            secondAxisX,
 64082            secondAxisY,
 64083            secondAxisZ,
 64084            secondDenominator);
 64085        Axis3 firstCandidate = firstAxis.ToWide();
 64086        Axis3 secondCandidate = secondAxis.ToWide();
 64087        var best = default(CylinderCylinderPenetration);
 64088        if (!TryKeepCylinderCylinderAxis(
 64089                firstCandidate,
 64090                firstCenter,
 64091                firstAxis,
 64092                firstLength,
 64093                firstRadius,
 64094                secondCenter,
 64095                secondAxis,
 64096                secondLength,
 64097                secondRadius,
 64098                ref best)
 64099            || !TryKeepCylinderCylinderAxis(
 640100                secondCandidate,
 640101                firstCenter,
 640102                firstAxis,
 640103                firstLength,
 640104                firstRadius,
 640105                secondCenter,
 640106                secondAxis,
 640107                secondLength,
 640108                secondRadius,
 640109                ref best)
 640110            || !TryKeepCylinderCylinderAxis(
 640111                Cross(firstCandidate, secondCandidate),
 640112                firstCenter,
 640113                firstAxis,
 640114                firstLength,
 640115                firstRadius,
 640116                secondCenter,
 640117                secondAxis,
 640118                secondLength,
 640119                secondRadius,
 640120                ref best))
 121        {
 150122            contact = default;
 150123            return false;
 124        }
 490125        if (!WideOrientedBox
 490126            .TryKeepCenteredRigidCylinderCylinderClosestAxis(
 490127                firstCenter,
 490128                firstRotation,
 490129                firstLocalAxis,
 490130                firstLength,
 490131                firstRadius,
 490132                secondCenter,
 490133                secondRotation,
 490134                secondLocalAxis,
 490135                secondLength,
 490136                secondRadius,
 490137                ref best))
 138        {
 51139            contact = default;
 51140            return false;
 141        }
 142        Vector3d normal;
 143        Fixed64 depth;
 144        bool depthIsClamped;
 439145        if (best.IsWide)
 146        {
 33147            normal = best.WideNormal;
 33148            depth = best.WideDepth;
 33149            depthIsClamped = best.WideDepthIsClamped;
 150        }
 151        else
 152        {
 406153            Axis3 orientedAxis = best.Negate
 406154                ? new Axis3(
 406155                    WideArithmetic.Negate(best.Axis.X),
 406156                    WideArithmetic.Negate(best.Axis.Y),
 406157                    WideArithmetic.Negate(best.Axis.Z))
 406158                : best.Axis;
 406159            normal = WideNormalization.GetNormalized(
 406160                Signed576.ExtendValue(orientedAxis.X),
 406161                Signed576.ExtendValue(orientedAxis.Y),
 406162                Signed576.ExtendValue(orientedAxis.Z));
 406163            GetRoundedCylinderCylinderDepth(
 406164                best.Depth,
 406165                out depth,
 406166                out depthIsClamped);
 167        }
 439168        FixedPointAnchor firstAnchor =
 439169            WideGeometry.GetCenteredCylinderSupportAnchor(
 439170                firstCenter,
 439171                firstRotation,
 439172                firstLocalAxis,
 439173                firstLength,
 439174                firstRadius,
 439175                normal);
 439176        FixedPointAnchor secondAnchor =
 439177            WideGeometry.GetCenteredCylinderSupportAnchor(
 439178                secondCenter,
 439179                secondRotation,
 439180                secondLocalAxis,
 439181                secondLength,
 439182                secondRadius,
 439183                -normal);
 439184        contact = new FixedContactAnchors(
 439185            firstAnchor,
 439186            secondAnchor,
 439187            normal,
 439188            depth,
 439189            depthIsClamped);
 439190        usedWideCandidate = best.IsWide;
 439191        usedMultiRadicalRanking =
 439192            best.IsWide
 439193            || !best.Depth.TryGetFastDepth(out _);
 439194        return true;
 195    }
 196
 197    internal static bool TryKeepWideCylinderCylinderAxis(
 198        WideCandidateAxis3 candidate,
 199        Vector3d firstCenter,
 200        FixedQuaternion firstRotation,
 201        Vector3d firstLocalAxis,
 202        Fixed64 firstLength,
 203        Fixed64 firstRadius,
 204        Vector3d secondCenter,
 205        FixedQuaternion secondRotation,
 206        Vector3d secondLocalAxis,
 207        Fixed64 secondLength,
 208        Fixed64 secondRadius,
 209        ref CylinderCylinderPenetration best)
 210    {
 490211        WideOrientedBox.GetRotatedLocalAxisNumerators(
 490212            firstRotation,
 490213            firstLocalAxis,
 490214            out Signed192 firstAxisX,
 490215            out Signed192 firstAxisY,
 490216            out Signed192 firstAxisZ,
 490217            out Signed192 firstDenominator);
 490218        WideOrientedBox.GetRotatedLocalAxisNumerators(
 490219            secondRotation,
 490220            secondLocalAxis,
 490221            out Signed192 secondAxisX,
 490222            out Signed192 secondAxisY,
 490223            out Signed192 secondAxisZ,
 490224            out Signed192 secondDenominator);
 490225        var firstAxis = new RigidAxis3(
 490226            firstAxisX,
 490227            firstAxisY,
 490228            firstAxisZ,
 490229            firstDenominator);
 490230        var secondAxis = new RigidAxis3(
 490231            secondAxisX,
 490232            secondAxisY,
 490233            secondAxisZ,
 490234            secondDenominator);
 490235        if (!candidate.TryNarrow(out Axis3 axis))
 236        {
 113237            return TryKeepWideCylinderCylinderAxisFallback(
 113238                candidate,
 113239                firstCenter,
 113240                firstAxis,
 113241                firstLength,
 113242                firstRadius,
 113243                secondCenter,
 113244                secondAxis,
 113245                secondLength,
 113246                secondRadius,
 113247                ref best);
 248        }
 377249        return TryKeepCylinderCylinderAxis(
 377250            axis,
 377251            firstCenter,
 377252            firstAxis,
 377253            firstLength,
 377254            firstRadius,
 377255            secondCenter,
 377256            secondAxis,
 377257            secondLength,
 377258            secondRadius,
 377259            ref best);
 260    }
 261
 262    private static bool TryKeepCylinderCylinderAxis(
 263        Axis3 axis,
 264        Vector3d firstCenter,
 265        RigidAxis3 firstAxis,
 266        Fixed64 firstLength,
 267        Fixed64 firstRadius,
 268        Vector3d secondCenter,
 269        RigidAxis3 secondAxis,
 270        Fixed64 secondLength,
 271        Fixed64 secondRadius,
 272        ref CylinderCylinderPenetration best)
 273    {
 2150274        if (axis.IsZero)
 270275            return true;
 276
 1880277        Signed576 firstAlignment =
 1880278            GetAxisProjection(axis, firstAxis);
 1880279        Signed576 secondAlignment =
 1880280            GetAxisProjection(axis, secondAxis);
 1880281        Signed576 centerProjection = GetDifferenceProjection(
 1880282            secondCenter,
 1880283            firstCenter,
 1880284            axis);
 1880285        Signed576 firstAxial = WideArithmetic.MultiplySigned576(
 1880286            WideArithmetic.MultiplySigned576(
 1880287                GetMagnitude576(firstAlignment),
 1880288                Signed192.Raw(firstLength)),
 1880289            secondAxis.RotationDenominator);
 1880290        Signed576 secondAxial = WideArithmetic.MultiplySigned576(
 1880291            WideArithmetic.MultiplySigned576(
 1880292                GetMagnitude576(secondAlignment),
 1880293                Signed192.Raw(secondLength)),
 1880294            firstAxis.RotationDenominator);
 1880295        Signed576 scaledCenter = WideArithmetic.MultiplySigned576(
 1880296            WideArithmetic.MultiplySigned576(
 1880297                WideArithmetic.MultiplySigned576(
 1880298                    GetMagnitude576(centerProjection),
 1880299                    firstAxis.RotationDenominator),
 1880300                secondAxis.RotationDenominator),
 1880301            Signed192.Raw(Fixed64.Two));
 1880302        Signed704 rational = Signed704.ExtendValue(
 1880303            WideArithmetic.SubtractSigned576(
 1880304                WideArithmetic.AddSigned576(
 1880305                    firstAxial,
 1880306                    secondAxial),
 1880307                scaledCenter));
 1880308        Signed576 axisSquared = GetAxisSquared(axis);
 1880309        Signed320 firstAxisSquared = GetAxisSquared(firstAxis);
 1880310        Signed320 secondAxisSquared = GetAxisSquared(secondAxis);
 1880311        Signed832 firstPlaneSquared = GetPlaneSquared(
 1880312            axisSquared,
 1880313            firstAxisSquared,
 1880314            firstAlignment);
 1880315        Signed832 secondPlaneSquared = GetPlaneSquared(
 1880316            axisSquared,
 1880317            secondAxisSquared,
 1880318            secondAlignment);
 1880319        Signed576 commonWide = WideArithmetic.MultiplySigned576(
 1880320            Signed576.ExtendValue(
 1880321                WideArithmetic.MultiplySigned192(
 1880322                    firstAxis.RotationDenominator,
 1880323                    secondAxis.RotationDenominator)),
 1880324            Signed192.Raw(Fixed64.Two));
 1880325        _ = Signed192.TryNarrowSigned(
 1880326            commonWide,
 1880327            out Signed192 common);
 1880328        var depth = new CylinderPairDepth(
 1880329            rational,
 1880330            common,
 1880331            firstRadius,
 1880332            secondRadius,
 1880333            axisSquared,
 1880334            firstAxisSquared,
 1880335            secondAxisSquared,
 1880336            firstPlaneSquared,
 1880337            secondPlaneSquared);
 1880338        if (!IsCylinderPairDepthNonNegative(depth))
 197339            return false;
 1683340        if (!best.HasValue
 1683341            || CompareCylinderPairDepths(
 1683342                depth,
 1683343                best.Depth) < 0)
 344        {
 1070345            best = new CylinderCylinderPenetration(
 1070346                axis,
 1070347                centerProjection.Sign < 0,
 1070348                depth);
 349        }
 1683350        return true;
 351    }
 352
 353    private static bool IsCylinderPairDepthNonNegative(
 354        in CylinderPairDepth depth)
 355    {
 1880356        if (depth.TryGetFastDepth(
 1880357                out ProjectionDepth fastDepth))
 358        {
 1746359            return IsProjectionNonNegative(fastDepth);
 360        }
 361
 134362        Span<ulong> radicands = stackalloc ulong[
 134363            CylinderPairRadicandWords * 3];
 134364        Span<int> signs = stackalloc int[3];
 134365        radicands.Clear();
 134366        signs.Clear();
 134367        BuildCylinderPairLocalRadicands(
 134368            depth,
 134369            radicands,
 134370            signs);
 134371        return WideArithmetic.GetLinearRadicalSumSign(
 134372            radicands,
 134373            CylinderPairRadicandWords,
 134374            signs) >= 0;
 375    }
 376
 377    private static int CompareCylinderPairDepths(
 378        in CylinderPairDepth left,
 379        in CylinderPairDepth right)
 380    {
 1097381        if (left.TryGetFastDepth(
 1097382                out ProjectionDepth leftFast)
 1097383            && right.TryGetFastDepth(
 1097384                out ProjectionDepth rightFast))
 385        {
 975386            return CompareProjectionDepths(
 975387                leftFast,
 975388                rightFast);
 389        }
 390
 122391        Span<ulong> radicands = stackalloc ulong[
 122392            CylinderPairRadicandWords * 6];
 122393        Span<int> signs = stackalloc int[6];
 122394        radicands.Clear();
 122395        signs.Clear();
 122396        BuildCylinderPairCrossRadicands(
 122397            left,
 122398            right,
 122399            radicands,
 122400            signs);
 122401        return WideArithmetic.GetLinearRadicalSumSign(
 122402            radicands,
 122403            CylinderPairRadicandWords,
 122404            signs);
 405    }
 406
 407    private static void BuildCylinderPairLocalRadicands(
 408        in CylinderPairDepth depth,
 409        Span<ulong> radicands,
 410        Span<int> signs)
 411    {
 2669412        Span<ulong> rational = radicands.Slice(
 2669413            0,
 2669414            CylinderPairRadicandWords);
 2669415        BuildCylinderPairRationalRadicand(
 2669416            depth.Rational,
 2669417            depth.FirstAxisSquared,
 2669418            depth.SecondAxisSquared,
 2669419            rational);
 2669420        signs[0] = depth.Rational.Sign;
 421
 2669422        Span<ulong> firstDisk = radicands.Slice(
 2669423            CylinderPairRadicandWords,
 2669424            CylinderPairRadicandWords);
 2669425        Signed320 firstCoefficient =
 2669426            WideArithmetic.MultiplySigned192(
 2669427                depth.Common,
 2669428                Signed192.Raw(depth.FirstRadius));
 2669429        BuildCylinderPairDiskRadicand(
 2669430            firstCoefficient,
 2669431            depth.FirstPlaneSquared,
 2669432            depth.SecondAxisSquared,
 2669433            firstDisk);
 2669434        signs[1] = 1;
 435
 2669436        Span<ulong> secondDisk = radicands.Slice(
 2669437            CylinderPairRadicandWords * 2,
 2669438            CylinderPairRadicandWords);
 2669439        Signed320 secondCoefficient =
 2669440            WideArithmetic.MultiplySigned192(
 2669441                depth.Common,
 2669442                Signed192.Raw(depth.SecondRadius));
 2669443        BuildCylinderPairDiskRadicand(
 2669444            secondCoefficient,
 2669445            depth.SecondPlaneSquared,
 2669446            depth.FirstAxisSquared,
 2669447            secondDisk);
 2669448        signs[2] = 1;
 2669449    }
 450
 451    private static void BuildCylinderPairCrossRadicands(
 452        in CylinderPairDepth left,
 453        in CylinderPairDepth right,
 454        Span<ulong> radicands,
 455        Span<int> signs)
 456    {
 122457        BuildCylinderPairCrossRationalRadicand(
 122458            left,
 122459            right,
 122460            radicands.Slice(
 122461                0,
 122462                CylinderPairRadicandWords));
 122463        signs[0] = left.Rational.Sign;
 122464        BuildCylinderPairCrossDiskRadicand(
 122465            left,
 122466            firstDisk: true,
 122467            right,
 122468            radicands.Slice(
 122469                CylinderPairRadicandWords,
 122470                CylinderPairRadicandWords));
 122471        signs[1] = 1;
 122472        BuildCylinderPairCrossDiskRadicand(
 122473            left,
 122474            firstDisk: false,
 122475            right,
 122476            radicands.Slice(
 122477                CylinderPairRadicandWords * 2,
 122478                CylinderPairRadicandWords));
 122479        signs[2] = 1;
 480
 122481        BuildCylinderPairCrossRationalRadicand(
 122482            right,
 122483            left,
 122484            radicands.Slice(
 122485                CylinderPairRadicandWords * 3,
 122486                CylinderPairRadicandWords));
 122487        signs[3] = -right.Rational.Sign;
 122488        BuildCylinderPairCrossDiskRadicand(
 122489            right,
 122490            firstDisk: true,
 122491            left,
 122492            radicands.Slice(
 122493                CylinderPairRadicandWords * 4,
 122494                CylinderPairRadicandWords));
 122495        signs[4] = -1;
 122496        BuildCylinderPairCrossDiskRadicand(
 122497            right,
 122498            firstDisk: false,
 122499            left,
 122500            radicands.Slice(
 122501                CylinderPairRadicandWords * 5,
 122502                CylinderPairRadicandWords));
 122503        signs[5] = -1;
 122504    }
 505
 506    private static void BuildCylinderPairRationalRadicand(
 507        Signed704 rational,
 508        Signed320 firstAxisSquared,
 509        Signed320 secondAxisSquared,
 510        Span<ulong> result)
 511    {
 2669512        Span<ulong> words = stackalloc ulong[11];
 2669513        WideArithmetic.GetMagnitude(rational, words);
 2669514        WideArithmetic.MultiplyMagnitudes(words, words, result);
 2669515        MultiplyCylinderPairBy(
 2669516            result,
 2669517            firstAxisSquared);
 2669518        MultiplyCylinderPairBy(
 2669519            result,
 2669520            secondAxisSquared);
 2669521    }
 522
 523    private static void BuildCylinderPairDiskRadicand(
 524        Signed320 coefficient,
 525        Signed832 planeSquared,
 526        Signed320 otherAxisSquared,
 527        Span<ulong> result)
 528    {
 5338529        Span<ulong> coefficientWords =
 5338530            stackalloc ulong[5];
 5338531        GetMagnitude(coefficient, coefficientWords);
 5338532        WideArithmetic.MultiplyMagnitudes(
 5338533            coefficientWords,
 5338534            coefficientWords,
 5338535            result);
 5338536        MultiplyCylinderPairBy(
 5338537            result,
 5338538            planeSquared);
 5338539        MultiplyCylinderPairBy(
 5338540            result,
 5338541            otherAxisSquared);
 5338542    }
 543
 544    private static void BuildCylinderPairCrossRationalRadicand(
 545        in CylinderPairDepth depth,
 546        in CylinderPairDepth other,
 547        Span<ulong> result)
 548    {
 244549        Span<ulong> rationalWords =
 244550            stackalloc ulong[11];
 244551        WideArithmetic.GetMagnitude(
 244552            depth.Rational,
 244553            rationalWords);
 244554        WideArithmetic.MultiplyMagnitudes(
 244555            rationalWords,
 244556            rationalWords,
 244557            result);
 244558        MultiplyCylinderPairBy(
 244559            result,
 244560            other.AxisSquared);
 244561        MultiplyCylinderPairBy(
 244562            result,
 244563            depth.FirstAxisSquared);
 244564        MultiplyCylinderPairBy(
 244565            result,
 244566            depth.SecondAxisSquared);
 244567        MultiplyCylinderPairBy(
 244568            result,
 244569            other.FirstAxisSquared);
 244570        MultiplyCylinderPairBy(
 244571            result,
 244572            other.SecondAxisSquared);
 244573    }
 574
 575    private static void BuildCylinderPairCrossDiskRadicand(
 576        in CylinderPairDepth depth,
 577        bool firstDisk,
 578        in CylinderPairDepth other,
 579        Span<ulong> result)
 580    {
 488581        Fixed64 radius = firstDisk
 488582            ? depth.FirstRadius
 488583            : depth.SecondRadius;
 488584        Signed832 planeSquared = firstDisk
 488585            ? depth.FirstPlaneSquared
 488586            : depth.SecondPlaneSquared;
 488587        Signed320 uncancelledAxisSquared = firstDisk
 488588            ? depth.SecondAxisSquared
 488589            : depth.FirstAxisSquared;
 488590        Signed320 coefficient =
 488591            WideArithmetic.MultiplySigned192(
 488592                depth.Common,
 488593                Signed192.Raw(radius));
 488594        Span<ulong> coefficientWords =
 488595            stackalloc ulong[5];
 488596        GetMagnitude(
 488597            coefficient,
 488598            coefficientWords);
 488599        WideArithmetic.MultiplyMagnitudes(
 488600            coefficientWords,
 488601            coefficientWords,
 488602            result);
 488603        MultiplyCylinderPairBy(
 488604            result,
 488605            planeSquared);
 488606        MultiplyCylinderPairBy(
 488607            result,
 488608            other.AxisSquared);
 488609        MultiplyCylinderPairBy(
 488610            result,
 488611            uncancelledAxisSquared);
 488612        MultiplyCylinderPairBy(
 488613            result,
 488614            other.FirstAxisSquared);
 488615        MultiplyCylinderPairBy(
 488616            result,
 488617            other.SecondAxisSquared);
 488618    }
 619
 620    private static void MultiplyCylinderPairBy(
 621        Span<ulong> value,
 622        Signed320 factor)
 623    {
 18186624        Span<ulong> words = stackalloc ulong[5];
 18186625        GetMagnitude(factor, words);
 18186626        MultiplyCylinderPairBy(value, words);
 18186627    }
 628
 629    private static void MultiplyCylinderPairBy(
 630        Span<ulong> value,
 631        Signed576 factor)
 632    {
 3267633        Span<ulong> words = stackalloc ulong[9];
 3267634        WideArithmetic.GetMagnitude(factor, words);
 3267635        MultiplyCylinderPairBy(value, words);
 3267636    }
 637
 638    private static void MultiplyCylinderPairBy(
 639        Span<ulong> value,
 640        Signed832 factor)
 641    {
 5826642        Span<ulong> words = stackalloc ulong[13];
 5826643        WideArithmetic.GetMagnitude(factor, words);
 5826644        MultiplyCylinderPairBy(value, words);
 5826645    }
 646
 647    private static void MultiplyCylinderPairBy(
 648        Span<ulong> value,
 649        ReadOnlySpan<ulong> factor)
 650    {
 27279651        Span<ulong> product =
 27279652            stackalloc ulong[CylinderPairRadicandWords];
 27279653        WideArithmetic.MultiplyMagnitudes(
 27279654            value,
 27279655            factor,
 27279656            product);
 27279657        product.CopyTo(value);
 27279658    }
 659}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.RigidCylinderPairs.Rounding.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.RigidCylinderPairs.Rounding.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Provides correctly-rounded penetration depth calculations for rigid cylinder
 14/// pairs, using fast scalar approximations corrected by exact fixed-point
 15/// comparisons, with a fallback binary search for edge cases near overflow.
 16/// </content>
 17internal static partial class WideConvexPrismRelations
 18{
 19    private static void GetRoundedCylinderCylinderDepth(
 20        in CylinderPairDepth depth,
 21        out Fixed64 result,
 22        out bool isClamped)
 23    {
 40624        if (!TryGetCylinderPairDepthApproximation(
 40625                depth,
 40626                out Fixed64 approximation))
 27        {
 2628            Signed192 maximumTwiceRaw = new(
 2629                0UL,
 2630                0UL,
 2631                unchecked((ulong)long.MaxValue << 1));
 2632            int maximumComparison =
 2633                CompareCylinderPairDepthToTwiceRaw(
 2634                    depth,
 2635                    maximumTwiceRaw);
 2636            if (maximumComparison >= 0)
 37            {
 138                result = Fixed64.MaxValue;
 139                isClamped = maximumComparison > 0;
 140                return;
 41            }
 2542            result = GetRoundedCylinderPairDepthBySearch(depth);
 2543            isClamped = false;
 2544            return;
 45        }
 46        // Each scalar approximation is rounded once, so the combined estimate
 47        // is only a few raw units from the exact projection. Correct those
 48        // units directly with the exact midpoint comparator.
 49        while (true)
 50        {
 45951            if (approximation == Fixed64.MaxValue)
 52            {
 253                Signed192 maximumTwiceRaw = new(
 254                    0UL,
 255                    0UL,
 256                    unchecked((ulong)long.MaxValue << 1));
 257                result = GetRoundedCylinderPairDepthBySearch(depth);
 258                isClamped =
 259                    CompareCylinderPairDepthToTwiceRaw(
 260                        depth,
 261                        maximumTwiceRaw) > 0;
 262                return;
 63            }
 64
 45765            if (approximation > Fixed64.Zero)
 66            {
 42767                Signed192 lowerMidpoint = new(
 42768                    0UL,
 42769                    0UL,
 42770                    unchecked((ulong)(
 42771                        approximation.m_rawValue
 42772                        + approximation.m_rawValue
 42773                        - 1L)));
 42774                int lowerComparison =
 42775                    CompareCylinderPairDepthToTwiceRaw(
 42776                        depth,
 42777                        lowerMidpoint);
 42778                if (lowerComparison
 42779                    < (approximation.m_rawValue & 1L))
 80                {
 681                    approximation = Fixed64.FromRaw(
 682                        approximation.m_rawValue - 1L);
 683                    continue;
 84                }
 85            }
 86
 45187            Signed192 upperMidpoint = new(
 45188                0UL,
 45189                0UL,
 45190                unchecked((ulong)approximation.m_rawValue << 1) | 1UL);
 45191            int upperComparison =
 45192                CompareCylinderPairDepthToTwiceRaw(
 45193                    depth,
 45194                    upperMidpoint);
 45195            if (upperComparison
 45196                + (approximation.m_rawValue & 1L) > 0)
 97            {
 7398                approximation = Fixed64.FromRaw(
 7399                    approximation.m_rawValue + 1L);
 73100                continue;
 101            }
 102
 378103            result = approximation;
 378104            isClamped = false;
 378105            return;
 106        }
 107    }
 108
 109    private static bool TryGetCylinderPairDepthApproximation(
 110        in CylinderPairDepth depth,
 111        out Fixed64 approximation)
 112    {
 406113        Signed576 axisLength =
 406114            WideArithmetic.GetFloorSquareRoot(
 406115                Signed704.ExtendValue(
 406116                    depth.AxisSquared));
 406117        Signed576 rationalDenominator =
 406118            WideArithmetic.MultiplySigned576(
 406119                axisLength,
 406120                depth.Common);
 406121        if (!Fixed64.TryGetSignedRawRatio(
 406122                depth.Rational,
 406123                Signed704.ExtendValue(
 406124                    rationalDenominator),
 406125                out Fixed64 rational))
 126        {
 25127            approximation = default;
 25128            return false;
 129        }
 381130        Fixed64 firstDisk =
 381131            GetDiskDepthApproximation(
 381132                depth.CreateFirstDiskDepth(),
 381133                axisLength);
 381134        Fixed64 secondDisk =
 381135            GetDiskDepthApproximation(
 381136                depth.CreateSecondDiskDepth(),
 381137                axisLength);
 381138        Signed192 combined = WideArithmetic.AddSigned192(
 381139            WideArithmetic.AddSigned192(
 381140                Signed192.Raw(rational),
 381141                Signed192.Raw(firstDisk)),
 381142            Signed192.Raw(secondDisk));
 381143        if (combined.Sign <= 0)
 144        {
 30145            approximation = Fixed64.Zero;
 30146            return true;
 147        }
 148        // Any upper word or low sign bit exceeds the nonnegative Fixed64 raw domain.
 351149        if ((combined.High
 351150                | combined.Middle
 351151                | (combined.Low >> 63)) != 0UL)
 152        {
 1153            approximation = default;
 1154            return false;
 155        }
 350156        approximation = Fixed64.FromRaw((long)combined.Low);
 350157        return true;
 158    }
 159
 160    private static Fixed64 GetRoundedCylinderPairDepthBySearch(
 161        in CylinderPairDepth depth)
 162    {
 27163        Signed192 maximumTwiceRaw = new(
 27164            0UL,
 27165            0UL,
 27166            unchecked((ulong)long.MaxValue << 1));
 27167        if (CompareCylinderPairDepthToTwiceRaw(
 27168                depth,
 27169                maximumTwiceRaw) >= 0)
 170        {
 2171            return Fixed64.MaxValue;
 172        }
 173
 25174        ulong low = 0UL;
 25175        ulong high = 1UL << 63;
 176        // The nonnegative Fixed64 raw domain contains exactly 2^63 values.
 177        // This upper-bound search therefore completes in at most 64 steps.
 1602178        while (low < high)
 179        {
 1577180            ulong midpoint = low + ((high - low) >> 1);
 1577181            int comparison =
 1577182                CompareCylinderPairDepthToTwiceRaw(
 1577183                    depth,
 1577184                    new Signed192(
 1577185                        0UL,
 1577186                        0UL,
 1577187                        midpoint << 1));
 1577188            if (comparison >= 0)
 25189                low = midpoint + 1UL;
 190            else
 1552191                high = midpoint;
 192        }
 193
 25194        ulong floor = low - 1UL;
 25195        int midpointComparison =
 25196            CompareCylinderPairDepthToTwiceRaw(
 25197                depth,
 25198                new Signed192(
 25199                    0UL,
 25200                    0UL,
 25201                    (floor << 1) | 1UL));
 25202        return Fixed64.FromRaw(
 25203            (long)(floor + GetNearestEvenIncrement(
 25204                midpointComparison,
 25205                floor)));
 206    }
 207
 208    private static int CompareCylinderPairDepthToTwiceRaw(
 209        in CylinderPairDepth depth,
 210        Signed192 twiceRaw)
 211    {
 2535212        Span<ulong> radicands = stackalloc ulong[
 2535213            CylinderPairRadicandWords * 4];
 2535214        Span<int> signs = stackalloc int[4];
 2535215        radicands.Clear();
 2535216        signs.Clear();
 2535217        BuildCylinderPairLocalRadicands(
 2535218            depth,
 2535219            radicands,
 2535220            signs);
 20280221        for (int index = 0; index < 3; index++)
 7605222            ShiftLeft(
 7605223                radicands.Slice(
 7605224                    index * CylinderPairRadicandWords,
 7605225                    CylinderPairRadicandWords),
 7605226                2);
 227
 2535228        Signed320 thresholdCoefficient =
 2535229            WideArithmetic.MultiplySigned192(
 2535230                depth.Common,
 2535231                twiceRaw);
 2535232        Span<ulong> threshold = radicands.Slice(
 2535233            CylinderPairRadicandWords * 3,
 2535234            CylinderPairRadicandWords);
 2535235        Span<ulong> coefficientWords =
 2535236            stackalloc ulong[5];
 2535237        GetMagnitude(
 2535238            thresholdCoefficient,
 2535239            coefficientWords);
 2535240        WideArithmetic.MultiplyMagnitudes(
 2535241            coefficientWords,
 2535242            coefficientWords,
 2535243            threshold);
 2535244        MultiplyCylinderPairBy(
 2535245            threshold,
 2535246            depth.AxisSquared);
 2535247        MultiplyCylinderPairBy(
 2535248            threshold,
 2535249            depth.FirstAxisSquared);
 2535250        MultiplyCylinderPairBy(
 2535251            threshold,
 2535252            depth.SecondAxisSquared);
 2535253        signs[3] = -1;
 2535254        return WideArithmetic.GetLinearRadicalSumSign(
 2535255            radicands,
 2535256            CylinderPairRadicandWords,
 2535257            signs);
 258    }
 259}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.RigidFiniteShapePairs.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.RigidFiniteShapePairs.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Contains high-precision contact resolution routines for pairs of rigid, finite
 14/// convex prism-based shapes (e.g. cylinders and capsules) using exact rational
 15/// (Signed192) axis representations to avoid precision loss during rotation.
 16/// </content>
 17internal static partial class WideConvexPrismRelations
 18{
 19    internal static bool TryGetCenteredFiniteCylinderCapsuleContact(
 20        Vector3d cylinderCenter,
 21        FixedQuaternion cylinderRotation,
 22        Vector3d cylinderLocalAxis,
 23        Fixed64 cylinderLength,
 24        Fixed64 cylinderRadius,
 25        Vector3d capsuleCenter,
 26        FixedQuaternion capsuleRotation,
 27        Vector3d capsuleLocalAxis,
 28        Fixed64 capsuleLength,
 29        Fixed64 capsuleRadius,
 30        out FixedContactAnchors contact) =>
 7931        TryGetCenteredFiniteCylinderCapsuleContact(
 7932            cylinderCenter,
 7933            cylinderRotation,
 7934            cylinderLocalAxis,
 7935            cylinderLength,
 7936            cylinderRadius,
 7937            capsuleCenter,
 7938            capsuleRotation,
 7939            capsuleLocalAxis,
 7940            capsuleLength,
 7941            capsuleRadius,
 7942            out contact,
 7943            out _);
 44
 45    internal static bool TryGetCenteredFiniteCylinderCapsuleContact(
 46        Vector3d cylinderCenter,
 47        FixedQuaternion cylinderRotation,
 48        Vector3d cylinderLocalAxis,
 49        Fixed64 cylinderLength,
 50        Fixed64 cylinderRadius,
 51        Vector3d capsuleCenter,
 52        FixedQuaternion capsuleRotation,
 53        Vector3d capsuleLocalAxis,
 54        Fixed64 capsuleLength,
 55        Fixed64 capsuleRadius,
 56        out FixedContactAnchors contact,
 57        out bool usedWideCandidate)
 58    {
 55759        usedWideCandidate = false;
 55760        WideOrientedBox.GetRotatedLocalAxisNumerators(
 55761            cylinderRotation,
 55762            cylinderLocalAxis,
 55763            out Signed192 cylinderAxisX,
 55764            out Signed192 cylinderAxisY,
 55765            out Signed192 cylinderAxisZ,
 55766            out Signed192 cylinderDenominator);
 55767        WideOrientedBox.GetRotatedLocalAxisNumerators(
 55768            capsuleRotation,
 55769            capsuleLocalAxis,
 55770            out Signed192 capsuleAxisX,
 55771            out Signed192 capsuleAxisY,
 55772            out Signed192 capsuleAxisZ,
 55773            out Signed192 capsuleDenominator);
 55774        var cylinderAxis = new RigidAxis3(
 55775            cylinderAxisX,
 55776            cylinderAxisY,
 55777            cylinderAxisZ,
 55778            cylinderDenominator);
 55779        var capsuleAxis = new RigidAxis3(
 55780            capsuleAxisX,
 55781            capsuleAxisY,
 55782            capsuleAxisZ,
 55783            capsuleDenominator);
 55784        Axis3 cylinderCandidate = cylinderAxis.ToWide();
 55785        Axis3 capsuleCandidate = capsuleAxis.ToWide();
 55786        var best = default(CylinderCapsulePenetration);
 55787        if (!TryKeepCylinderCapsuleAxis(
 55788                cylinderCandidate,
 55789                cylinderCenter,
 55790                cylinderAxis,
 55791                cylinderLength,
 55792                cylinderRadius,
 55793                capsuleCenter,
 55794                capsuleAxis,
 55795                capsuleLength,
 55796                capsuleRadius,
 55797                ref best)
 55798            || !TryKeepCylinderCapsuleAxis(
 55799                capsuleCandidate,
 557100                cylinderCenter,
 557101                cylinderAxis,
 557102                cylinderLength,
 557103                cylinderRadius,
 557104                capsuleCenter,
 557105                capsuleAxis,
 557106                capsuleLength,
 557107                capsuleRadius,
 557108                ref best)
 557109            || !TryKeepCylinderCapsuleAxis(
 557110                Cross(cylinderCandidate, capsuleCandidate),
 557111                cylinderCenter,
 557112                cylinderAxis,
 557113                cylinderLength,
 557114                cylinderRadius,
 557115                capsuleCenter,
 557116                capsuleAxis,
 557117                capsuleLength,
 557118                capsuleRadius,
 557119                ref best))
 120        {
 120121            contact = default;
 120122            return false;
 123        }
 437124        Axis3 centerDifference = GetCenterDifferenceAxis(
 437125            cylinderCenter,
 437126            capsuleCenter);
 437127        if (!TryKeepCylinderCapsuleAxis(
 437128                centerDifference,
 437129                cylinderCenter,
 437130                cylinderAxis,
 437131                cylinderLength,
 437132                cylinderRadius,
 437133                capsuleCenter,
 437134                capsuleAxis,
 437135                capsuleLength,
 437136                capsuleRadius,
 437137                ref best))
 138        {
 55139            contact = default;
 55140            return false;
 141        }
 382142        if (!WideOrientedBox
 382143            .TryKeepCenteredRigidCylinderCapsuleClosestAxis(
 382144                cylinderCenter,
 382145                cylinderRotation,
 382146                cylinderLocalAxis,
 382147                cylinderLength,
 382148                cylinderRadius,
 382149                capsuleCenter,
 382150                capsuleRotation,
 382151                capsuleLocalAxis,
 382152                capsuleLength,
 382153                capsuleRadius,
 382154                ref best))
 155        {
 25156            contact = default;
 25157            return false;
 158        }
 159
 160        Vector3d normal;
 161        Fixed64 depth;
 162        bool depthIsClamped;
 357163        if (best.IsWide)
 164        {
 80165            normal = best.WideNormal;
 80166            depth = best.WideDepth;
 80167            depthIsClamped = best.WideDepthIsClamped;
 168        }
 169        else
 170        {
 277171            Axis3 orientedAxis = best.Negate
 277172                ? new Axis3(
 277173                     WideArithmetic.Negate(best.Axis.X),
 277174                     WideArithmetic.Negate(best.Axis.Y),
 277175                     WideArithmetic.Negate(best.Axis.Z))
 277176                : best.Axis;
 277177            normal = WideNormalization.GetNormalized(
 277178                Signed576.ExtendValue(orientedAxis.X),
 277179                Signed576.ExtendValue(orientedAxis.Y),
 277180                Signed576.ExtendValue(orientedAxis.Z));
 277181            GetRoundedCylinderCapsuleDepth(
 277182                best.ExactDepth,
 277183                capsuleRadius,
 277184                out depth,
 277185                out depthIsClamped);
 186        }
 357187        FixedPointAnchor cylinderAnchor =
 357188            WideGeometry.GetCenteredCylinderSupportAnchor(
 357189                cylinderCenter,
 357190                cylinderRotation,
 357191                cylinderLocalAxis,
 357192                cylinderLength,
 357193                cylinderRadius,
 357194                normal);
 357195        FixedPointAnchor capsuleAnchor =
 357196            WideGeometry.GetCenteredCapsuleSupportAnchor(
 357197                capsuleCenter,
 357198                capsuleRotation,
 357199                capsuleLocalAxis,
 357200                capsuleLength,
 357201                capsuleRadius,
 357202                -normal);
 357203        contact = new FixedContactAnchors(
 357204            cylinderAnchor,
 357205            capsuleAnchor,
 357206            normal,
 357207            depth,
 357208            depthIsClamped);
 357209        usedWideCandidate = best.IsWide;
 357210        return true;
 211    }
 212
 213    private static bool TryKeepCylinderCapsuleAxis(
 214        Axis3 axis,
 215        Vector3d cylinderCenter,
 216        RigidAxis3 cylinderAxis,
 217        Fixed64 cylinderLength,
 218        Fixed64 cylinderRadius,
 219        Vector3d capsuleCenter,
 220        RigidAxis3 capsuleAxis,
 221        Fixed64 capsuleLength,
 222        Fixed64 capsuleRadius,
 223        ref CylinderCapsulePenetration best)
 224    {
 2240225        if (axis.IsZero)
 179226            return true;
 227
 2061228        Signed576 cylinderAlignment =
 2061229            GetAxisProjection(axis, cylinderAxis);
 2061230        Signed576 capsuleAlignment =
 2061231            GetAxisProjection(axis, capsuleAxis);
 2061232        Signed576 centerProjection = GetDifferenceProjection(
 2061233            capsuleCenter,
 2061234            cylinderCenter,
 2061235            axis);
 2061236        Signed576 cylinderAxial = WideArithmetic.MultiplySigned576(
 2061237            WideArithmetic.MultiplySigned576(
 2061238                GetMagnitude576(cylinderAlignment),
 2061239                Signed192.Raw(cylinderLength)),
 2061240            capsuleAxis.RotationDenominator);
 2061241        Signed576 capsuleAxial = WideArithmetic.MultiplySigned576(
 2061242            WideArithmetic.MultiplySigned576(
 2061243                GetMagnitude576(capsuleAlignment),
 2061244                Signed192.Raw(capsuleLength)),
 2061245            cylinderAxis.RotationDenominator);
 2061246        Signed576 scaledCenter = WideArithmetic.MultiplySigned576(
 2061247            WideArithmetic.MultiplySigned576(
 2061248                WideArithmetic.MultiplySigned576(
 2061249                    GetMagnitude576(centerProjection),
 2061250                    cylinderAxis.RotationDenominator),
 2061251                capsuleAxis.RotationDenominator),
 2061252            Signed192.Raw(Fixed64.Two));
 2061253        Signed576 rational = WideArithmetic.SubtractSigned576(
 2061254            WideArithmetic.AddSigned576(
 2061255                cylinderAxial,
 2061256                capsuleAxial),
 2061257            scaledCenter);
 2061258        Signed576 axisSquared = GetAxisSquared(axis);
 2061259        Signed320 cylinderAxisSquared =
 2061260            GetAxisSquared(cylinderAxis);
 2061261        Signed832 planeSquared = GetPlaneSquared(
 2061262            axisSquared,
 2061263            cylinderAxisSquared,
 2061264            cylinderAlignment);
 2061265        Signed576 commonWide = WideArithmetic.MultiplySigned576(
 2061266            Signed576.ExtendValue(
 2061267                WideArithmetic.MultiplySigned192(
 2061268                    cylinderAxis.RotationDenominator,
 2061269                    capsuleAxis.RotationDenominator)),
 2061270            Signed192.Raw(Fixed64.Two));
 2061271        _ = Signed192.TryNarrowSigned(
 2061272            commonWide,
 2061273            out Signed192 common);
 2061274        var exactDepth = new ProjectionDepth(
 2061275            Signed704.ExtendValue(rational),
 2061276            common,
 2061277            cylinderRadius,
 2061278            RadialKind.Disk,
 2061279            axisSquared,
 2061280            cylinderAxisSquared,
 2061281            planeSquared);
 2061282        if (!IsCylinderCapsuleProjectionNonNegative(
 2061283                exactDepth,
 2061284                capsuleRadius))
 285        {
 199286            return false;
 287        }
 288
 1862289        if (!best.HasValue
 1862290            || CompareSignedProjectionDepths(
 1862291                exactDepth,
 1862292                best.ExactDepth) < 0)
 293        {
 1147294            best = new CylinderCapsulePenetration(
 1147295                axis,
 1147296                centerProjection.Sign < 0,
 1147297                exactDepth);
 298        }
 1862299        return true;
 300    }
 301
 302    private static Axis3 GetCenterDifferenceAxis(
 303        Vector3d start,
 304        Vector3d end) =>
 437305        new(
 437306            Signed320.ExtendValue(
 437307                WideArithmetic.SubtractSigned192(
 437308                    Signed192.Raw(end.X),
 437309                    Signed192.Raw(start.X))),
 437310            Signed320.ExtendValue(
 437311                WideArithmetic.SubtractSigned192(
 437312                    Signed192.Raw(end.Y),
 437313                    Signed192.Raw(start.Y))),
 437314            Signed320.ExtendValue(
 437315                WideArithmetic.SubtractSigned192(
 437316                    Signed192.Raw(end.Z),
 437317                    Signed192.Raw(start.Z))));
 318
 319    private static bool IsCylinderCapsuleProjectionNonNegative(
 320        in ProjectionDepth baseDepth,
 321        Fixed64 capsuleRadius)
 322    {
 2061323        if (IsProjectionNonNegative(baseDepth))
 1385324            return true;
 325
 676326        Span<ulong> disk = stackalloc ulong[40];
 676327        Span<ulong> capsule = stackalloc ulong[40];
 676328        Span<ulong> rational = stackalloc ulong[40];
 676329        Span<ulong> zero = stackalloc ulong[40];
 676330        BuildLocalDiskRadicand(baseDepth, disk);
 676331        var capsuleDepth = new ProjectionDepth(
 676332            default,
 676333            baseDepth.Common,
 676334            capsuleRadius,
 676335            RadialKind.Capsule,
 676336            baseDepth.AxisSquared,
 676337            baseDepth.ShapeAxisSquared,
 676338            default);
 676339        BuildThresholdCapsuleRadicand(capsuleDepth, capsule);
 676340        BuildProduct(
 676341            baseDepth.Rational,
 676342            baseDepth.Rational,
 676343            baseDepth.ShapeAxisSquared,
 676344            rational);
 676345        zero.Clear();
 676346        return CompareRadicalPairs(
 676347            disk,
 676348            capsule,
 676349            rational,
 676350            zero) >= 0;
 351    }
 352
 353    private static void GetRoundedCylinderCapsuleDepth(
 354        in ProjectionDepth baseDepth,
 355        Fixed64 capsuleRadius,
 356        out Fixed64 depth,
 357        out bool isClamped)
 358    {
 277359        if (!TryGetCylinderCapsuleDepthApproximation(
 277360                baseDepth,
 277361                capsuleRadius,
 277362                out Fixed64 approximation))
 363        {
 28364            Signed192 maximumTwiceRaw = new(
 28365                0UL,
 28366                0UL,
 28367                unchecked((ulong)long.MaxValue << 1));
 28368            int maximumComparison =
 28369                CompareCylinderCapsuleDepthToTwiceRaw(
 28370                    baseDepth,
 28371                    capsuleRadius,
 28372                    maximumTwiceRaw);
 28373            if (maximumComparison >= 0)
 374            {
 1375                depth = Fixed64.MaxValue;
 1376                isClamped = maximumComparison > 0;
 1377                return;
 378            }
 27379            depth = GetRoundedCylinderCapsuleDepthBySearch(
 27380                baseDepth,
 27381                capsuleRadius);
 27382            isClamped = false;
 27383            return;
 384        }
 385        // Each scalar approximation is rounded once, so the combined estimate
 386        // is only a few raw units from the exact projection. Correct those
 387        // units directly with the exact midpoint comparator.
 388        while (true)
 389        {
 271390            if (approximation == Fixed64.MaxValue)
 391            {
 2392                Signed192 maximumTwiceRaw = new(
 2393                    0UL,
 2394                    0UL,
 2395                    unchecked((ulong)long.MaxValue << 1));
 2396                depth = GetRoundedCylinderCapsuleDepthBySearch(
 2397                    baseDepth,
 2398                    capsuleRadius);
 2399                isClamped =
 2400                    CompareCylinderCapsuleDepthToTwiceRaw(
 2401                        baseDepth,
 2402                        capsuleRadius,
 2403                        maximumTwiceRaw) > 0;
 2404                return;
 405            }
 406
 269407            if (approximation > Fixed64.Zero)
 408            {
 257409                Signed192 lowerMidpoint = new(
 257410                    0UL,
 257411                    0UL,
 257412                    unchecked((ulong)(
 257413                        approximation.m_rawValue
 257414                        + approximation.m_rawValue
 257415                        - 1L)));
 257416                int lowerComparison =
 257417                    CompareCylinderCapsuleDepthToTwiceRaw(
 257418                        baseDepth,
 257419                        capsuleRadius,
 257420                        lowerMidpoint);
 257421                if (lowerComparison
 257422                    < (approximation.m_rawValue & 1L))
 423                {
 4424                    approximation = Fixed64.FromRaw(
 4425                        approximation.m_rawValue - 1L);
 4426                    continue;
 427                }
 428            }
 429
 265430            Signed192 upperMidpoint = new(
 265431                0UL,
 265432                0UL,
 265433                unchecked((ulong)approximation.m_rawValue << 1) | 1UL);
 265434            int upperComparison =
 265435                CompareCylinderCapsuleDepthToTwiceRaw(
 265436                    baseDepth,
 265437                    capsuleRadius,
 265438                    upperMidpoint);
 265439            if (upperComparison
 265440                + (approximation.m_rawValue & 1L) > 0)
 441            {
 18442                approximation = Fixed64.FromRaw(
 18443                    approximation.m_rawValue + 1L);
 18444                continue;
 445            }
 446
 247447            depth = approximation;
 247448            isClamped = false;
 247449            return;
 450        }
 451    }
 452
 453    private static bool TryGetCylinderCapsuleDepthApproximation(
 454        in ProjectionDepth baseDepth,
 455        Fixed64 capsuleRadius,
 456        out Fixed64 approximation)
 457    {
 277458        Signed576 axisLength = WideArithmetic.GetFloorSquareRoot(
 277459            Signed704.ExtendValue(baseDepth.AxisSquared));
 277460        Signed576 rationalDenominator = WideArithmetic.MultiplySigned576(
 277461            axisLength,
 277462            baseDepth.Common);
 277463        if (!Fixed64.TryGetSignedRawRatio(
 277464                baseDepth.Rational,
 277465                Signed704.ExtendValue(rationalDenominator),
 277466                out Fixed64 rational))
 467        {
 27468            approximation = default;
 27469            return false;
 470        }
 250471        Fixed64 radial =
 250472            GetDiskDepthApproximation(baseDepth, axisLength);
 250473        Signed192 combined = WideArithmetic.AddSigned192(
 250474            WideArithmetic.AddSigned192(
 250475                Signed192.Raw(rational),
 250476                Signed192.Raw(radial)),
 250477            Signed192.Raw(capsuleRadius));
 250478        if (combined.Sign <= 0)
 479        {
 12480            approximation = Fixed64.Zero;
 12481            return true;
 482        }
 483        // Any upper word or low sign bit exceeds the nonnegative Fixed64 raw domain.
 238484        if ((combined.High
 238485                | combined.Middle
 238486                | (combined.Low >> 63)) != 0UL)
 487        {
 1488            approximation = default;
 1489            return false;
 490        }
 237491        approximation = Fixed64.FromRaw((long)combined.Low);
 237492        return true;
 493    }
 494
 495    private static Fixed64 GetRoundedCylinderCapsuleDepthBySearch(
 496        in ProjectionDepth baseDepth,
 497        Fixed64 capsuleRadius)
 498    {
 29499        Signed192 maximumTwiceRaw = new(
 29500            0UL,
 29501            0UL,
 29502            unchecked((ulong)long.MaxValue << 1));
 29503        if (CompareCylinderCapsuleDepthToTwiceRaw(
 29504                baseDepth,
 29505                capsuleRadius,
 29506                maximumTwiceRaw) >= 0)
 507        {
 2508            return Fixed64.MaxValue;
 509        }
 510
 27511        ulong low = 0UL;
 27512        ulong high = 1UL << 63;
 513        // The nonnegative Fixed64 raw domain contains exactly 2^63 values.
 514        // This upper-bound search therefore completes in at most 64 steps.
 1730515        while (low < high)
 516        {
 1703517            ulong midpoint = low + ((high - low) >> 1);
 1703518            int comparison =
 1703519                CompareCylinderCapsuleDepthToTwiceRaw(
 1703520                    baseDepth,
 1703521                    capsuleRadius,
 1703522                    new Signed192(
 1703523                        0UL,
 1703524                        0UL,
 1703525                        midpoint << 1));
 1703526            if (comparison >= 0)
 527            {
 87528                low = midpoint + 1UL;
 529            }
 530            else
 531            {
 1616532                high = midpoint;
 533            }
 534        }
 535
 27536        ulong floor = low - 1UL;
 27537        int midpointComparison =
 27538            CompareCylinderCapsuleDepthToTwiceRaw(
 27539                baseDepth,
 27540                capsuleRadius,
 27541                new Signed192(
 27542                    0UL,
 27543                    0UL,
 27544                    (floor << 1) | 1UL));
 27545        return Fixed64.FromRaw(
 27546            (long)(floor + GetNearestEvenIncrement(
 27547                midpointComparison,
 27548                floor)));
 549    }
 550
 551    private static int CompareCylinderCapsuleDepthToTwiceRaw(
 552        in ProjectionDepth baseDepth,
 553        Fixed64 capsuleRadius,
 554        Signed192 twiceRaw)
 555    {
 2311556        Signed192 target = WideArithmetic.SubtractSigned192(
 2311557            twiceRaw,
 2311558            WideArithmetic.AddSigned192(
 2311559                Signed192.Raw(capsuleRadius),
 2311560                Signed192.Raw(capsuleRadius)));
 2311561        if (target.Sign >= 0)
 562        {
 324563            return CompareProjectionDepthToTwiceRaw(
 324564                baseDepth,
 324565                target);
 566        }
 1987567        if (IsProjectionNonNegative(baseDepth))
 1568            return 1;
 569
 1986570        Signed192 positiveTarget =
 1986571            WideArithmetic.SubtractSigned192(default, target);
 1986572        Span<ulong> radial = stackalloc ulong[40];
 1986573        Span<ulong> threshold = stackalloc ulong[40];
 1986574        Span<ulong> rational = stackalloc ulong[40];
 1986575        Span<ulong> zero = stackalloc ulong[40];
 1986576        BuildLocalDiskRadicand(baseDepth, radial);
 1986577        ShiftLeft(radial, 2);
 1986578        Signed320 thresholdCoefficient =
 1986579            WideArithmetic.MultiplySigned192(
 1986580                baseDepth.Common,
 1986581                positiveTarget);
 1986582        BuildThresholdRadicand(
 1986583            thresholdCoefficient,
 1986584            baseDepth.ShapeAxisSquared,
 1986585            baseDepth.AxisSquared,
 1986586            threshold);
 1986587        BuildProduct(
 1986588            baseDepth.Rational,
 1986589            baseDepth.Rational,
 1986590            baseDepth.ShapeAxisSquared,
 1986591            rational);
 1986592        ShiftLeft(rational, 2);
 1986593        zero.Clear();
 1986594        return CompareRadicalPairs(
 1986595            radial,
 1986596            threshold,
 1986597            rational,
 1986598            zero);
 599    }
 600
 601    private static Signed576 GetMagnitude576(Signed576 value) =>
 11823602        value.Sign >= 0
 11823603            ? value
 11823604            : WideArithmetic.SubtractSigned576(default, value);
 605
 606    internal static bool TryKeepWideCylinderCapsuleAxis(
 607        WideCandidateAxis3 candidate,
 608        Vector3d cylinderCenter,
 609        FixedQuaternion cylinderRotation,
 610        Vector3d cylinderLocalAxis,
 611        Fixed64 cylinderLength,
 612        Fixed64 cylinderRadius,
 613        Vector3d capsuleCenter,
 614        FixedQuaternion capsuleRotation,
 615        Vector3d capsuleLocalAxis,
 616        Fixed64 capsuleLength,
 617        Fixed64 capsuleRadius,
 618        ref CylinderCapsulePenetration best)
 619    {
 382620        WideOrientedBox.GetRotatedLocalAxisNumerators(
 382621            cylinderRotation,
 382622            cylinderLocalAxis,
 382623            out Signed192 cylinderAxisX,
 382624            out Signed192 cylinderAxisY,
 382625            out Signed192 cylinderAxisZ,
 382626            out Signed192 cylinderDenominator);
 382627        WideOrientedBox.GetRotatedLocalAxisNumerators(
 382628            capsuleRotation,
 382629            capsuleLocalAxis,
 382630            out Signed192 capsuleAxisX,
 382631            out Signed192 capsuleAxisY,
 382632            out Signed192 capsuleAxisZ,
 382633            out Signed192 capsuleDenominator);
 382634        var cylinderAxis = new RigidAxis3(
 382635            cylinderAxisX,
 382636            cylinderAxisY,
 382637            cylinderAxisZ,
 382638            cylinderDenominator);
 382639        var capsuleAxis = new RigidAxis3(
 382640            capsuleAxisX,
 382641            capsuleAxisY,
 382642            capsuleAxisZ,
 382643            capsuleDenominator);
 382644        if (!candidate.TryNarrow(out Axis3 axis))
 645        {
 149646            return TryKeepWideCylinderCapsuleAxisFallback(
 149647                candidate,
 149648                cylinderCenter,
 149649                cylinderAxis,
 149650                cylinderLength,
 149651                cylinderRadius,
 149652                capsuleCenter,
 149653                capsuleAxis,
 149654                capsuleLength,
 149655                capsuleRadius,
 149656                ref best);
 657        }
 233658        return TryKeepCylinderCapsuleAxis(
 233659            axis,
 233660            cylinderCenter,
 233661            cylinderAxis,
 233662            cylinderLength,
 233663            cylinderRadius,
 233664            capsuleCenter,
 233665            capsuleAxis,
 233666            capsuleLength,
 233667            capsuleRadius,
 233668            ref best);
 669    }
 670}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.Support.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.Support.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024–present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Support types and helpers for convex prism relations, including finite shape kinds,
 14/// radial classifications, and high-precision axis representations used in narrow-phase math.
 15/// </content>
 16internal static partial class WideConvexPrismRelations
 17{
 118    private static readonly Signed192 Scale = Signed192.One;
 19    private const int CylinderPairRadicandWords = 64;
 20    private const int WideCandidateWords = 160;
 21
 22    #region Nested Types
 23
 24    private enum FiniteShapeKind
 25    {
 26        Capsule,
 27        Cylinder,
 28        Cone,
 29    }
 30
 31    internal enum RadialKind
 32    {
 33        None,
 34        Capsule,
 35        Disk,
 36    }
 37
 38    internal enum ShapeSupportFeature
 39    {
 40        Axis,
 41        ConeApex,
 42        ConeBase,
 43    }
 44
 45    internal readonly struct Axis3
 46    {
 47        internal readonly Signed320 X;
 48        internal readonly Signed320 Y;
 49        internal readonly Signed320 Z;
 50
 51        internal Axis3(Signed320 x, Signed320 y, Signed320 z)
 52        {
 635453            X = x;
 635454            Y = y;
 635455            Z = z;
 635456        }
 57
 569258        internal bool IsZero => X.IsZero && Y.IsZero && Z.IsZero;
 59    }
 60
 61    private readonly struct RigidAxis3
 62    {
 63        internal readonly Signed192 X;
 64        internal readonly Signed192 Y;
 65        internal readonly Signed192 Z;
 66        internal readonly Signed192 RotationDenominator;
 67
 68        internal RigidAxis3(
 69            Signed192 x,
 70            Signed192 y,
 71            Signed192 z,
 72            Signed192 rotationDenominator)
 73        {
 418974            X = x;
 418975            Y = y;
 418976            Z = z;
 418977            RotationDenominator = rotationDenominator;
 418978        }
 79
 80        internal Axis3 ToWide() =>
 244581            new(
 244582                Signed320.ExtendValue(X),
 244583                Signed320.ExtendValue(Y),
 244584                Signed320.ExtendValue(Z));
 85    }
 86
 87    private readonly struct Penetration
 88    {
 89        internal readonly Vector3d Normal;
 90        internal readonly FixedPointAnchor ShapeAnchor;
 91        internal readonly FixedQuaternion PrismOrientation;
 92        internal readonly Vector3d PrismOffset;
 93        internal readonly Fixed64 Depth;
 94        internal readonly bool DepthIsClamped;
 95
 96        internal Penetration(
 97            Vector3d normal,
 98            FixedPointAnchor shapeAnchor,
 99            FixedQuaternion prismOrientation,
 100            Vector3d prismOffset,
 101            Fixed64 depth,
 102            bool depthIsClamped)
 103        {
 40104            Normal = normal;
 40105            ShapeAnchor = shapeAnchor;
 40106            PrismOrientation = prismOrientation;
 40107            PrismOffset = prismOffset;
 40108            Depth = depth;
 40109            DepthIsClamped = depthIsClamped;
 40110        }
 111    }
 112
 113    private readonly struct PenetrationCandidate
 114    {
 115        internal readonly Axis3 Axis;
 116        internal readonly bool Negate;
 117        internal readonly ShapeSupportFeature SupportFeature;
 118        internal readonly Vector3d PrismOffset;
 119        internal readonly bool PrismPlanarTie;
 120        internal readonly ProjectionDepth ExactDepth;
 121
 122        internal PenetrationCandidate(
 123            Axis3 axis,
 124            bool negate,
 125            ShapeSupportFeature supportFeature,
 126            Vector3d prismOffset,
 127            bool prismPlanarTie,
 128            ProjectionDepth exactDepth)
 129        {
 89130            Axis = axis;
 89131            Negate = negate;
 89132            SupportFeature = supportFeature;
 89133            PrismOffset = prismOffset;
 89134            PrismPlanarTie = prismPlanarTie;
 89135            ExactDepth = exactDepth;
 89136            HasValue = true;
 89137        }
 138
 139        internal bool HasValue { get; }
 140    }
 141
 142    internal readonly struct ProjectionDepth
 143    {
 144        internal readonly Signed704 Rational;
 145        internal readonly Signed192 Common;
 146        internal readonly Fixed64 Radius;
 147        internal readonly RadialKind RadialKind;
 148        internal readonly Signed576 AxisSquared;
 149        internal readonly Signed320 ShapeAxisSquared;
 150        internal readonly Signed832 PlaneSquared;
 151        internal readonly ShapeSupportFeature SupportFeature;
 152
 153        internal ProjectionDepth(
 154            Signed704 rational,
 155            Signed192 common,
 156            Fixed64 radius,
 157            RadialKind radialKind,
 158            Signed576 axisSquared,
 159            Signed320 shapeAxisSquared,
 160            Signed832 planeSquared,
 161            ShapeSupportFeature supportFeature = ShapeSupportFeature.Axis)
 162        {
 10041163            Rational = rational;
 10041164            Common = common;
 10041165            Radius = radius;
 10041166            RadialKind = radialKind;
 10041167            AxisSquared = axisSquared;
 10041168            ShapeAxisSquared = shapeAxisSquared;
 10041169            PlaneSquared = planeSquared;
 10041170            SupportFeature = supportFeature;
 10041171        }
 172    }
 173
 174    internal readonly struct CylinderCylinderPenetration
 175    {
 176        internal readonly Axis3 Axis;
 177        internal readonly bool Negate;
 178        internal readonly CylinderPairDepth Depth;
 179        internal readonly Vector3d WideNormal;
 180        internal readonly Fixed64 WideDepth;
 181        internal readonly bool WideDepthIsClamped;
 182        internal readonly bool IsWide;
 183
 184        internal CylinderCylinderPenetration(
 185            Axis3 axis,
 186            bool negate,
 187            CylinderPairDepth depth)
 188        {
 1070189            Axis = axis;
 1070190            Negate = negate;
 1070191            Depth = depth;
 1070192            WideNormal = default;
 1070193            WideDepth = default;
 1070194            WideDepthIsClamped = false;
 1070195            IsWide = false;
 1070196            HasValue = true;
 1070197        }
 198
 199        internal CylinderCylinderPenetration(
 200            Vector3d normal,
 201            Fixed64 depth,
 202            bool depthIsClamped)
 203        {
 33204            Axis = default;
 33205            Negate = false;
 33206            Depth = default;
 33207            WideNormal = normal;
 33208            WideDepth = depth;
 33209            WideDepthIsClamped = depthIsClamped;
 33210            IsWide = true;
 33211            HasValue = true;
 33212        }
 213
 214        internal bool HasValue { get; }
 215    }
 216
 217    internal readonly struct CylinderPairDepth
 218    {
 219        internal readonly Signed704 Rational;
 220        internal readonly Signed192 Common;
 221        internal readonly Fixed64 FirstRadius;
 222        internal readonly Fixed64 SecondRadius;
 223        internal readonly Signed576 AxisSquared;
 224        internal readonly Signed320 FirstAxisSquared;
 225        internal readonly Signed320 SecondAxisSquared;
 226        internal readonly Signed832 FirstPlaneSquared;
 227        internal readonly Signed832 SecondPlaneSquared;
 228
 229        internal CylinderPairDepth(
 230            Signed704 rational,
 231            Signed192 common,
 232            Fixed64 firstRadius,
 233            Fixed64 secondRadius,
 234            Signed576 axisSquared,
 235            Signed320 firstAxisSquared,
 236            Signed320 secondAxisSquared,
 237            Signed832 firstPlaneSquared,
 238            Signed832 secondPlaneSquared)
 239        {
 1880240            Rational = rational;
 1880241            Common = common;
 1880242            FirstRadius = firstRadius;
 1880243            SecondRadius = secondRadius;
 1880244            AxisSquared = axisSquared;
 1880245            FirstAxisSquared = firstAxisSquared;
 1880246            SecondAxisSquared = secondAxisSquared;
 1880247            FirstPlaneSquared = firstPlaneSquared;
 1880248            SecondPlaneSquared = secondPlaneSquared;
 1880249        }
 250
 251        internal bool TryGetFastDepth(
 252            out ProjectionDepth depth)
 253        {
 4358254            bool firstZero =
 4358255                FirstRadius == Fixed64.Zero
 4358256                || FirstPlaneSquared.IsZero;
 4358257            bool secondZero =
 4358258                SecondRadius == Fixed64.Zero
 4358259                || SecondPlaneSquared.IsZero;
 4358260            bool firstFull =
 4358261                !firstZero
 4358262                && IsFullDiskProjection(
 4358263                    AxisSquared,
 4358264                    FirstAxisSquared,
 4358265                    FirstPlaneSquared);
 4358266            bool secondFull =
 4358267                !secondZero
 4358268                && IsFullDiskProjection(
 4358269                    AxisSquared,
 4358270                    SecondAxisSquared,
 4358271                    SecondPlaneSquared);
 4358272            if (firstZero && secondZero)
 273            {
 1410274                depth = new ProjectionDepth(
 1410275                    Rational,
 1410276                    Common,
 1410277                    Fixed64.Zero,
 1410278                    RadialKind.None,
 1410279                    AxisSquared,
 1410280                    FirstAxisSquared,
 1410281                    default);
 1410282                return true;
 283            }
 2948284            if (firstZero || secondZero)
 285            {
 1981286                bool useFirst = secondZero;
 1981287                depth = new ProjectionDepth(
 1981288                    Rational,
 1981289                    Common,
 1981290                    useFirst
 1981291                        ? FirstRadius
 1981292                        : SecondRadius,
 1981293                    useFirst
 1981294                        ? (firstFull
 1981295                            ? RadialKind.Capsule
 1981296                            : RadialKind.Disk)
 1981297                        : (secondFull
 1981298                            ? RadialKind.Capsule
 1981299                            : RadialKind.Disk),
 1981300                    AxisSquared,
 1981301                    useFirst
 1981302                        ? FirstAxisSquared
 1981303                        : SecondAxisSquared,
 1981304                    useFirst
 1981305                        ? FirstPlaneSquared
 1981306                        : SecondPlaneSquared);
 1981307                return true;
 308            }
 967309            if (firstFull
 967310                && secondFull
 967311                && Fixed64.TryAdd(
 967312                    FirstRadius,
 967313                    SecondRadius,
 967314                    out Fixed64 combinedRadius))
 315            {
 633316                depth = new ProjectionDepth(
 633317                    Rational,
 633318                    Common,
 633319                    combinedRadius,
 633320                    RadialKind.Capsule,
 633321                    AxisSquared,
 633322                    FirstAxisSquared,
 633323                    default);
 633324                return true;
 325            }
 326
 334327            depth = default;
 334328            return false;
 329        }
 330
 331        internal ProjectionDepth CreateFirstDiskDepth() =>
 381332            new(
 381333                default,
 381334                Common,
 381335                FirstRadius,
 381336                RadialKind.Disk,
 381337                AxisSquared,
 381338                FirstAxisSquared,
 381339                FirstPlaneSquared);
 340
 341        internal ProjectionDepth CreateSecondDiskDepth() =>
 381342            new(
 381343                default,
 381344                Common,
 381345                SecondRadius,
 381346                RadialKind.Disk,
 381347                AxisSquared,
 381348                SecondAxisSquared,
 381349                SecondPlaneSquared);
 350
 351        private static bool IsFullDiskProjection(
 352            Signed576 axisSquared,
 353            Signed320 shapeAxisSquared,
 354            Signed832 planeSquared)
 355        {
 3915356            Signed832 fullPlane =
 3915357                WideArithmetic.MultiplyNonNegativeToSigned832(
 3915358                    Signed832.ExtendValue(
 3915359                        axisSquared),
 3915360                    shapeAxisSquared);
 3915361            return fullPlane.Equals(planeSquared);
 362        }
 363    }
 364
 365    internal readonly ref struct WideCandidateAxis3
 366    {
 367        internal readonly ReadOnlySpan<ulong> X;
 368        internal readonly ReadOnlySpan<ulong> Y;
 369        internal readonly ReadOnlySpan<ulong> Z;
 370        internal readonly int XSign;
 371        internal readonly int YSign;
 372        internal readonly int ZSign;
 373
 374        internal WideCandidateAxis3(
 375            ReadOnlySpan<ulong> x,
 376            int xSign,
 377            ReadOnlySpan<ulong> y,
 378            int ySign,
 379            ReadOnlySpan<ulong> z,
 380            int zSign)
 381        {
 872382            X = x;
 872383            Y = y;
 872384            Z = z;
 872385            XSign = xSign;
 872386            YSign = ySign;
 872387            ZSign = zSign;
 872388        }
 389
 390        internal bool TryNarrow(out Axis3 axis)
 391        {
 872392            bool representable =
 872393                TryNarrowComponent(X, XSign, out Signed320 x)
 872394                & TryNarrowComponent(Y, YSign, out Signed320 y)
 872395                & TryNarrowComponent(Z, ZSign, out Signed320 z);
 872396            axis = representable
 872397                ? new Axis3(x, y, z)
 872398                : default;
 872399            return representable;
 400        }
 401
 402        private static bool TryNarrowComponent(
 403            ReadOnlySpan<ulong> magnitude,
 404            int sign,
 405            out Signed320 result)
 406        {
 2616407            int length = WideArithmetic.GetActiveMagnitudeLength(magnitude);
 2616408            if (length > 5
 2616409                || (length == 5
 2616410                    && (magnitude[4] & (1UL << 63)) != 0UL))
 411            {
 621412                result = default;
 621413                return false;
 414            }
 415
 1995416            Signed320 positive = new(
 1995417                length > 4 ? magnitude[4] : 0UL,
 1995418                length > 3 ? magnitude[3] : 0UL,
 1995419                length > 2 ? magnitude[2] : 0UL,
 1995420                length > 1 ? magnitude[1] : 0UL,
 1995421                length > 0 ? magnitude[0] : 0UL);
 1995422            result = sign < 0
 1995423                ? WideArithmetic.SubtractSigned320(default, positive)
 1995424                : positive;
 1995425            return true;
 426        }
 427    }
 428
 429    internal readonly struct CylinderCapsulePenetration
 430    {
 431        internal readonly Axis3 Axis;
 432        internal readonly bool Negate;
 433        internal readonly ProjectionDepth ExactDepth;
 434        internal readonly Vector3d WideNormal;
 435        internal readonly Fixed64 WideDepth;
 436        internal readonly bool WideDepthIsClamped;
 437        internal readonly bool IsWide;
 438
 439        internal CylinderCapsulePenetration(
 440            Axis3 axis,
 441            bool negate,
 442            ProjectionDepth exactDepth)
 443        {
 1147444            Axis = axis;
 1147445            Negate = negate;
 1147446            ExactDepth = exactDepth;
 1147447            WideNormal = default;
 1147448            WideDepth = default;
 1147449            WideDepthIsClamped = false;
 1147450            IsWide = false;
 1147451            HasValue = true;
 1147452        }
 453
 454        internal CylinderCapsulePenetration(
 455            Vector3d normal,
 456            Fixed64 depth,
 457            bool depthIsClamped)
 458        {
 80459            Axis = default;
 80460            Negate = false;
 80461            ExactDepth = default;
 80462            WideNormal = normal;
 80463            WideDepth = depth;
 80464            WideDepthIsClamped = depthIsClamped;
 80465            IsWide = true;
 80466            HasValue = true;
 80467        }
 468
 469        internal bool HasValue { get; }
 470    }
 471
 472    private readonly ref struct WideCylinderCapsuleDepth
 473    {
 474        internal readonly ReadOnlySpan<ulong> Rational;
 475        internal readonly int RationalSign;
 476        internal readonly Signed192 Common;
 477        internal readonly Fixed64 Radius;
 478        internal readonly ReadOnlySpan<ulong> AxisSquared;
 479        internal readonly Signed320 ShapeAxisSquared;
 480        internal readonly ReadOnlySpan<ulong> PlaneSquared;
 481
 482        internal WideCylinderCapsuleDepth(
 483            ReadOnlySpan<ulong> rational,
 484            int rationalSign,
 485            Signed192 common,
 486            Fixed64 radius,
 487            ReadOnlySpan<ulong> axisSquared,
 488            Signed320 shapeAxisSquared,
 489            ReadOnlySpan<ulong> planeSquared)
 490        {
 149491            Rational = rational;
 149492            RationalSign = rationalSign;
 149493            Common = common;
 149494            Radius = radius;
 149495            AxisSquared = axisSquared;
 149496            ShapeAxisSquared = shapeAxisSquared;
 149497            PlaneSquared = planeSquared;
 149498        }
 499    }
 500
 501    private readonly ref struct WideCylinderPairDepth
 502    {
 503        internal readonly ReadOnlySpan<ulong> Rational;
 504        internal readonly int RationalSign;
 505        internal readonly Signed192 Common;
 506        internal readonly Fixed64 FirstRadius;
 507        internal readonly Fixed64 SecondRadius;
 508        internal readonly ReadOnlySpan<ulong> AxisSquared;
 509        internal readonly Signed320 FirstAxisSquared;
 510        internal readonly Signed320 SecondAxisSquared;
 511        internal readonly ReadOnlySpan<ulong> FirstPlaneSquared;
 512        internal readonly ReadOnlySpan<ulong> SecondPlaneSquared;
 513
 514        internal WideCylinderPairDepth(
 515            ReadOnlySpan<ulong> rational,
 516            int rationalSign,
 517            Signed192 common,
 518            Fixed64 firstRadius,
 519            Fixed64 secondRadius,
 520            ReadOnlySpan<ulong> axisSquared,
 521            Signed320 firstAxisSquared,
 522            Signed320 secondAxisSquared,
 523            ReadOnlySpan<ulong> firstPlaneSquared,
 524            ReadOnlySpan<ulong> secondPlaneSquared)
 525        {
 113526            Rational = rational;
 113527            RationalSign = rationalSign;
 113528            Common = common;
 113529            FirstRadius = firstRadius;
 113530            SecondRadius = secondRadius;
 113531            AxisSquared = axisSquared;
 113532            FirstAxisSquared = firstAxisSquared;
 113533            SecondAxisSquared = secondAxisSquared;
 113534            FirstPlaneSquared = firstPlaneSquared;
 113535            SecondPlaneSquared = secondPlaneSquared;
 113536        }
 537    }
 538
 539    #endregion
 540
 541    private static Penetration MaterializeContact(
 542        in PenetrationCandidate candidate,
 543        FiniteShapeKind shapeKind,
 544        RigidAxis3 shapeAxis,
 545        Vector3d center,
 546        FixedQuaternion shapeRotation,
 547        Vector3d localAxisDirection,
 548        Fixed64 axisLength,
 549        Fixed64 radius,
 550        Vector3d prismOrigin,
 551        FixedQuaternion prismOrientation,
 552        ReadOnlySpan<Vector2d> prismOffsets,
 553        Fixed64 prismHalfThickness)
 554    {
 40555        Axis3 orientedAxis = candidate.Negate
 40556            ? Negate(candidate.Axis)
 40557            : candidate.Axis;
 40558        Vector3d normal = WideNormalization.GetNormalized(
 40559            Signed576.ExtendValue(orientedAxis.X),
 40560            Signed576.ExtendValue(orientedAxis.Y),
 40561            Signed576.ExtendValue(orientedAxis.Z));
 40562        int axialSign = GetAxisProjection(
 40563            orientedAxis,
 40564            shapeAxis).Sign;
 40565        FixedPointAnchor shapeAnchor = GetShapeSupportAnchor(
 40566            shapeKind,
 40567            center,
 40568            axisLength,
 40569            radius,
 40570            shapeRotation,
 40571            localAxisDirection,
 40572            normal,
 40573            axialSign,
 40574            candidate.SupportFeature);
 40575        GetMatchedPrismOffset(
 40576            shapeAnchor,
 40577            prismOrigin,
 40578            prismOrientation,
 40579            prismOffsets,
 40580            prismHalfThickness,
 40581            candidate.PrismOffset,
 40582            orientedAxis,
 40583            candidate.PrismPlanarTie,
 40584            out Vector3d prismOffset);
 40585        Fixed64 depth = GetRoundedDepth(
 40586            candidate.ExactDepth,
 40587            out bool depthIsClamped);
 40588        return new Penetration(
 40589            normal,
 40590            shapeAnchor,
 40591            prismOrientation,
 40592            prismOffset,
 40593            depth,
 40594            depthIsClamped);
 595    }
 596
 597    private static void GetMatchedPrismOffset(
 598        FixedPointAnchor shapeAnchor,
 599        Vector3d prismOrigin,
 600        FixedQuaternion prismOrientation,
 601        ReadOnlySpan<Vector2d> prismOffsets,
 602        Fixed64 prismHalfThickness,
 603        Vector3d fallbackPrismOffset,
 604        Axis3 normalAxis,
 605        bool planarSupportTie,
 606        out Vector3d prismOffset)
 607    {
 40608        if (!shapeAnchor.TryGetLocalPointIn(
 40609                prismOrigin,
 40610                prismOrientation,
 40611                out Vector3d localPoint))
 612        {
 1613            prismOffset = fallbackPrismOffset;
 1614            return;
 615        }
 616
 39617        var planarPoint = new Vector2d(localPoint.X, localPoint.Z);
 618        Vector2d planarOffset;
 39619        bool verticalNormal = normalAxis.X.IsZero
 39620            && normalAxis.Z.IsZero;
 39621        if (verticalNormal
 39622            && FixedConvex2dRelations.ContainsPoint(
 39623                planarPoint,
 39624                Vector2d.Zero,
 39625                prismOffsets))
 626        {
 13627            planarOffset = planarPoint;
 628        }
 26629        else if (verticalNormal | planarSupportTie)
 630        {
 22631            planarOffset = FixedConvex2dRelations.GetClosestPointOffset(
 22632                planarPoint,
 22633                Vector2d.Zero,
 22634                prismOffsets);
 635        }
 636        else
 637        {
 4638            planarOffset = new Vector2d(
 4639                fallbackPrismOffset.X,
 4640                fallbackPrismOffset.Z);
 641        }
 642
 39643        Fixed64 y = normalAxis.Y.IsZero
 39644            ? FixedMath.Clamp(
 39645                localPoint.Y,
 39646                -prismHalfThickness,
 39647                prismHalfThickness)
 39648            : fallbackPrismOffset.Y;
 39649        prismOffset = new Vector3d(planarOffset.X, y, planarOffset.Y);
 39650    }
 651
 652    private static FixedPointAnchor GetShapeSupportAnchor(
 653        FiniteShapeKind shapeKind,
 654        Vector3d center,
 655        Fixed64 axisLength,
 656        Fixed64 radius,
 657        FixedQuaternion shapeRotation,
 658        Vector3d localAxisDirection,
 659        Vector3d direction,
 660        int axialSign,
 661        ShapeSupportFeature supportFeature) =>
 40662        shapeKind switch
 40663        {
 40664            FiniteShapeKind.Capsule =>
 28665                WideGeometry.GetCenteredCapsuleSupportAnchor(
 28666                    center,
 28667                    shapeRotation,
 28668                    localAxisDirection,
 28669                    axisLength,
 28670                    radius,
 28671                    direction,
 28672                    axialSign),
 40673            FiniteShapeKind.Cylinder =>
 6674                WideGeometry.GetCenteredCylinderSupportAnchor(
 6675                    center,
 6676                    shapeRotation,
 6677                    localAxisDirection,
 6678                    axisLength,
 6679                    radius,
 6680                    direction,
 6681                    axialSign),
 6682            _ => WideGeometry.GetCenteredConeSupportAnchor(
 6683                    center,
 6684                    shapeRotation,
 6685                    localAxisDirection,
 6686                axisLength,
 6687                radius,
 6688                direction,
 6689                supportFeature == ShapeSupportFeature.ConeApex),
 40690        };
 691
 692    private static Axis3 FromDirection(Vector3d direction) =>
 51693        new(
 51694            Signed320.ExtendValue(Signed192.Raw(direction.X)),
 51695            Signed320.ExtendValue(Signed192.Raw(direction.Y)),
 51696            Signed320.ExtendValue(Signed192.Raw(direction.Z)));
 697
 698    private static int Compare(Signed576 left, Signed576 right) =>
 7624699        WideArithmetic.SubtractSigned576(left, right).Sign;
 700
 701    private static Axis3 Negate(Axis3 axis) =>
 19702        new(
 19703            WideArithmetic.Negate(axis.X),
 19704            WideArithmetic.Negate(axis.Y),
 19705            WideArithmetic.Negate(axis.Z));
 706}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.WideCandidate.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.WideCandidate.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Wide-precision candidate axis helpers for cylinder/capsule separating-axis
 14/// tests, using high-word arithmetic to build and evaluate axis projections
 15/// without precision loss.
 16/// </content>
 17internal static partial class WideConvexPrismRelations
 18{
 19    private static bool TryKeepWideCylinderCapsuleAxisFallback(
 20        WideCandidateAxis3 candidate,
 21        Vector3d cylinderCenter,
 22        RigidAxis3 cylinderAxis,
 23        Fixed64 cylinderLength,
 24        Fixed64 cylinderRadius,
 25        Vector3d capsuleCenter,
 26        RigidAxis3 capsuleAxis,
 27        Fixed64 capsuleLength,
 28        Fixed64 capsuleRadius,
 29        ref CylinderCapsulePenetration best)
 30    {
 14931        Span<ulong> axisSquared =
 14932            stackalloc ulong[WideCandidateWords];
 14933        BuildWideAxisSquared(candidate, axisSquared);
 14934        Span<ulong> cylinderAlignment =
 14935            stackalloc ulong[WideCandidateWords];
 14936        BuildWideDot(
 14937            candidate,
 14938            cylinderAxis.X,
 14939            cylinderAxis.Y,
 14940            cylinderAxis.Z,
 14941            cylinderAlignment,
 14942            out int cylinderAlignmentSign);
 14943        Span<ulong> capsuleAlignment =
 14944            stackalloc ulong[WideCandidateWords];
 14945        BuildWideDot(
 14946            candidate,
 14947            capsuleAxis.X,
 14948            capsuleAxis.Y,
 14949            capsuleAxis.Z,
 14950            capsuleAlignment,
 14951            out int capsuleAlignmentSign);
 14952        Span<ulong> centerProjection =
 14953            stackalloc ulong[WideCandidateWords];
 14954        BuildWideDot(
 14955            candidate,
 14956            WideArithmetic.SubtractSigned192(
 14957                Signed192.Raw(capsuleCenter.X),
 14958                Signed192.Raw(cylinderCenter.X)),
 14959            WideArithmetic.SubtractSigned192(
 14960                Signed192.Raw(capsuleCenter.Y),
 14961                Signed192.Raw(cylinderCenter.Y)),
 14962            WideArithmetic.SubtractSigned192(
 14963                Signed192.Raw(capsuleCenter.Z),
 14964                Signed192.Raw(cylinderCenter.Z)),
 14965            centerProjection,
 14966            out _);
 67
 14968        Span<ulong> cylinderAxial =
 14969            stackalloc ulong[WideCandidateWords];
 14970        BuildWideScaledAlignment(
 14971            cylinderAlignment,
 14972            cylinderLength,
 14973            capsuleAxis.RotationDenominator,
 14974            cylinderAxial);
 14975        Span<ulong> capsuleAxial =
 14976            stackalloc ulong[WideCandidateWords];
 14977        BuildWideScaledAlignment(
 14978            capsuleAlignment,
 14979            capsuleLength,
 14980            cylinderAxis.RotationDenominator,
 14981            capsuleAxial);
 14982        Span<ulong> scaledCenter =
 14983            stackalloc ulong[WideCandidateWords];
 14984        BuildWideScaledCenter(
 14985            centerProjection,
 14986            cylinderAxis.RotationDenominator,
 14987            capsuleAxis.RotationDenominator,
 14988            scaledCenter);
 14989        Span<ulong> rational =
 14990            stackalloc ulong[WideCandidateWords];
 14991        Span<ulong> axialSum =
 14992            stackalloc ulong[WideCandidateWords];
 14993        WideArithmetic.AddEqualMagnitudes(
 14994            cylinderAxial,
 14995            capsuleAxial,
 14996            axialSum);
 14997        CombineWideSignedMagnitudes(
 14998            axialSum,
 14999            1,
 149100            scaledCenter,
 149101            -1,
 149102            rational,
 149103            out int rationalSign);
 104        // A non-narrowable closest-point residual is nonzero. The endpoint
 105        // KKT relation makes support minus center -K * |residual|^2, K > 0.
 106
 149107        Signed320 cylinderAxisSquared =
 149108            GetAxisSquared(cylinderAxis);
 149109        Span<ulong> planeSquared =
 149110            stackalloc ulong[WideCandidateWords];
 149111        BuildWidePlaneSquared(
 149112            axisSquared,
 149113            cylinderAxisSquared,
 149114            cylinderAlignment,
 149115            planeSquared);
 149116        Signed576 commonWide = WideArithmetic.MultiplySigned576(
 149117            Signed576.ExtendValue(
 149118                WideArithmetic.MultiplySigned192(
 149119                    cylinderAxis.RotationDenominator,
 149120                    capsuleAxis.RotationDenominator)),
 149121            Signed192.Raw(Fixed64.Two));
 149122        _ = Signed192.TryNarrowSigned(
 149123            commonWide,
 149124            out Signed192 common);
 149125        var depth = new WideCylinderCapsuleDepth(
 149126            rational,
 149127            rationalSign,
 149128            common,
 149129            cylinderRadius,
 149130            axisSquared,
 149131            cylinderAxisSquared,
 149132            planeSquared);
 149133        if (!IsWideCylinderCapsuleProjectionNonNegative(
 149134                depth,
 149135                capsuleRadius))
 136        {
 1137            return false;
 138        }
 139
 148140        if (CompareWideProjectionDepth(
 148141                depth,
 148142                best.ExactDepth) >= 0)
 143        {
 68144            return true;
 145        }
 146
 80147        GetRoundedWideCylinderCapsuleDepth(
 80148            depth,
 80149            capsuleRadius,
 80150            out Fixed64 roundedDepth,
 80151            out bool depthIsClamped);
 80152        Vector3d normal = GetWideCandidateNormal(candidate);
 80153        best = new CylinderCapsulePenetration(
 80154            normal,
 80155            roundedDepth,
 80156            depthIsClamped);
 80157        return true;
 158    }
 159
 160    private static void BuildWideAxisSquared(
 161        WideCandidateAxis3 axis,
 162        Span<ulong> result)
 163    {
 262164        Span<ulong> xSquared =
 262165            stackalloc ulong[WideCandidateWords];
 262166        Span<ulong> ySquared =
 262167            stackalloc ulong[WideCandidateWords];
 262168        Span<ulong> zSquared =
 262169            stackalloc ulong[WideCandidateWords];
 262170        Span<ulong> sum =
 262171            stackalloc ulong[WideCandidateWords];
 262172        WideArithmetic.MultiplyMagnitudes(axis.X, axis.X, xSquared);
 262173        WideArithmetic.MultiplyMagnitudes(axis.Y, axis.Y, ySquared);
 262174        WideArithmetic.MultiplyMagnitudes(axis.Z, axis.Z, zSquared);
 262175        WideArithmetic.AddEqualMagnitudes(xSquared, ySquared, sum);
 262176        WideArithmetic.AddEqualMagnitudes(sum, zSquared, result);
 262177    }
 178
 179    private static void BuildWideDot(
 180        WideCandidateAxis3 axis,
 181        Signed192 x,
 182        Signed192 y,
 183        Signed192 z,
 184        Span<ulong> result,
 185        out int sign)
 186    {
 786187        Span<ulong> xMagnitude = stackalloc ulong[3];
 786188        Span<ulong> yMagnitude = stackalloc ulong[3];
 786189        Span<ulong> zMagnitude = stackalloc ulong[3];
 786190        WideArithmetic.GetMagnitude(
 786191            x,
 786192            out xMagnitude[2],
 786193            out xMagnitude[1],
 786194            out xMagnitude[0]);
 786195        WideArithmetic.GetMagnitude(
 786196            y,
 786197            out yMagnitude[2],
 786198            out yMagnitude[1],
 786199            out yMagnitude[0]);
 786200        WideArithmetic.GetMagnitude(
 786201            z,
 786202            out zMagnitude[2],
 786203            out zMagnitude[1],
 786204            out zMagnitude[0]);
 786205        Span<ulong> xTerm =
 786206            stackalloc ulong[WideCandidateWords];
 786207        Span<ulong> yTerm =
 786208            stackalloc ulong[WideCandidateWords];
 786209        Span<ulong> zTerm =
 786210            stackalloc ulong[WideCandidateWords];
 786211        Span<ulong> xy =
 786212            stackalloc ulong[WideCandidateWords];
 786213        WideArithmetic.MultiplyMagnitudes(axis.X, xMagnitude, xTerm);
 786214        WideArithmetic.MultiplyMagnitudes(axis.Y, yMagnitude, yTerm);
 786215        WideArithmetic.MultiplyMagnitudes(axis.Z, zMagnitude, zTerm);
 786216        CombineWideSignedMagnitudes(
 786217            xTerm,
 786218            axis.XSign * x.Sign,
 786219            yTerm,
 786220            axis.YSign * y.Sign,
 786221            xy,
 786222            out int xySign);
 786223        CombineWideSignedMagnitudes(
 786224            xy,
 786225            xySign,
 786226            zTerm,
 786227            axis.ZSign * z.Sign,
 786228            result,
 786229            out sign);
 786230    }
 231
 232    private static void BuildWideScaledAlignment(
 233        ReadOnlySpan<ulong> alignment,
 234        Fixed64 length,
 235        Signed192 otherDenominator,
 236        Span<ulong> result)
 237    {
 524238        Span<ulong> lengthMagnitude = stackalloc ulong[3];
 524239        Span<ulong> denominatorMagnitude = stackalloc ulong[3];
 524240        WideArithmetic.GetMagnitude(
 524241            Signed192.Raw(length),
 524242            out lengthMagnitude[2],
 524243            out lengthMagnitude[1],
 524244            out lengthMagnitude[0]);
 524245        WideArithmetic.GetMagnitude(
 524246            otherDenominator,
 524247            out denominatorMagnitude[2],
 524248            out denominatorMagnitude[1],
 524249            out denominatorMagnitude[0]);
 524250        Span<ulong> withLength =
 524251            stackalloc ulong[WideCandidateWords];
 524252        WideArithmetic.MultiplyMagnitudes(
 524253            alignment,
 524254            lengthMagnitude,
 524255            withLength);
 524256        WideArithmetic.MultiplyMagnitudes(
 524257            withLength,
 524258            denominatorMagnitude,
 524259            result);
 524260    }
 261
 262    private static void BuildWideScaledCenter(
 263        ReadOnlySpan<ulong> projection,
 264        Signed192 firstDenominator,
 265        Signed192 secondDenominator,
 266        Span<ulong> result)
 267    {
 262268        Span<ulong> firstMagnitude = stackalloc ulong[3];
 262269        Span<ulong> secondMagnitude = stackalloc ulong[3];
 262270        Span<ulong> twiceMagnitude = stackalloc ulong[3];
 262271        WideArithmetic.GetMagnitude(
 262272            firstDenominator,
 262273            out firstMagnitude[2],
 262274            out firstMagnitude[1],
 262275            out firstMagnitude[0]);
 262276        WideArithmetic.GetMagnitude(
 262277            secondDenominator,
 262278            out secondMagnitude[2],
 262279            out secondMagnitude[1],
 262280            out secondMagnitude[0]);
 262281        WideArithmetic.GetMagnitude(
 262282            Signed192.Raw(Fixed64.Two),
 262283            out twiceMagnitude[2],
 262284            out twiceMagnitude[1],
 262285            out twiceMagnitude[0]);
 262286        Span<ulong> firstProduct =
 262287            stackalloc ulong[WideCandidateWords];
 262288        Span<ulong> secondProduct =
 262289            stackalloc ulong[WideCandidateWords];
 262290        WideArithmetic.MultiplyMagnitudes(
 262291            projection,
 262292            firstMagnitude,
 262293            firstProduct);
 262294        WideArithmetic.MultiplyMagnitudes(
 262295            firstProduct,
 262296            secondMagnitude,
 262297            secondProduct);
 262298        WideArithmetic.MultiplyMagnitudes(
 262299            secondProduct,
 262300            twiceMagnitude,
 262301            result);
 262302    }
 303
 304    private static void BuildWidePlaneSquared(
 305        ReadOnlySpan<ulong> axisSquared,
 306        Signed320 shapeAxisSquared,
 307        ReadOnlySpan<ulong> alignment,
 308        Span<ulong> result)
 309    {
 375310        Span<ulong> shapeAxisMagnitude = stackalloc ulong[5];
 375311        GetMagnitude(shapeAxisSquared, shapeAxisMagnitude);
 375312        Span<ulong> first =
 375313            stackalloc ulong[WideCandidateWords];
 375314        Span<ulong> second =
 375315            stackalloc ulong[WideCandidateWords];
 375316        WideArithmetic.MultiplyMagnitudes(
 375317            axisSquared,
 375318            shapeAxisMagnitude,
 375319            first);
 375320        WideArithmetic.MultiplyMagnitudes(
 375321            alignment,
 375322            alignment,
 375323            second);
 375324        WideArithmetic.SubtractEqualMagnitudes(first, second, result);
 375325    }
 326
 327    private static bool IsWideCylinderCapsuleProjectionNonNegative(
 328        WideCylinderCapsuleDepth depth,
 329        Fixed64 capsuleRadius)
 330    {
 149331        Span<ulong> disk =
 149332            stackalloc ulong[WideCandidateWords];
 149333        Span<ulong> capsule =
 149334            stackalloc ulong[WideCandidateWords];
 149335        Span<ulong> rational =
 149336            stackalloc ulong[WideCandidateWords];
 149337        Span<ulong> zero =
 149338            stackalloc ulong[WideCandidateWords];
 149339        BuildWideDiskRadicand(depth, disk);
 149340        BuildWideCapsuleRadicand(
 149341            depth,
 149342            capsuleRadius,
 149343            capsule);
 149344        BuildWideRationalRadicand(depth, rational);
 149345        zero.Clear();
 149346        return CompareRadicalPairs(
 149347            disk,
 149348            capsule,
 149349            rational,
 149350            zero) >= 0;
 351    }
 352
 353    private static bool IsWideBaseProjectionNonNegative(
 354        WideCylinderCapsuleDepth depth)
 355    {
 2157356        Span<ulong> disk =
 2157357            stackalloc ulong[WideCandidateWords];
 2157358        Span<ulong> rational =
 2157359            stackalloc ulong[WideCandidateWords];
 2157360        BuildWideDiskRadicand(depth, disk);
 2157361        BuildWideRationalRadicand(depth, rational);
 2157362        return WideArithmetic.CompareMagnitudeEqualLength(disk, rational) >= 0;
 363    }
 364
 365    private static int CompareWideProjectionDepth(
 366        WideCylinderCapsuleDepth left,
 367        in ProjectionDepth right)
 368    {
 148369        Span<ulong> radicands = stackalloc ulong[
 148370            WideCandidateWords * 4];
 148371        Span<int> signs = stackalloc int[4];
 148372        radicands.Clear();
 148373        signs.Clear();
 148374        BuildWideRationalRadicand(
 148375            left,
 148376            right.AxisSquared,
 148377            radicands.Slice(
 148378                0,
 148379                WideCandidateWords));
 148380        signs[0] = left.RationalSign;
 148381        BuildWideDiskRadicand(
 148382            left,
 148383            right.AxisSquared,
 148384            radicands.Slice(
 148385                WideCandidateWords,
 148386                WideCandidateWords));
 148387        signs[1] = 1;
 148388        BuildNarrowRationalRadicand(
 148389            right,
 148390            left.AxisSquared,
 148391            radicands.Slice(
 148392                WideCandidateWords * 2,
 148393                WideCandidateWords));
 148394        signs[2] = -right.Rational.Sign;
 148395        BuildNarrowDiskRadicand(
 148396            right,
 148397            left.AxisSquared,
 148398            radicands.Slice(
 148399                WideCandidateWords * 3,
 148400                WideCandidateWords));
 148401        signs[3] = -1;
 148402        return WideArithmetic.GetLinearRadicalSumSign(
 148403            radicands,
 148404            WideCandidateWords,
 148405            signs);
 406    }
 407
 408    private static void BuildWideRationalRadicand(
 409        WideCylinderCapsuleDepth depth,
 410        Span<ulong> result)
 411    {
 7574412        Span<ulong> shapeAxisMagnitude = stackalloc ulong[5];
 7574413        GetMagnitude(
 7574414            depth.ShapeAxisSquared,
 7574415            shapeAxisMagnitude);
 7574416        Span<ulong> square =
 7574417            stackalloc ulong[WideCandidateWords];
 7574418        WideArithmetic.MultiplyMagnitudes(
 7574419            depth.Rational,
 7574420            depth.Rational,
 7574421            square);
 7574422        WideArithmetic.MultiplyMagnitudes(
 7574423            square,
 7574424            shapeAxisMagnitude,
 7574425            result);
 7574426    }
 427
 428    private static void BuildWideRationalRadicand(
 429        WideCylinderCapsuleDepth depth,
 430        Signed576 otherAxisSquared,
 431        Span<ulong> result)
 432    {
 148433        Span<ulong> baseRadicand =
 148434            stackalloc ulong[WideCandidateWords];
 148435        Span<ulong> otherMagnitude = stackalloc ulong[9];
 148436        BuildWideRationalRadicand(
 148437            depth,
 148438            baseRadicand);
 148439        WideArithmetic.GetMagnitude(
 148440            otherAxisSquared,
 148441            otherMagnitude);
 148442        WideArithmetic.MultiplyMagnitudes(
 148443            baseRadicand,
 148444            otherMagnitude,
 148445            result);
 148446    }
 447
 448    private static void BuildWideDiskRadicand(
 449        WideCylinderCapsuleDepth depth,
 450        Span<ulong> result)
 451    {
 7574452        Span<ulong> coefficient = stackalloc ulong[5];
 7574453        GetWideRadialCoefficient(
 7574454            depth.Common,
 7574455            depth.Radius,
 7574456            coefficient);
 7574457        Span<ulong> coefficientSquared =
 7574458            stackalloc ulong[WideCandidateWords];
 7574459        WideArithmetic.MultiplyMagnitudes(
 7574460            coefficient,
 7574461            coefficient,
 7574462            coefficientSquared);
 7574463        WideArithmetic.MultiplyMagnitudes(
 7574464            coefficientSquared,
 7574465            depth.PlaneSquared,
 7574466            result);
 7574467    }
 468
 469    private static void BuildWideDiskRadicand(
 470        WideCylinderCapsuleDepth depth,
 471        Signed576 otherAxisSquared,
 472        Span<ulong> result)
 473    {
 148474        Span<ulong> baseRadicand =
 148475            stackalloc ulong[WideCandidateWords];
 148476        Span<ulong> otherMagnitude = stackalloc ulong[9];
 148477        BuildWideDiskRadicand(
 148478            depth,
 148479            baseRadicand);
 148480        WideArithmetic.GetMagnitude(
 148481            otherAxisSquared,
 148482            otherMagnitude);
 148483        WideArithmetic.MultiplyMagnitudes(
 148484            baseRadicand,
 148485            otherMagnitude,
 148486            result);
 148487    }
 488
 489    private static void BuildWideCapsuleRadicand(
 490        WideCylinderCapsuleDepth depth,
 491        Fixed64 radius,
 492        Span<ulong> result)
 493    {
 149494        Span<ulong> coefficient = stackalloc ulong[5];
 149495        Span<ulong> shapeAxisMagnitude = stackalloc ulong[5];
 149496        GetWideRadialCoefficient(
 149497            depth.Common,
 149498            radius,
 149499            coefficient);
 149500        GetMagnitude(
 149501            depth.ShapeAxisSquared,
 149502            shapeAxisMagnitude);
 149503        Span<ulong> coefficientSquared =
 149504            stackalloc ulong[WideCandidateWords];
 149505        Span<ulong> withShapeAxis =
 149506            stackalloc ulong[WideCandidateWords];
 149507        WideArithmetic.MultiplyMagnitudes(
 149508            coefficient,
 149509            coefficient,
 149510            coefficientSquared);
 149511        WideArithmetic.MultiplyMagnitudes(
 149512            coefficientSquared,
 149513            shapeAxisMagnitude,
 149514            withShapeAxis);
 149515        WideArithmetic.MultiplyMagnitudes(
 149516            withShapeAxis,
 149517            depth.AxisSquared,
 149518            result);
 149519    }
 520
 521    private static void BuildNarrowRationalRadicand(
 522        in ProjectionDepth depth,
 523        ReadOnlySpan<ulong> otherAxisSquared,
 524        Span<ulong> result)
 525    {
 148526        Span<ulong> rationalMagnitude = stackalloc ulong[11];
 148527        Span<ulong> shapeAxisMagnitude = stackalloc ulong[5];
 148528        WideArithmetic.GetMagnitude(
 148529            depth.Rational,
 148530            rationalMagnitude);
 148531        GetMagnitude(
 148532            depth.ShapeAxisSquared,
 148533            shapeAxisMagnitude);
 148534        Span<ulong> square =
 148535            stackalloc ulong[WideCandidateWords];
 148536        Span<ulong> withShapeAxis =
 148537            stackalloc ulong[WideCandidateWords];
 148538        WideArithmetic.MultiplyMagnitudes(
 148539            rationalMagnitude,
 148540            rationalMagnitude,
 148541            square);
 148542        WideArithmetic.MultiplyMagnitudes(
 148543            square,
 148544            shapeAxisMagnitude,
 148545            withShapeAxis);
 148546        WideArithmetic.MultiplyMagnitudes(
 148547            withShapeAxis,
 148548            otherAxisSquared,
 148549            result);
 148550    }
 551
 552    private static void BuildNarrowDiskRadicand(
 553        in ProjectionDepth depth,
 554        ReadOnlySpan<ulong> otherAxisSquared,
 555        Span<ulong> result)
 556    {
 148557        Span<ulong> coefficient = stackalloc ulong[5];
 148558        Span<ulong> planeMagnitude = stackalloc ulong[13];
 148559        GetWideRadialCoefficient(
 148560            depth.Common,
 148561            depth.Radius,
 148562            coefficient);
 148563        WideArithmetic.GetMagnitude(
 148564            depth.PlaneSquared,
 148565            planeMagnitude);
 148566        Span<ulong> coefficientSquared =
 148567            stackalloc ulong[WideCandidateWords];
 148568        Span<ulong> withPlane =
 148569            stackalloc ulong[WideCandidateWords];
 148570        WideArithmetic.MultiplyMagnitudes(
 148571            coefficient,
 148572            coefficient,
 148573            coefficientSquared);
 148574        WideArithmetic.MultiplyMagnitudes(
 148575            coefficientSquared,
 148576            planeMagnitude,
 148577            withPlane);
 148578        WideArithmetic.MultiplyMagnitudes(
 148579            withPlane,
 148580            otherAxisSquared,
 148581            result);
 148582    }
 583
 584    private static void GetWideRadialCoefficient(
 585        Signed192 common,
 586        Fixed64 radius,
 587        Span<ulong> result)
 588    {
 12823589        Signed320 coefficient =
 12823590            WideArithmetic.MultiplySigned192(
 12823591                common,
 12823592                Signed192.Raw(radius));
 12823593        GetMagnitude(coefficient, result);
 12823594    }
 595
 596}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.WideCandidate.Rounding.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.WideCandidate.Rounding.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Provides rounding utilities for wide-precision cylinder/capsule depth
 14/// calculations, using binary search to find the correctly rounded
 15/// (round-to-nearest-even) <see cref="Fixed64"/> result while detecting
 16/// clamping at the representable range boundary.
 17/// </content>
 18internal static partial class WideConvexPrismRelations
 19{
 20    private static void GetRoundedWideCylinderCapsuleDepth(
 21        WideCylinderCapsuleDepth depth,
 22        Fixed64 capsuleRadius,
 23        out Fixed64 rounded,
 24        out bool isClamped)
 25    {
 8026        ulong low = 0UL;
 8027        ulong high = 1UL << 63;
 512028        while (low < high)
 29        {
 504030            ulong midpoint = low + ((high - low) >> 1);
 504031            int comparison =
 504032                CompareWideCylinderCapsuleDepthToTwiceRaw(
 504033                    depth,
 504034                    capsuleRadius,
 504035                    new Signed192(0UL, 0UL, midpoint << 1));
 504036            if (comparison >= 0)
 298837                low = midpoint + 1UL;
 38            else
 205239                high = midpoint;
 40        }
 41
 8042        ulong floor = low - 1UL;
 8043        if (floor == unchecked((ulong)long.MaxValue))
 44        {
 145            rounded = Fixed64.MaxValue;
 146            isClamped =
 147                CompareWideCylinderCapsuleDepthToTwiceRaw(
 148                    depth,
 149                    capsuleRadius,
 150                    new Signed192(
 151                        0UL,
 152                        0UL,
 153                        unchecked((ulong)long.MaxValue << 1))) > 0;
 154            return;
 55        }
 56
 7957        int midpointComparison =
 7958            CompareWideCylinderCapsuleDepthToTwiceRaw(
 7959                depth,
 7960                capsuleRadius,
 7961                new Signed192(
 7962                    0UL,
 7963                    0UL,
 7964                    (floor << 1) | 1UL));
 7965        rounded = Fixed64.FromRaw(
 7966            (long)(floor + GetNearestEvenIncrement(
 7967                midpointComparison,
 7968                floor)));
 7969        isClamped = false;
 7970    }
 71
 72    private static int CompareWideCylinderCapsuleDepthToTwiceRaw(
 73            WideCylinderCapsuleDepth depth,
 74            Fixed64 capsuleRadius,
 75            Signed192 twiceRaw)
 76    {
 512077        Signed192 target = WideArithmetic.SubtractSigned192(
 512078            twiceRaw,
 512079            new Signed192(
 512080                0UL,
 512081                0UL,
 512082                unchecked((ulong)(
 512083                    capsuleRadius.m_rawValue
 512084                    + capsuleRadius.m_rawValue))));
 512085        Span<ulong> disk =
 512086            stackalloc ulong[WideCandidateWords];
 512087        Span<ulong> rational =
 512088            stackalloc ulong[WideCandidateWords];
 512089        Span<ulong> threshold =
 512090            stackalloc ulong[WideCandidateWords];
 512091        Span<ulong> leftFirst =
 512092            stackalloc ulong[WideCandidateWords];
 512093        Span<ulong> leftSecond =
 512094            stackalloc ulong[WideCandidateWords];
 512095        Span<ulong> rightFirst =
 512096            stackalloc ulong[WideCandidateWords];
 512097        Span<ulong> rightSecond =
 512098            stackalloc ulong[WideCandidateWords];
 512099        BuildWideDiskRadicand(depth, disk);
 5120100        BuildWideRationalRadicand(depth, rational);
 5120101        ShiftLeft(disk, 2);
 5120102        ShiftLeft(rational, 2);
 5120103        BuildWideThresholdRadicand(
 5120104            depth,
 5120105            target.Sign < 0
 5120106                ? WideArithmetic.SubtractSigned192(default, target)
 5120107                : target,
 5120108            threshold);
 5120109        leftFirst.Clear();
 5120110        leftSecond.Clear();
 5120111        rightFirst.Clear();
 5120112        rightSecond.Clear();
 5120113        if (target.Sign >= 0)
 114        {
 115            // Wide admission is reached only for a nonzero closest-point
 116            // residual. Its KKT support-minus-center term is strictly negative.
 2963117            CopyPositiveRadicand(
 2963118                rational,
 2963119                rightFirst,
 2963120                rightSecond);
 2963121            CopyPositiveRadicand(
 2963122                disk,
 2963123                leftFirst,
 2963124                leftSecond);
 2963125            CopyPositiveRadicand(
 2963126                threshold,
 2963127                rightFirst,
 2963128                rightSecond);
 129        }
 130        else
 131        {
 2157132            if (IsWideBaseProjectionNonNegative(depth))
 1423133                return 1;
 734134            CopyPositiveRadicand(
 734135                disk,
 734136                leftFirst,
 734137                leftSecond);
 734138            CopyPositiveRadicand(
 734139                threshold,
 734140                leftFirst,
 734141                leftSecond);
 734142            CopyPositiveRadicand(
 734143                rational,
 734144                rightFirst,
 734145                rightSecond);
 146        }
 3697147        return CompareRadicalPairs(
 3697148            leftFirst,
 3697149            leftSecond,
 3697150            rightFirst,
 3697151            rightSecond);
 152    }
 153
 154    private static void BuildWideThresholdRadicand(
 155        WideCylinderCapsuleDepth depth,
 156        Signed192 twiceRaw,
 157        Span<ulong> result)
 158    {
 5120159        Signed320 coefficient =
 5120160            WideArithmetic.MultiplySigned192(
 5120161                depth.Common,
 5120162                twiceRaw);
 5120163        Span<ulong> coefficientMagnitude = stackalloc ulong[5];
 5120164        Span<ulong> shapeAxisMagnitude = stackalloc ulong[5];
 5120165        GetMagnitude(
 5120166            coefficient,
 5120167            coefficientMagnitude);
 5120168        GetMagnitude(
 5120169            depth.ShapeAxisSquared,
 5120170            shapeAxisMagnitude);
 5120171        Span<ulong> square =
 5120172            stackalloc ulong[WideCandidateWords];
 5120173        Span<ulong> withShapeAxis =
 5120174            stackalloc ulong[WideCandidateWords];
 5120175        WideArithmetic.MultiplyMagnitudes(
 5120176            coefficientMagnitude,
 5120177            coefficientMagnitude,
 5120178            square);
 5120179        WideArithmetic.MultiplyMagnitudes(
 5120180            square,
 5120181            shapeAxisMagnitude,
 5120182            withShapeAxis);
 5120183        WideArithmetic.MultiplyMagnitudes(
 5120184            withShapeAxis,
 5120185            depth.AxisSquared,
 5120186            result);
 5120187    }
 188
 189    private static Vector3d GetWideCandidateNormal(
 190        WideCandidateAxis3 axis)
 191    {
 113192        int maximumBitLength = Math.Max(
 113193            WideArithmetic.GetMagnitudeBitLength(axis.X),
 113194            Math.Max(
 113195                WideArithmetic.GetMagnitudeBitLength(axis.Y),
 113196                WideArithmetic.GetMagnitudeBitLength(axis.Z)));
 113197        int shift = Math.Max(0, maximumBitLength - 380);
 198        // The candidate is the first-minus-second closest-point residual.
 199        // Its admitted nonzero center projection against second-minus-first
 200        // is strictly negative by the segment KKT relation.
 113201        Signed576 x = GetShiftedSigned576(
 113202            axis.X,
 113203            -axis.XSign,
 113204            shift);
 113205        Signed576 y = GetShiftedSigned576(
 113206            axis.Y,
 113207            -axis.YSign,
 113208            shift);
 113209        Signed576 z = GetShiftedSigned576(
 113210            axis.Z,
 113211            -axis.ZSign,
 113212            shift);
 113213        return WideNormalization.GetNormalized(x, y, z);
 214    }
 215
 216    private static Signed576 GetShiftedSigned576(
 217        ReadOnlySpan<ulong> magnitude,
 218        int sign,
 219        int shift)
 220    {
 339221        Span<ulong> words = stackalloc ulong[9];
 339222        Span<ulong> sourceWords = stackalloc ulong[7];
 339223        words.Clear();
 339224        sourceWords.Clear();
 339225        int wordShift = shift >> 6;
 339226        int bitShift = shift & 63;
 227        // Closest-candidate components contain 36 words. Shifting their
 228        // maximum to at most 380 bits leaves at least six source words; the
 229        // seventh word is a cleared carry sentinel at the maximum shift.
 339230        int sourceLength = Math.Min(
 339231            sourceWords.Length,
 339232            magnitude.Length - wordShift);
 339233        magnitude.Slice(
 339234            wordShift,
 339235            sourceLength).CopyTo(sourceWords);
 4746236        for (int index = 0; index < 6; index++)
 237        {
 2034238            ulong value = sourceWords[index] >> bitShift;
 2034239            if (bitShift != 0)
 1116240                value |= sourceWords[index + 1] << (64 - bitShift);
 2034241            words[index] = value;
 242        }
 339243        Signed576 positive = new(
 339244            words[8],
 339245            words[7],
 339246            words[6],
 339247            words[5],
 339248            words[4],
 339249            words[3],
 339250            words[2],
 339251            words[1],
 339252            words[0]);
 339253        return sign < 0
 339254            ? WideArithmetic.SubtractSigned576(default, positive)
 339255            : positive;
 256    }
 257
 258    private static void CopyPositiveRadicand(
 259        ReadOnlySpan<ulong> radicand,
 260        Span<ulong> first,
 261        Span<ulong> second)
 262    {
 11091263        if (IsZero(first))
 7394264            radicand.CopyTo(first);
 265        else
 3697266            radicand.CopyTo(second);
 3697267    }
 268
 269    private static void CombineWideSignedMagnitudes(
 270        ReadOnlySpan<ulong> left,
 271        int leftSign,
 272        ReadOnlySpan<ulong> right,
 273        int rightSign,
 274        Span<ulong> result,
 275        out int resultSign)
 276    {
 1834277        if (leftSign == 0 || IsZero(left))
 278        {
 749279            right.CopyTo(result);
 749280            resultSign = IsZero(right) ? 0 : rightSign;
 749281            return;
 282        }
 1085283        if (rightSign == 0 || IsZero(right))
 284        {
 313285            left.CopyTo(result);
 313286            resultSign = leftSign;
 313287            return;
 288        }
 772289        if (leftSign == rightSign)
 290        {
 372291            WideArithmetic.AddEqualMagnitudes(left, right, result);
 372292            resultSign = leftSign;
 372293            return;
 294        }
 295
 400296        int comparison = WideArithmetic.CompareMagnitudeEqualLength(left, right);
 400297        if (comparison == 0)
 298        {
 233299            result.Clear();
 233300            resultSign = 0;
 233301            return;
 302        }
 167303        if (comparison > 0)
 304        {
 30305            WideArithmetic.SubtractEqualMagnitudes(left, right, result);
 30306            resultSign = leftSign;
 307        }
 308        else
 309        {
 137310            WideArithmetic.SubtractEqualMagnitudes(right, left, result);
 137311            resultSign = rightSign;
 312        }
 137313    }
 314}

/home/runner/work/FixedMathSharp/FixedMathSharp/src/FixedMathSharp/Geometry/Wide/Convex/WideConvexPrismRelations.WideCylinderPairs.cs

#LineLine coverage
 1//=======================================================================
 2// WideConvexPrismRelations.WideCylinderPairs.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10namespace FixedMathSharp.Geometry;
 11
 12/// <content>
 13/// Wide-precision helpers for resolving cylinder-cylinder separating axis
 14/// candidates, including axis alignment, center projection, and penetration
 15/// fallback calculations.
 16/// </content>
 17internal static partial class WideConvexPrismRelations
 18{
 19    private static bool TryKeepWideCylinderCylinderAxisFallback(
 20        WideCandidateAxis3 candidate,
 21        Vector3d firstCenter,
 22        RigidAxis3 firstAxis,
 23        Fixed64 firstLength,
 24        Fixed64 firstRadius,
 25        Vector3d secondCenter,
 26        RigidAxis3 secondAxis,
 27        Fixed64 secondLength,
 28        Fixed64 secondRadius,
 29        ref CylinderCylinderPenetration best)
 30    {
 11331        Span<ulong> axisSquared =
 11332            stackalloc ulong[WideCandidateWords];
 11333        Span<ulong> firstAlignment =
 11334            stackalloc ulong[WideCandidateWords];
 11335        Span<ulong> secondAlignment =
 11336            stackalloc ulong[WideCandidateWords];
 11337        Span<ulong> centerProjection =
 11338            stackalloc ulong[WideCandidateWords];
 11339        BuildWideAxisSquared(candidate, axisSquared);
 11340        BuildWideDot(
 11341            candidate,
 11342            firstAxis.X,
 11343            firstAxis.Y,
 11344            firstAxis.Z,
 11345            firstAlignment,
 11346            out _);
 11347        BuildWideDot(
 11348            candidate,
 11349            secondAxis.X,
 11350            secondAxis.Y,
 11351            secondAxis.Z,
 11352            secondAlignment,
 11353            out _);
 11354        BuildWideDot(
 11355            candidate,
 11356            WideArithmetic.SubtractSigned192(
 11357                Signed192.Raw(secondCenter.X),
 11358                Signed192.Raw(firstCenter.X)),
 11359            WideArithmetic.SubtractSigned192(
 11360                Signed192.Raw(secondCenter.Y),
 11361                Signed192.Raw(firstCenter.Y)),
 11362            WideArithmetic.SubtractSigned192(
 11363                Signed192.Raw(secondCenter.Z),
 11364                Signed192.Raw(firstCenter.Z)),
 11365            centerProjection,
 11366            out _);
 67
 11368        Span<ulong> firstAxial =
 11369            stackalloc ulong[WideCandidateWords];
 11370        Span<ulong> secondAxial =
 11371            stackalloc ulong[WideCandidateWords];
 11372        Span<ulong> scaledCenter =
 11373            stackalloc ulong[WideCandidateWords];
 11374        Span<ulong> axialSum =
 11375            stackalloc ulong[WideCandidateWords];
 11376        Span<ulong> rational =
 11377            stackalloc ulong[WideCandidateWords];
 11378        BuildWideScaledAlignment(
 11379            firstAlignment,
 11380            firstLength,
 11381            secondAxis.RotationDenominator,
 11382            firstAxial);
 11383        BuildWideScaledAlignment(
 11384            secondAlignment,
 11385            secondLength,
 11386            firstAxis.RotationDenominator,
 11387            secondAxial);
 11388        BuildWideScaledCenter(
 11389            centerProjection,
 11390            firstAxis.RotationDenominator,
 11391            secondAxis.RotationDenominator,
 11392            scaledCenter);
 11393        WideArithmetic.AddEqualMagnitudes(
 11394            firstAxial,
 11395            secondAxial,
 11396            axialSum);
 11397        CombineWideSignedMagnitudes(
 11398            axialSum,
 11399            1,
 113100            scaledCenter,
 113101            -1,
 113102            rational,
 113103            out int rationalSign);
 104
 113105        Signed320 firstAxisSquared =
 113106            GetAxisSquared(firstAxis);
 113107        Signed320 secondAxisSquared =
 113108            GetAxisSquared(secondAxis);
 113109        Span<ulong> firstPlaneSquared =
 113110            stackalloc ulong[WideCandidateWords];
 113111        Span<ulong> secondPlaneSquared =
 113112            stackalloc ulong[WideCandidateWords];
 113113        BuildWidePlaneSquared(
 113114            axisSquared,
 113115            firstAxisSquared,
 113116            firstAlignment,
 113117            firstPlaneSquared);
 113118        BuildWidePlaneSquared(
 113119            axisSquared,
 113120            secondAxisSquared,
 113121            secondAlignment,
 113122            secondPlaneSquared);
 113123        Signed576 commonWide = WideArithmetic.MultiplySigned576(
 113124            Signed576.ExtendValue(
 113125                WideArithmetic.MultiplySigned192(
 113126                    firstAxis.RotationDenominator,
 113127                    secondAxis.RotationDenominator)),
 113128            Signed192.Raw(Fixed64.Two));
 113129        _ = Signed192.TryNarrowSigned(
 113130            commonWide,
 113131            out Signed192 common);
 113132        var depth = new WideCylinderPairDepth(
 113133            rational,
 113134            rationalSign,
 113135            common,
 113136            firstRadius,
 113137            secondRadius,
 113138            axisSquared,
 113139            firstAxisSquared,
 113140            secondAxisSquared,
 113141            firstPlaneSquared,
 113142            secondPlaneSquared);
 113143        if (!IsWideCylinderPairDepthNonNegative(depth))
 4144            return false;
 109145        if (CompareWideCylinderPairDepth(
 109146                depth,
 109147                best.Depth) >= 0)
 148        {
 76149            return true;
 150        }
 151
 33152        GetRoundedWideCylinderPairDepth(
 33153            depth,
 33154            out Fixed64 roundedDepth,
 33155            out bool depthIsClamped);
 33156        best = new CylinderCylinderPenetration(
 33157            GetWideCandidateNormal(candidate),
 33158            roundedDepth,
 33159            depthIsClamped);
 33160        return true;
 161    }
 162
 163    private static bool IsWideCylinderPairDepthNonNegative(
 164        WideCylinderPairDepth depth)
 165    {
 113166        Span<ulong> radicands = stackalloc ulong[
 113167            WideCandidateWords * 3];
 113168        Span<int> signs = stackalloc int[3];
 113169        radicands.Clear();
 113170        signs.Clear();
 113171        BuildWideCylinderPairLocalRadicands(
 113172            depth,
 113173            radicands,
 113174            signs);
 113175        return WideArithmetic.GetLinearRadicalSumSign(
 113176            radicands,
 113177            WideCandidateWords,
 113178            signs) >= 0;
 179    }
 180
 181    private static int CompareWideCylinderPairDepth(
 182        WideCylinderPairDepth left,
 183        in CylinderPairDepth right)
 184    {
 109185        Span<ulong> radicands = stackalloc ulong[
 109186            WideCandidateWords * 6];
 109187        Span<int> signs = stackalloc int[6];
 109188        radicands.Clear();
 109189        signs.Clear();
 109190        BuildWideCylinderPairCrossRadicands(
 109191            left,
 109192            right,
 109193            radicands,
 109194            signs);
 109195        return WideArithmetic.GetLinearRadicalSumSign(
 109196            radicands,
 109197            WideCandidateWords,
 109198            signs);
 199    }
 200
 201    private static void BuildWideCylinderPairLocalRadicands(
 202        WideCylinderPairDepth depth,
 203        Span<ulong> radicands,
 204        Span<int> signs)
 205    {
 2258206        BuildWideCylinderPairRationalRadicand(
 2258207            depth,
 2258208            radicands.Slice(
 2258209                0,
 2258210                WideCandidateWords));
 2258211        signs[0] = depth.RationalSign;
 2258212        BuildWideCylinderPairDiskRadicand(
 2258213            depth,
 2258214            firstDisk: true,
 2258215            radicands.Slice(
 2258216                WideCandidateWords,
 2258217                WideCandidateWords));
 2258218        signs[1] = 1;
 2258219        BuildWideCylinderPairDiskRadicand(
 2258220            depth,
 2258221            firstDisk: false,
 2258222            radicands.Slice(
 2258223                WideCandidateWords * 2,
 2258224                WideCandidateWords));
 2258225        signs[2] = 1;
 2258226    }
 227
 228    private static void BuildWideCylinderPairCrossRadicands(
 229        WideCylinderPairDepth left,
 230        in CylinderPairDepth right,
 231        Span<ulong> radicands,
 232        Span<int> signs)
 233    {
 109234        BuildWideCylinderPairCrossRationalRadicand(
 109235            left,
 109236            right,
 109237            radicands.Slice(
 109238                0,
 109239                WideCandidateWords));
 109240        signs[0] = left.RationalSign;
 109241        BuildWideCylinderPairCrossDiskRadicand(
 109242            left,
 109243            firstDisk: true,
 109244            right,
 109245            radicands.Slice(
 109246                WideCandidateWords,
 109247                WideCandidateWords));
 109248        signs[1] = 1;
 109249        BuildWideCylinderPairCrossDiskRadicand(
 109250            left,
 109251            firstDisk: false,
 109252            right,
 109253            radicands.Slice(
 109254                WideCandidateWords * 2,
 109255                WideCandidateWords));
 109256        signs[2] = 1;
 257
 109258        BuildNarrowCylinderPairCrossRationalRadicand(
 109259            right,
 109260            left,
 109261            radicands.Slice(
 109262                WideCandidateWords * 3,
 109263                WideCandidateWords));
 109264        signs[3] = -right.Rational.Sign;
 109265        BuildNarrowCylinderPairCrossDiskRadicand(
 109266            right,
 109267            firstDisk: true,
 109268            left,
 109269            radicands.Slice(
 109270                WideCandidateWords * 4,
 109271                WideCandidateWords));
 109272        signs[4] = -1;
 109273        BuildNarrowCylinderPairCrossDiskRadicand(
 109274            right,
 109275            firstDisk: false,
 109276            left,
 109277            radicands.Slice(
 109278                WideCandidateWords * 5,
 109279                WideCandidateWords));
 109280        signs[5] = -1;
 109281    }
 282
 283    private static void BuildWideCylinderPairRationalRadicand(
 284        WideCylinderPairDepth depth,
 285        Span<ulong> result)
 286    {
 2258287        WideArithmetic.MultiplyMagnitudes(
 2258288            depth.Rational,
 2258289            depth.Rational,
 2258290            result);
 2258291        MultiplyWideCylinderPairBy(
 2258292            result,
 2258293            depth.FirstAxisSquared);
 2258294        MultiplyWideCylinderPairBy(
 2258295            result,
 2258296            depth.SecondAxisSquared);
 2258297    }
 298
 299    private static void BuildWideCylinderPairDiskRadicand(
 300        WideCylinderPairDepth depth,
 301        bool firstDisk,
 302        Span<ulong> result)
 303    {
 4734304        Span<ulong> coefficient =
 4734305            stackalloc ulong[5];
 4734306        GetWideRadialCoefficient(
 4734307            depth.Common,
 4734308            firstDisk
 4734309                ? depth.FirstRadius
 4734310                : depth.SecondRadius,
 4734311            coefficient);
 4734312        WideArithmetic.MultiplyMagnitudes(
 4734313            coefficient,
 4734314            coefficient,
 4734315            result);
 4734316        MultiplyWideCylinderPairBy(
 4734317            result,
 4734318            firstDisk
 4734319                ? depth.FirstPlaneSquared
 4734320                : depth.SecondPlaneSquared);
 4734321        MultiplyWideCylinderPairBy(
 4734322            result,
 4734323            firstDisk
 4734324                ? depth.SecondAxisSquared
 4734325                : depth.FirstAxisSquared);
 4734326    }
 327
 328    private static void BuildWideCylinderPairCrossRationalRadicand(
 329        WideCylinderPairDepth depth,
 330        in CylinderPairDepth other,
 331        Span<ulong> result)
 332    {
 109333        WideArithmetic.MultiplyMagnitudes(
 109334            depth.Rational,
 109335            depth.Rational,
 109336            result);
 109337        MultiplyWideCylinderPairBy(
 109338            result,
 109339            other.AxisSquared);
 109340        MultiplyWideCylinderPairBy(
 109341            result,
 109342            depth.FirstAxisSquared);
 109343        MultiplyWideCylinderPairBy(
 109344            result,
 109345            depth.SecondAxisSquared);
 109346        MultiplyWideCylinderPairBy(
 109347            result,
 109348            other.FirstAxisSquared);
 109349        MultiplyWideCylinderPairBy(
 109350            result,
 109351            other.SecondAxisSquared);
 109352    }
 353
 354    private static void BuildWideCylinderPairCrossDiskRadicand(
 355        WideCylinderPairDepth depth,
 356        bool firstDisk,
 357        in CylinderPairDepth other,
 358        Span<ulong> result)
 359    {
 218360        BuildWideCylinderPairDiskRadicand(
 218361            depth,
 218362            firstDisk,
 218363            result);
 218364        MultiplyWideCylinderPairBy(
 218365            result,
 218366            other.AxisSquared);
 218367        MultiplyWideCylinderPairBy(
 218368            result,
 218369            other.FirstAxisSquared);
 218370        MultiplyWideCylinderPairBy(
 218371            result,
 218372            other.SecondAxisSquared);
 218373    }
 374
 375    private static void BuildNarrowCylinderPairCrossRationalRadicand(
 376        in CylinderPairDepth depth,
 377        WideCylinderPairDepth other,
 378        Span<ulong> result)
 379    {
 109380        Span<ulong> rationalWords =
 109381            stackalloc ulong[11];
 109382        WideArithmetic.GetMagnitude(
 109383            depth.Rational,
 109384            rationalWords);
 109385        WideArithmetic.MultiplyMagnitudes(
 109386            rationalWords,
 109387            rationalWords,
 109388            result);
 109389        MultiplyWideCylinderPairBy(
 109390            result,
 109391            other.AxisSquared);
 109392        MultiplyWideCylinderPairBy(
 109393            result,
 109394            depth.FirstAxisSquared);
 109395        MultiplyWideCylinderPairBy(
 109396            result,
 109397            depth.SecondAxisSquared);
 109398        MultiplyWideCylinderPairBy(
 109399            result,
 109400            other.FirstAxisSquared);
 109401        MultiplyWideCylinderPairBy(
 109402            result,
 109403            other.SecondAxisSquared);
 109404    }
 405
 406    private static void BuildNarrowCylinderPairCrossDiskRadicand(
 407        in CylinderPairDepth depth,
 408        bool firstDisk,
 409        WideCylinderPairDepth other,
 410        Span<ulong> result)
 411    {
 218412        Span<ulong> coefficient =
 218413            stackalloc ulong[5];
 218414        GetWideRadialCoefficient(
 218415            depth.Common,
 218416            firstDisk
 218417                ? depth.FirstRadius
 218418                : depth.SecondRadius,
 218419            coefficient);
 218420        WideArithmetic.MultiplyMagnitudes(
 218421            coefficient,
 218422            coefficient,
 218423            result);
 218424        if (firstDisk)
 425        {
 109426            MultiplyWideCylinderPairBy(
 109427                result,
 109428                depth.FirstPlaneSquared);
 109429            MultiplyWideCylinderPairBy(
 109430                result,
 109431                depth.SecondAxisSquared);
 432        }
 433        else
 434        {
 109435            MultiplyWideCylinderPairBy(
 109436                result,
 109437                depth.SecondPlaneSquared);
 109438            MultiplyWideCylinderPairBy(
 109439                result,
 109440                depth.FirstAxisSquared);
 441        }
 218442        MultiplyWideCylinderPairBy(
 218443            result,
 218444            other.AxisSquared);
 218445        MultiplyWideCylinderPairBy(
 218446            result,
 218447            other.FirstAxisSquared);
 218448        MultiplyWideCylinderPairBy(
 218449            result,
 218450            other.SecondAxisSquared);
 218451    }
 452
 453    private static void MultiplyWideCylinderPairBy(
 454        Span<ulong> value,
 455        Signed320 factor)
 456    {
 15502457        Span<ulong> words = stackalloc ulong[5];
 15502458        GetMagnitude(factor, words);
 15502459        MultiplyWideCylinderPairBy(
 15502460            value,
 15502461            words);
 15502462    }
 463
 464    private static void MultiplyWideCylinderPairBy(
 465        Span<ulong> value,
 466        Signed576 factor)
 467    {
 327468        Span<ulong> words = stackalloc ulong[9];
 327469        WideArithmetic.GetMagnitude(
 327470            factor,
 327471            words);
 327472        MultiplyWideCylinderPairBy(
 327473            value,
 327474            words);
 327475    }
 476
 477    private static void MultiplyWideCylinderPairBy(
 478        Span<ulong> value,
 479        Signed832 factor)
 480    {
 218481        Span<ulong> words = stackalloc ulong[13];
 218482        WideArithmetic.GetMagnitude(
 218483            factor,
 218484            words);
 218485        MultiplyWideCylinderPairBy(
 218486            value,
 218487            words);
 218488    }
 489
 490    private static void MultiplyWideCylinderPairBy(
 491        Span<ulong> value,
 492        ReadOnlySpan<ulong> factor)
 493    {
 23253494        Span<ulong> product =
 23253495            stackalloc ulong[WideCandidateWords];
 23253496        WideArithmetic.MultiplyMagnitudes(
 23253497            value,
 23253498            factor,
 23253499            product);
 23253500        product.CopyTo(value);
 23253501    }
 502
 503    private static void GetRoundedWideCylinderPairDepth(
 504        WideCylinderPairDepth depth,
 505        out Fixed64 rounded,
 506        out bool isClamped)
 507    {
 33508        ulong low = 0UL;
 33509        ulong high = 1UL << 63;
 2112510        while (low < high)
 511        {
 2079512            ulong midpoint =
 2079513                low + ((high - low) >> 1);
 2079514            int comparison =
 2079515                CompareWideCylinderPairDepthToTwiceRaw(
 2079516                    depth,
 2079517                    new Signed192(
 2079518                        0UL,
 2079519                        0UL,
 2079520                        midpoint << 1));
 2079521            if (comparison >= 0)
 657522                low = midpoint + 1UL;
 523            else
 1422524                high = midpoint;
 525        }
 526
 33527        ulong floor = low - 1UL;
 33528        int midpointComparison =
 33529            CompareWideCylinderPairDepthToTwiceRaw(
 33530                depth,
 33531                new Signed192(
 33532                    0UL,
 33533                    0UL,
 33534                    (floor << 1) | 1UL));
 33535        ulong roundedRaw = floor + GetNearestEvenIncrement(
 33536            midpointComparison,
 33537            floor);
 33538        rounded = Fixed64.FromRaw(
 33539            (long)Math.Min(
 33540                roundedRaw,
 33541                unchecked((ulong)long.MaxValue)));
 33542        isClamped =
 33543            CompareWideCylinderPairDepthToTwiceRaw(
 33544                depth,
 33545                new Signed192(
 33546                    0UL,
 33547                    0UL,
 33548                    unchecked((ulong)long.MaxValue << 1))) > 0;
 33549    }
 550
 551    private static int CompareWideCylinderPairDepthToTwiceRaw(
 552        WideCylinderPairDepth depth,
 553        Signed192 twiceRaw)
 554    {
 2145555        Span<ulong> radicands = stackalloc ulong[
 2145556            WideCandidateWords * 4];
 2145557        Span<int> signs = stackalloc int[4];
 2145558        radicands.Clear();
 2145559        signs.Clear();
 2145560        BuildWideCylinderPairLocalRadicands(
 2145561            depth,
 2145562            radicands,
 2145563            signs);
 17160564        for (int index = 0; index < 3; index++)
 565        {
 6435566            ShiftLeft(
 6435567                radicands.Slice(
 6435568                    index * WideCandidateWords,
 6435569                    WideCandidateWords),
 6435570                2);
 571        }
 572
 2145573        Signed320 thresholdCoefficient =
 2145574            WideArithmetic.MultiplySigned192(
 2145575                depth.Common,
 2145576                twiceRaw);
 2145577        Span<ulong> coefficient =
 2145578            stackalloc ulong[5];
 2145579        GetMagnitude(
 2145580            thresholdCoefficient,
 2145581            coefficient);
 2145582        Span<ulong> threshold = radicands.Slice(
 2145583            WideCandidateWords * 3,
 2145584            WideCandidateWords);
 2145585        WideArithmetic.MultiplyMagnitudes(
 2145586            coefficient,
 2145587            coefficient,
 2145588            threshold);
 2145589        MultiplyWideCylinderPairBy(
 2145590            threshold,
 2145591            depth.AxisSquared);
 2145592        MultiplyWideCylinderPairBy(
 2145593            threshold,
 2145594            depth.FirstAxisSquared);
 2145595        MultiplyWideCylinderPairBy(
 2145596            threshold,
 2145597            depth.SecondAxisSquared);
 2145598        signs[3] = -1;
 2145599        return WideArithmetic.GetLinearRadicalSumSign(
 2145600            radicands,
 2145601            WideCandidateWords,
 2145602            signs);
 603    }
 604}
Generated by: ReportGenerator 5.5.11.0
08/04/2026 - 20:34:44
GitHub | reportgenerator.io

Methods/Properties

TryGetCenteredCapsuleContact(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&)
TryGetCenteredCylinderContact(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&)
TryGetCenteredConeContact(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&)
TryGetAnchoredContact(FixedMathSharp.Geometry.WideConvexPrismRelations/FiniteShapeKind,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&)
TryGetContact(FixedMathSharp.Geometry.WideConvexPrismRelations/FiniteShapeKind,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/Penetration&)
TryKeepAxis(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Geometry.WideConvexPrismRelations/FiniteShapeKind,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.FixedQuaternion,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/PenetrationCandidate&)
GetPrismProjection(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,FixedMathSharp.Signed576&,FixedMathSharp.Signed576&,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&,System.Boolean&,System.Boolean&)
GetEndpointToPrismVertexAxis(FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,System.Int32,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,System.Int32)
GetScaledEndpointDifference(FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Fixed64,System.Int32)
GetPlanarEdge(FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
GetPlanarVertexAxis(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d)
Cross(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3)
SubtractProducts(FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed320)
GetDifferenceProjection(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3)
GetRotatedPlanarProjection(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3)
ScaleProjection(FixedMathSharp.Signed576)
GetShapeProjectionDepths(FixedMathSharp.Geometry.WideConvexPrismRelations/FiniteShapeKind,FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Signed576,FixedMathSharp.Signed576,FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&)
IsProjectionNonNegative(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&)
CompareProjectionDepths(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&)
CompareSignedProjectionDepths(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&)
GetRadialProjection(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Signed576,FixedMathSharp.Signed320&,FixedMathSharp.Signed832&,FixedMathSharp.Signed576&)
GetRoundedDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,System.Boolean&)
GetDepthApproximation(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&)
GetDiskDepthApproximation(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Signed576)
GetCeilingSquareRoot(FixedMathSharp.Signed704,FixedMathSharp.Signed576)
GetNearestEvenIncrement(System.Int32,System.UInt64)
CompareProjectionDepthToTwiceRaw(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Signed192)
IsDiskRadicalAtLeast(FixedMathSharp.Signed192,FixedMathSharp.Fixed64,FixedMathSharp.Signed832,FixedMathSharp.Signed320,FixedMathSharp.Signed704)
GetAxisProjection(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3)
GetAxisSquared(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3)
GetAxisSquared(FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3)
GetPlaneSquared(FixedMathSharp.Signed576,FixedMathSharp.Signed320,FixedMathSharp.Signed576)
GetMagnitude(FixedMathSharp.Signed704)
BuildLocalCapsuleRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,System.Span`1<System.UInt64>)
BuildThresholdCapsuleRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,System.Span`1<System.UInt64>)
BuildLocalDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,System.Span`1<System.UInt64>)
BuildRationalSquare(FixedMathSharp.Signed704,System.Span`1<System.UInt64>)
BuildThresholdRadicand(FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed576,System.Span`1<System.UInt64>)
CompareRadicalPairs(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
ComparePositiveRadicalDifference(System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
BuildProduct(FixedMathSharp.Signed704,FixedMathSharp.Signed704,System.Span`1<System.UInt64>)
BuildProduct(FixedMathSharp.Signed704,FixedMathSharp.Signed704,FixedMathSharp.Signed320,System.Span`1<System.UInt64>)
BuildProduct(FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed576,System.Span`1<System.UInt64>)
BuildProduct(FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed832,System.Span`1<System.UInt64>)
BuildProduct(FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed576,System.Span`1<System.UInt64>)
MultiplyBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed320)
MultiplyBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed576)
MultiplyBy(System.Span`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
GetMagnitude(FixedMathSharp.Signed320,System.Span`1<System.UInt64>)
ShiftLeft(System.Span`1<System.UInt64>,System.Int32)
IsZero(System.ReadOnlySpan`1<System.UInt64>)
TryGetCenteredFiniteCylindersContact(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&)
TryGetCenteredFiniteCylindersContact(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&,System.Boolean&,System.Boolean&)
TryKeepWideCylinderCylinderAxis(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCandidateAxis3,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderCylinderPenetration&)
TryKeepCylinderCylinderAxis(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderCylinderPenetration&)
IsCylinderPairDepthNonNegative(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&)
CompareCylinderPairDepths(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&)
BuildCylinderPairLocalRadicands(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Span`1<System.UInt64>,System.Span`1<System.Int32>)
BuildCylinderPairCrossRadicands(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Span`1<System.UInt64>,System.Span`1<System.Int32>)
BuildCylinderPairRationalRadicand(FixedMathSharp.Signed704,FixedMathSharp.Signed320,FixedMathSharp.Signed320,System.Span`1<System.UInt64>)
BuildCylinderPairDiskRadicand(FixedMathSharp.Signed320,FixedMathSharp.Signed832,FixedMathSharp.Signed320,System.Span`1<System.UInt64>)
BuildCylinderPairCrossRationalRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Span`1<System.UInt64>)
BuildCylinderPairCrossDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Boolean,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Span`1<System.UInt64>)
MultiplyCylinderPairBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed320)
MultiplyCylinderPairBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed576)
MultiplyCylinderPairBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed832)
MultiplyCylinderPairBy(System.Span`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
GetRoundedCylinderCylinderDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,FixedMathSharp.Fixed64&,System.Boolean&)
TryGetCylinderPairDepthApproximation(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,FixedMathSharp.Fixed64&)
GetRoundedCylinderPairDepthBySearch(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&)
CompareCylinderPairDepthToTwiceRaw(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,FixedMathSharp.Signed192)
TryGetCenteredFiniteCylinderCapsuleContact(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&)
TryGetCenteredFiniteCylinderCapsuleContact(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&,System.Boolean&)
TryKeepCylinderCapsuleAxis(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderCapsulePenetration&)
GetCenterDifferenceAxis(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
IsCylinderCapsuleProjectionNonNegative(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Fixed64)
GetRoundedCylinderCapsuleDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,System.Boolean&)
TryGetCylinderCapsuleDepthApproximation(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
GetRoundedCylinderCapsuleDepthBySearch(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Fixed64)
CompareCylinderCapsuleDepthToTwiceRaw(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,FixedMathSharp.Fixed64,FixedMathSharp.Signed192)
GetMagnitude576(FixedMathSharp.Signed576)
TryKeepWideCylinderCapsuleAxis(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCandidateAxis3,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderCapsulePenetration&)
.cctor()
.ctor(FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed320)
get_IsZero()
.ctor(FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192)
ToWide()
.ctor(FixedMathSharp.Vector3d,FixedMathSharp.Geometry.FixedPointAnchor,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.Boolean)
.ctor(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,System.Boolean,FixedMathSharp.Geometry.WideConvexPrismRelations/ShapeSupportFeature,FixedMathSharp.Vector3d,System.Boolean,FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth)
.ctor(FixedMathSharp.Signed704,FixedMathSharp.Signed192,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/RadialKind,FixedMathSharp.Signed576,FixedMathSharp.Signed320,FixedMathSharp.Signed832,FixedMathSharp.Geometry.WideConvexPrismRelations/ShapeSupportFeature)
.ctor(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,System.Boolean,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth)
.ctor(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.Boolean)
.ctor(FixedMathSharp.Signed704,FixedMathSharp.Signed192,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Signed576,FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed832,FixedMathSharp.Signed832)
TryGetFastDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&)
CreateFirstDiskDepth()
CreateSecondDiskDepth()
IsFullDiskProjection(FixedMathSharp.Signed576,FixedMathSharp.Signed320,FixedMathSharp.Signed832)
.ctor(System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32)
TryNarrow(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3&)
TryNarrowComponent(System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Signed320&)
.ctor(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,System.Boolean,FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth)
.ctor(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.Boolean)
.ctor(System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Signed192,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Signed320,System.ReadOnlySpan`1<System.UInt64>)
.ctor(System.ReadOnlySpan`1<System.UInt64>,System.Int32,FixedMathSharp.Signed192,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Signed320,FixedMathSharp.Signed320,System.ReadOnlySpan`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
MaterializeContact(FixedMathSharp.Geometry.WideConvexPrismRelations/PenetrationCandidate&,FixedMathSharp.Geometry.WideConvexPrismRelations/FiniteShapeKind,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64)
GetMatchedPrismOffset(FixedMathSharp.Geometry.FixedPointAnchor,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,System.ReadOnlySpan`1<FixedMathSharp.Vector2d>,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3,System.Boolean,FixedMathSharp.Vector3d&)
GetShapeSupportAnchor(FixedMathSharp.Geometry.WideConvexPrismRelations/FiniteShapeKind,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Int32,FixedMathSharp.Geometry.WideConvexPrismRelations/ShapeSupportFeature)
FromDirection(FixedMathSharp.Vector3d)
Compare(FixedMathSharp.Signed576,FixedMathSharp.Signed576)
Negate(FixedMathSharp.Geometry.WideConvexPrismRelations/Axis3)
TryKeepWideCylinderCapsuleAxisFallback(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCandidateAxis3,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderCapsulePenetration&)
BuildWideAxisSquared(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCandidateAxis3,System.Span`1<System.UInt64>)
BuildWideDot(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCandidateAxis3,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,System.Span`1<System.UInt64>,System.Int32&)
BuildWideScaledAlignment(System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Fixed64,FixedMathSharp.Signed192,System.Span`1<System.UInt64>)
BuildWideScaledCenter(System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Signed192,FixedMathSharp.Signed192,System.Span`1<System.UInt64>)
BuildWidePlaneSquared(System.ReadOnlySpan`1<System.UInt64>,FixedMathSharp.Signed320,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
IsWideCylinderCapsuleProjectionNonNegative(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Fixed64)
IsWideBaseProjectionNonNegative(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth)
CompareWideProjectionDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&)
BuildWideRationalRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,System.Span`1<System.UInt64>)
BuildWideRationalRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Signed576,System.Span`1<System.UInt64>)
BuildWideDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,System.Span`1<System.UInt64>)
BuildWideDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Signed576,System.Span`1<System.UInt64>)
BuildWideCapsuleRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>)
BuildNarrowRationalRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
BuildNarrowDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/ProjectionDepth&,System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>)
GetWideRadialCoefficient(FixedMathSharp.Signed192,FixedMathSharp.Fixed64,System.Span`1<System.UInt64>)
GetRoundedWideCylinderCapsuleDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,System.Boolean&)
CompareWideCylinderCapsuleDepthToTwiceRaw(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Fixed64,FixedMathSharp.Signed192)
BuildWideThresholdRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderCapsuleDepth,FixedMathSharp.Signed192,System.Span`1<System.UInt64>)
GetWideCandidateNormal(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCandidateAxis3)
GetShiftedSigned576(System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.Int32)
CopyPositiveRadicand(System.ReadOnlySpan`1<System.UInt64>,System.Span`1<System.UInt64>,System.Span`1<System.UInt64>)
CombineWideSignedMagnitudes(System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.ReadOnlySpan`1<System.UInt64>,System.Int32,System.Span`1<System.UInt64>,System.Int32&)
TryKeepWideCylinderCylinderAxisFallback(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCandidateAxis3,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideConvexPrismRelations/RigidAxis3,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderCylinderPenetration&)
IsWideCylinderPairDepthNonNegative(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth)
CompareWideCylinderPairDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&)
BuildWideCylinderPairLocalRadicands(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,System.Span`1<System.UInt64>,System.Span`1<System.Int32>)
BuildWideCylinderPairCrossRadicands(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Span`1<System.UInt64>,System.Span`1<System.Int32>)
BuildWideCylinderPairRationalRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,System.Span`1<System.UInt64>)
BuildWideCylinderPairDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,System.Boolean,System.Span`1<System.UInt64>)
BuildWideCylinderPairCrossRationalRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Span`1<System.UInt64>)
BuildWideCylinderPairCrossDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,System.Boolean,FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Span`1<System.UInt64>)
BuildNarrowCylinderPairCrossRationalRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,System.Span`1<System.UInt64>)
BuildNarrowCylinderPairCrossDiskRadicand(FixedMathSharp.Geometry.WideConvexPrismRelations/CylinderPairDepth&,System.Boolean,FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,System.Span`1<System.UInt64>)
MultiplyWideCylinderPairBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed320)
MultiplyWideCylinderPairBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed576)
MultiplyWideCylinderPairBy(System.Span`1<System.UInt64>,FixedMathSharp.Signed832)
MultiplyWideCylinderPairBy(System.Span`1<System.UInt64>,System.ReadOnlySpan`1<System.UInt64>)
GetRoundedWideCylinderPairDepth(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,FixedMathSharp.Fixed64&,System.Boolean&)
CompareWideCylinderPairDepthToTwiceRaw(FixedMathSharp.Geometry.WideConvexPrismRelations/WideCylinderPairDepth,FixedMathSharp.Signed192)